thug/Code/Core/Math/geometry.cpp

/*****************************************************************************
**																			**
**			              Neversoft Entertainment							**
**																		   	**
**				   Copyright (C) 1999 - All Rights Reserved				   	**
**																			**
******************************************************************************
**																			**
**	Project:		Core Library											**
**																			**
**	Module:			Geometry (MTH)	 										**
**																			**
**	File name:		Geometry.cpp											**
**																			**
**	Created by:		11/14/00	-	Mick									**
**																			**
**	Description:	Math 									**
// This module handles the representation and manipulation of line segements
// and planes
// 
// a line segment (Mth::Line) is defined by two points, m_start and m_end
//
// a plane is defined by a point and a normal

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


/*****************************************************************************
**							  	  Includes									**
*****************************************************************************/

#include <core/defines.h>
#include <core/math.h>
#include <core/math/geometry.h>

#include <gfx/debuggfx.h>

/*****************************************************************************
**							  DBG Information								**
*****************************************************************************/

namespace Mth
{



/*****************************************************************************
**							  DBG Information								**
*****************************************************************************/

//

/*****************************************************************************
**								  Externals									**
*****************************************************************************/

/*****************************************************************************
**								   Defines									**
*****************************************************************************/


/*****************************************************************************
**								Private Types								**
*****************************************************************************/


/*****************************************************************************
**								 Private Data								**
*****************************************************************************/


/*****************************************************************************
**								 Public Data								**
*****************************************************************************/


/*****************************************************************************
**							  Private Prototypes							**
*****************************************************************************/


/*****************************************************************************
**							   Private Functions							**
*****************************************************************************/


/*****************************************************************************
**							   Public Functions								**
*****************************************************************************/

Line::Line()
{
}

Line::Line ( const Vector &start, const Vector &end )			
{
		m_start = start;
		m_end = end;
}

Plane::Plane()
{
} 
 
Plane::Plane (const Vector &point, const Vector &normal)
{
		m_point = point;
		m_normal = normal;
}		

Rectangle::Rectangle()
{
}

Rectangle::Rectangle ( const Vector& corner, const Vector& first_edge, const Vector& second_edge )
{
	m_corner = corner;
	m_first_edge = first_edge;
	m_second_edge = second_edge;
}

/*
   Calculate the line segment PaPb that is the shortest route between
   two lines P1P2 and P3P4. Calculate also the values of mua and mub where
      Pa = P1 + mua (P2 - P1)
      Pb = P3 + mub (P4 - P3)
   Return FALSE if no solution exists.
   original algorithm from  http://www.swin.edu.au/astronomy/pbourke/geometry/lineline3d/
   note that I (Mick) modified it to clamp the points to within the line segments
   if you remove the "Clamp" calls below, then the lines will be assumed to be
   of infinite length 
*/

#define EPS  0.00001f

bool LineLineIntersect( Line & l1, Line & l2, Vector *pa, Vector *pb, float *mua, float *mub, bool clamp )
{

	Vector &p1 = l1.m_start;
	Vector &p2 = l1.m_end;
	Vector &p3 = l2.m_start;
	Vector &p4 = l2.m_end;
   
	Vector p13,p43,p21;
	  
	float d1343,d4321,d1321,d4343,d2121;
	float numer,denom;
	
	p13 = p1 - p3;
	p43 = p4 - p3;
	
	if (Abs(p43[X])  < EPS && Abs(p43[Y])  < EPS && Abs(p43[Z])  < EPS)
	  return(false);
	
	p21 = p2 - p1;  
		  
	if (Abs(p21[X])  < EPS && Abs(p21[Y])  < EPS && Abs(p21[Z])  < EPS)
	  return(false);

	d1343 = DotProduct(p13,p43);
	d4321 = DotProduct(p43,p21);
	d1321 = DotProduct(p13,p21);
	d4343 = DotProduct(p43,p43);
	d2121 = DotProduct(p21,p21);
	
	denom = d2121 * d4343 - d4321 * d4321;
	if (Abs(denom) < EPS)
	  return(false);
	numer = d1343 * d4321 - d1321 * d4343;
	
	*mua = numer / denom;
	if( clamp )
	{
		*mua = Clamp(*mua,0.0f,1.0f);
	}
	*mub = (d1343 + d4321 * (*mua)) / d4343;

	if( clamp )
	{
		*mub = Clamp(*mub,0.0f,1.0f);
	}
	
	*pa = p1 + (*mua * p21);     
	*pb = p3 + (*mub * p43);     
	
	return(true);
}

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

CBBox::CBBox()
{
	Reset();
}

CBBox::CBBox(const Vector &point)   
{
	Set(point, point);
}

CBBox::CBBox(const Vector &min, const Vector &max)			
{
	Set(min, max);
}

void CBBox::AddPoint(const Vector &point)   
{
	// Adjust min/max points
	if (point[X] < m_min[X])
		m_min[X] = point[X];
	if (point[Y] < m_min[Y])
		m_min[Y] = point[Y];
	if (point[Z] < m_min[Z])
		m_min[Z] = point[Z];

	if (point[X] > m_max[X])
		m_max[X] = point[X];
	if (point[Y] > m_max[Y])
		m_max[Y] = point[Y];
	if (point[Z] > m_max[Z])
		m_max[Z] = point[Z];
}

// Returns number of axes within and flags for which ones
int CBBox::WithinAxes(const Vector &point, uint32 &axis_flags) const
{
	int number_of_axes = 0;
	axis_flags = 0;

	if ((point[X] >= m_min[X]) && (point[X] <= m_max[X]))
	{
		number_of_axes++;
		axis_flags |= 1 << X;
	}

	if ((point[Y] >= m_min[Y]) && (point[Y] <= m_max[Y]))
	{
		number_of_axes++;
		axis_flags |= 1 << Y;
	}

	if ((point[Z] >= m_min[Z]) && (point[Z] <= m_max[Z]))
	{
		number_of_axes++;
		axis_flags |= 1 << Z;
	}

	return number_of_axes;
}

void CBBox::DebugRender(uint32 rgba, int frames) const
{
	// Min YZ square
	Gfx::AddDebugLine(Mth::Vector(m_min[X],m_min[Y],m_min[Z]),Mth::Vector(m_min[X],m_min[Y],m_max[Z]),rgba,rgba,frames);
	Gfx::AddDebugLine(Mth::Vector(m_min[X],m_min[Y],m_max[Z]),Mth::Vector(m_min[X],m_max[Y],m_max[Z]),rgba,rgba,frames);
	Gfx::AddDebugLine(Mth::Vector(m_min[X],m_max[Y],m_max[Z]),Mth::Vector(m_min[X],m_max[Y],m_min[Z]),rgba,rgba,frames);
	Gfx::AddDebugLine(Mth::Vector(m_min[X],m_max[Y],m_min[Z]),Mth::Vector(m_min[X],m_min[Y],m_min[Z]),rgba,rgba,frames);

	// Max YZ square
	Gfx::AddDebugLine(Mth::Vector(m_max[X],m_min[Y],m_min[Z]),Mth::Vector(m_max[X],m_min[Y],m_max[Z]),rgba,rgba,frames);
	Gfx::AddDebugLine(Mth::Vector(m_max[X],m_min[Y],m_max[Z]),Mth::Vector(m_max[X],m_max[Y],m_max[Z]),rgba,rgba,frames);
	Gfx::AddDebugLine(Mth::Vector(m_max[X],m_max[Y],m_max[Z]),Mth::Vector(m_max[X],m_max[Y],m_min[Z]),rgba,rgba,frames);
	Gfx::AddDebugLine(Mth::Vector(m_max[X],m_max[Y],m_min[Z]),Mth::Vector(m_max[X],m_min[Y],m_min[Z]),rgba,rgba,frames);

	// lines joining corners
	Gfx::AddDebugLine(Mth::Vector(m_min[X],m_min[Y],m_min[Z]),Mth::Vector(m_max[X],m_min[Y],m_min[Z]),rgba,rgba,frames);
	Gfx::AddDebugLine(Mth::Vector(m_min[X],m_min[Y],m_max[Z]),Mth::Vector(m_max[X],m_min[Y],m_max[Z]),rgba,rgba,frames);
	Gfx::AddDebugLine(Mth::Vector(m_min[X],m_max[Y],m_max[Z]),Mth::Vector(m_max[X],m_max[Y],m_max[Z]),rgba,rgba,frames);
	Gfx::AddDebugLine(Mth::Vector(m_min[X],m_max[Y],m_min[Z]),Mth::Vector(m_max[X],m_max[Y],m_min[Z]),rgba,rgba,frames);
}

void CBBox::DebugRender(const Mth::Matrix & transform, uint32 rgba, int frames) const
{
	uint32 rgba_min = 0xFF000080;
	uint32 rgba_max = 0x0000FF80;

	Mth::Vector min_yz_1(m_min[X], m_min[Y], m_min[Z]);
	Mth::Vector min_yz_2(m_min[X], m_min[Y], m_max[Z]);
	Mth::Vector min_yz_3(m_min[X], m_max[Y], m_max[Z]);
	Mth::Vector min_yz_4(m_min[X], m_max[Y], m_min[Z]);

	Mth::Vector max_yz_1(m_max[X], m_min[Y], m_min[Z]);
	Mth::Vector max_yz_2(m_max[X], m_min[Y], m_max[Z]);
	Mth::Vector max_yz_3(m_max[X], m_max[Y], m_max[Z]);
	Mth::Vector max_yz_4(m_max[X], m_max[Y], m_min[Z]);

	// Transform
	min_yz_1 = transform.Transform(min_yz_1);
	min_yz_2 = transform.Transform(min_yz_2);
	min_yz_3 = transform.Transform(min_yz_3);
	min_yz_4 = transform.Transform(min_yz_4);

	max_yz_1 = transform.Transform(max_yz_1);
	max_yz_2 = transform.Transform(max_yz_2);
	max_yz_3 = transform.Transform(max_yz_3);
	max_yz_4 = transform.Transform(max_yz_4);

	// Min YZ square
	Gfx::AddDebugLine(min_yz_1, min_yz_2,rgba_min,rgba_min,frames);
	Gfx::AddDebugLine(min_yz_2, min_yz_3,rgba_min,rgba_min,frames);
	Gfx::AddDebugLine(min_yz_3, min_yz_4,rgba_min,rgba_min,frames);
	Gfx::AddDebugLine(min_yz_4, min_yz_1,rgba_min,rgba_min,frames);

	// Max YZ square
	Gfx::AddDebugLine(max_yz_1, max_yz_2,rgba_max,rgba_max,frames);
	Gfx::AddDebugLine(max_yz_2, max_yz_3,rgba_max,rgba_max,frames);
	Gfx::AddDebugLine(max_yz_3, max_yz_4,rgba_max,rgba_max,frames);
	Gfx::AddDebugLine(max_yz_4, max_yz_1,rgba_max,rgba_max,frames);

	// lines joining corners
	Gfx::AddDebugLine(min_yz_1, max_yz_1,rgba,rgba,frames);
	Gfx::AddDebugLine(min_yz_2, max_yz_2,rgba,rgba,frames);
	Gfx::AddDebugLine(min_yz_3, max_yz_3,rgba,rgba,frames);
	Gfx::AddDebugLine(min_yz_4, max_yz_4,rgba,rgba,frames);
}

bool CBBox::LineIntersect( const Mth::Line &line, Mth::Vector &point, Mth::Vector &normal ) const
{
	bool start_point_within = Within(line.m_start);
	bool end_point_within = Within(line.m_end);

	// Doesn't intersect side if both points within
	if (start_point_within && end_point_within)
	{
		return false;
	}

	// Trivial rejection.
	if ((line.m_start[Y] > m_max[Y]) && (line.m_end[Y] > m_max[Y])) return false;
	if ((line.m_start[Y] < m_min[Y]) && (line.m_end[Y] < m_min[Y])) return false;
	if ((line.m_start[X] > m_max[X]) && (line.m_end[X] > m_max[X])) return false;
	if ((line.m_start[X] < m_min[X]) && (line.m_end[X] < m_min[X])) return false;
	if ((line.m_start[Z] > m_max[Z]) && (line.m_end[Z] > m_max[Z])) return false;
	if ((line.m_start[Z] < m_min[Z]) && (line.m_end[Z] < m_min[Z])) return false;

	float dx = line.m_end[X] - line.m_start[X];
	float dy = line.m_end[Y] - line.m_start[Y];
	float dz = line.m_end[Z] - line.m_start[Z];
	
	// avoid divide by zeros.
	if ( !dx )
	{
		dx = ( 0.000001f );
	}
	if ( !dy )
	{
		dy = ( 0.000001f );
	}
	if ( !dz )
	{
		dz = ( 0.000001f );
	}

	//
	// Garrett: I back-face cull 3 of the sides to reduce the calculations and keep
	// the collisions down to one side.

	// Check the max-x face.
	if (line.m_start[X] > m_max[X] && line.m_end[X] < m_max[X] && (dx < 0.0f))
	{
		// It crosses the plane of the face, so calculate the y & z coords
		// of the intersection and see if they are in the face,
		float d=m_max[X] - line.m_start[X];
		float y=d*dy/dx + line.m_start[Y];
		float z=d*dz/dx + line.m_start[Z];
		if (y < m_max[Y] && y > m_min[Y] && z < m_max[Z] && z > m_min[Z])
		{
			// It does collide!
			point = Mth::Vector(m_max[X], y, z, 1.0f);
			normal = Mth::Vector(1.0f, 0.0f, 0.0f, 0.0f);
			return true;
		}
	}

	// Check the min-x face.
	if (line.m_start[X] < m_min[X] && line.m_end[X] > m_min[X] && (dx > 0.0f))
	{
		// It crosses the plane of the face, so calculate the y & z coords
		// of the intersection and see if they are in the face,
		float d=m_min[X] - line.m_start[X];
		float y=d*dy/dx + line.m_start[Y];
		float z=d*dz/dx + line.m_start[Z];
		if (y < m_max[Y] && y > m_min[Y] && z < m_max[Z] && z > m_min[Z])
		{
			// It does collide!
			point = Mth::Vector(m_min[X], y, z, 1.0f);
			normal = Mth::Vector(-1.0f, 0.0f, 0.0f, 0.0f);
			return true;
		}
	}

	// Check the max-y face.
	if (line.m_start[Y] > m_max[Y] && line.m_end[Y] < m_max[Y] && (dy < 0.0f))
	{
		// It crosses the plane of the face, so calculate the x & z coords
		// of the intersection and see if they are in the face,
		float d=m_max[Y] - line.m_start[Y];
		float x=d*dx/dy + line.m_start[X];
		float z=d*dz/dy + line.m_start[Z];
		if (x < m_max[X] && x > m_min[X] && z < m_max[Z] && z > m_min[Z])
		{
			// It does collide!
			point = Mth::Vector(x, m_max[Y], z, 1.0f);
			normal = Mth::Vector(0.0f, 1.0f, 0.0f, 0.0f);
			return true;
		}
	}

	// Check the min-y face.
	if (line.m_start[Y] < m_min[Y] && line.m_end[Y] > m_min[Y] && (dy > 0.0f))
	{
		// It crosses the plane of the face, so calculate the x & z coords
		// of the intersection and see if they are in the face,
		float d=m_min[Y] - line.m_start[Y];
		float x=d*dx/dy + line.m_start[X];
		float z=d*dz/dy + line.m_start[Z];
		if (x < m_max[X] && x > m_min[X] && z < m_max[Z] && z > m_min[Z])
		{
			// It does collide!
			point = Mth::Vector(x, m_min[Y], z, 1.0f);
			normal = Mth::Vector(0.0f, -1.0f, 0.0f, 0.0f);
			return true;
		}
	}

	// Check the max-z face.
	if (line.m_start[Z] > m_max[Z] && line.m_end[Z] < m_max[Z] && (dz < 0.0f))
	{
		// It crosses the plane of the face, so calculate the x & y coords
		// of the intersection and see if they are in the face,
		float d=m_max[Z] - line.m_start[Z];
		float x=d*dx/dz + line.m_start[X];
		float y=d*dy/dz + line.m_start[Y];
		if (x < m_max[X] && x > m_min[X] && y < m_max[Y] && y > m_min[Y])
		{
			// It does collide!
			point = Mth::Vector(x, y, m_max[Z], 1.0f);
			normal = Mth::Vector(0.0f, 0.0f, 1.0f, 0.0f);
			return true;
		}
	}


	// Check the min-z face.
	if (line.m_start[Z] < m_min[Z] && line.m_end[Z] > m_min[Z] && (dz > 0.0f))
	{
		// It crosses the plane of the face, so calculate the x & y coords
		// of the intersection and see if they are in the face,
		float d=m_min[Z] - line.m_start[Z];
		float x=d*dx/dz + line.m_start[X];
		float y=d*dy/dz + line.m_start[Y];
		if (x < m_max[X] && x > m_min[X] && y < m_max[Y] && y > m_min[Y])
		{
			// It does collide!
			point = Mth::Vector(x, y, m_min[Z], 1.0f);
			normal = Mth::Vector(0.0f, 0.0f, -1.0f, 0.0f);
			return true;
		}
	}

	return false;
}

void CBBox::GetClosestIntersectPoint(const Mth::Vector &pos, Mth::Vector &point) const
{
	uint32 axes_within_flags;
	int axes_within = WithinAxes(pos, axes_within_flags);

	if (axes_within == 3)
	{
		float closest_dist = -1.0f;
		int closest_axis = -1;
		float closest_axis_coord = 0.0f;		// Value of the minimum axis coordinate

		// Find the closest rectangle and move the point to that rectangle to find the intersection
		// point.
		for (int axis = X; axis <= Z; axis++)
		{
			float min_to_pos_dist = pos[axis] - m_min[axis];
			float max_to_pos_dist = m_max[axis] - pos[axis];

			Dbg_Assert(min_to_pos_dist >= 0.0f);
			Dbg_Assert(max_to_pos_dist >= 0.0f);

			float min_dist;
			float min_coord;

			// Figure out the closest distance
			if (min_to_pos_dist < max_to_pos_dist)
			{
				min_dist = min_to_pos_dist;
				min_coord = m_min[axis];
			}
			else
			{
				min_dist = max_to_pos_dist;
				min_coord = m_max[axis];
			}
			
			// See if this is the first axis or if this one is closer
			if ((axis == X) || (min_dist < closest_dist))
			{
				closest_axis = axis;
				closest_dist = min_dist;
				closest_axis_coord = min_coord;
			}
		}

		Dbg_Assert((closest_axis >= X) && (closest_axis <= Z));
		Dbg_Assert(closest_dist >= 0.0f);

		// Calc the intersection point
		point = pos;				// Start with position
		point[closest_axis] = closest_axis_coord;

#if 0
		static int print_now;
		if ((print_now++ % 60) == 0)
		{
			Dbg_Message("Closest axis %d, (min - max) pos (%f - %f) %f, closest dist %f, closest coord %f", closest_axis,
						m_min[closest_axis], m_max[closest_axis], pos[closest_axis], closest_dist, closest_axis_coord);
		}
#endif
	}
	else
	{
		point[W] = 1.0f;			// Homogeneous

		// Go through each axis.  If the pos coordinate is between the min and max, use that
		// coordinate.  Otherwise, use the min or max coordinate that is closest.
		for (int axis = X; axis <= Z; axis++)
		{
			if (axes_within_flags & (1 << axis))
			{
				// Within, use pos coord
				point[axis] = pos[axis];
			}
			else
			{
				// Figure out which end we need
				if (pos[axis] <= m_min[axis])
				{
					point[axis] = m_min[axis];
				}
				else
				{
					point[axis] = m_max[axis];
				}
			}
		}
	}
}


}