#define PLANE_EPSILON 0.0001f/** * A class to represent a plane. */class CPlane{public:	util::CVector3d m_normal;	///< The normal of the plane	float m_distance;		///< The plane-shift constant, the distance of the plane							///< from the origin. Is calculated by the dot product				///< between the normal and a point on the plane.										/**	 * Default constructor	 */	CPlane()		:	m_normal(0,0,0), m_distance(0)	{	}	/**	 * Constructor	 * \param n A const reference to a CVector3d object that is the normal of the plane	 * \param s The plane-shift constant (the dot product with the normal and a point on the plane	 */	CPlane( const util::CVector3d& n, float s )		:	m_normal(n), m_distance(s)	{		m_normal.normalize();	// make sure that the vector is normalized	}	/**	 * Constructor	 * \param n A const reference to a CVector3d object that is the normal of the plane	 * \param p A point on the plane	 */	CPlane( const util::CVector3d& n, const util::CVector3d& p )		:	m_normal(n)	{		m_normal.normalize();				// make sure that the vector is normalized		m_distance = n.dotProduct(p);	// calc the plane-shift constant	}	/**	 * Assignment operator	 * \param o A const reference to a CPlane object.	 * \return A reference to 'this' CPlane object.	 */	CPlane& operator=(const CPlane& p)	{		if( this != &p ){			m_normal = p.m_normal;			m_distance = p.m_distance;		}		return *this;	}	/**	 * Calculate if a point is on the plane.	 * \param p A const reference to a CVector3d object that represents a point	 * \return A boolean value that represents whether the passed point is on the plane	 */	bool pointOnPlane(const util::CVector3d& p) const	{		/*			Because the plane shift constant is the dot product between the normal of the plane,			to find out if the point is on the plane all we need to do is calc the dot product			of the normal and the passed point and see if that equals the shift constant we have		 */		float dot = m_normal.dotProduct(p);		return ( dot >= m_distance - PLANE_EPSILON  ||  dot <= m_distance + PLANE_EPSILON );	}	/**	 * Find if the passed ray intersects with this plane.	 * \param o The origin point of the vector.	 * \param d The direction of the vector (normalized)	 * \return 'true' if the plane and ray intersect, else 'false'	 */	bool intersect(const util::CVector3d& o, const util::CVector3d& d) const	{		util::CVector3d dir(d);				float len = dir.getLengthSquared();		// who wants to do an unecessary sqrt?		if( !(len >= 1.0f - PLANE_EPSILON  ||  len <= 1.0f + PLANE_EPSILON) )			dir.normalize();					// make sure the vector is normalized		// if direction of the ray is perpendicular to the plane normal, then the plane		// and the direction are parallel, so there won't even be an intersection,		// or, the ray is part of the plane (completely on it)		float dot = m_normal.dotProduct(dir);		if( !(dot >= 0.0f - PLANE_EPSILON  ||  dot <= 0.0f + PLANE_EPSILON) )			return false;		// no intersection		return true;		// there is an intersection	}	/**	 * Return the point of intersection between the ray and plane.	 * \param o The origin point of the vector.	 * \param d The direction of the vector (normalized)	 * \param intersection A reference to a vector in which the	 * intersection point, if there is one, will be stored.	 * \return 'true' if there is an intersection point, else 'false'	 */	bool intersectionPoint(const util::CVector3d& o, const util::CVector3d& d, util::CVector3d& intersection) const	{		float t;		/* calculate the intersection point */		util::CVector3d dir(d);				float len = dir.getLengthSquared();		// who wants to do an unecessary sqrt?		if( !(len >= 1.0f - PLANE_EPSILON  ||  len <= 1.0f + PLANE_EPSILON) )			dir.normalize();					// make sure the vector is normalized		// get the distance from the origin point of the ray along direction 'd' to 'this' plane		if( !distance(o,dir,t) )			return false;		// no intersection point		// calcuate the intersection point by starting with the origin of the ray and 		// scaling the ray's direction vector by the distance between the start point and		// the plane.		intersection = o + (dir * t);		return true;	}	/**	 * Returns 'true' if there is an intersection between the ray and 'this' plane.	 * \param o The origin point of the vector.	 * \param d The direction of the vector (normalized)	 * \param dist The distance from the origin point to 'this' plane	 * \return 'true' if the distance is calculated, else 'false	 */	bool distance(const util::CVector3d& o, const util::CVector3d& d, float& dist) const	{		// check to see if there is an intersection point		if( !intersect(o,d) )			return false;		/* calculate the intersection point */		util::CVector3d dir(d);				float len = dir.getLengthSquared();		// who wants to do an unecessary sqrt?		if( !(len >= 1.0f - PLANE_EPSILON  ||  len <= 1.0f + PLANE_EPSILON) )			dir.normalize();					// make sure the vector is normalized		// http://www.siggraph.org/education/materials/HyperGraph/raytrace/rayplane_intersection.htm		// this finds the distance between the origin point and the plane in the direction		// of 'd'		dist = (-(m_normal.dotProduct(o) + m_distance)) / m_normal.dotProduct(dir);		return true;	}};

/** * Default constructor * NOTE:: min is the bottom left corner toward the negative z-axis * NOTE:: max is the top right corner toward the positive z-axis */aabbox::aabbox():	min(util::CVector3d(-1,-1,-1)), max(util::CVector3d(1,1,1)){}/** * Constructor * NOTE:: min is the bottom left corner toward the negative z-axis * NOTE:: max is the top right corner toward the positive z-axis * \param nmin The new min value of the AABB * \param nmax The new max value of the AABB */aabbox::aabbox(const util::CVector3d& nmin, const util::CVector3d& nmax ):	min(nmin), max(nmax){}/** * Copy constructor * NOTE:: min is the bottom left corner toward the negative z-axis * NOTE:: max is the top right corner toward the positive z-axis */aabbox::aabbox(const aabbox&a):	min(a.min), max(a.max){}/** * Overloaded assignment operator. * \param a A const reference to an aabbox object * \return A reference to 'this' aabbox object */aabbox& aabbox::operator=(const aabbox& a){	if( &a != this ){		min = a.min;		max = a.max;	}	return *this;}/** * Add a point to the inside of the AABB and increase * the AABB max and min points if necessary to ensure that * point <= max && point >= min */void aabbox::addPoint(const util::CVector3d& p){	if( p.x > max.x )	max.x = p.x;	if( p.y > max.y )	max.y = p.y;	if( p.z > max.z )	max.z = p.z;	if( p.x < min.x )	min.x = p.x;	if( p.y < min.y )	min.y = p.y;	if( p.z < min.z )	min.z = p.z;}/** * Check if a point is within the bounds of the aabbox. * \param p A const reference to a CVector3d object * \return 'true' if the point is within the AABB, else 'false' */bool aabbox::checkPoint(const util::CVector3d& p) const{	// if point 'p' is within the AABB bounds	if( p.x >= min.x  &&  p.x <= max.x )		if( p.y >= min.y  &&  p.y <= max.y )			if( p.z >= min.z  &&  p.z <= max.z )				return true;	return false;}/** * Reset the bounding box. * \param initValue a reference to a CVector3d object that is the new min and max. */void aabbox::reset(const util::CVector3d& initValue){	min = initValue;	max = initValue;}/** * Return if the aabbox and the passed parameter aabbox intersect. * \param b Another aabbox to test for intersection with 'this' aabbox * \return 'true' if the two boxes intersect, else 'false' */bool aabbox::intersection(const aabbox& b) const{	if( (this->min.x > b.max.x) || (b.min.x > this->max.x) )		return false;	if( (this->min.y > b.max.y) || (b.min.y > this->max.y) )		return false;	if( (this->min.z > b.max.z) || (b.min.z > this->max.z) )		return false;	return true;	// the boxes intersect}/** * Return true if there is an intersection between the passed ray * and 'this' aabbox. * This creates 6 planes, one for each side of the aabb and then uses * Woo's method to find if there is an intersection with the ray. * http://gamealgorithms.tar.hu/ch22lev1sec2.html * * \param o A const reference to a CVector3d object that is the origin point of the ray. * \param d A const reference to a CVector3d object that is the direction of the ray (normalized vector) * \param intersection A reference to a CVector3d object in which the intersection point will be stored * if there is a valid point. * \return 'true' if there is an intersection, else 'false' */bool aabbox::intersection(const util::CVector3d& o, const util::CVector3d& d, util::CVector3d& intersection) const{	util::CPlane sides[6];	// 6 planes making up each side of the aabb	util::CPlane can[3];	// the 3 candidate planes	util::CVector3d ntemp;	// temp vector for holding the aabb normals	// top of the box	ntemp = util::CVector3d(0,1,0);	sides[0] = util::CPlane( ntemp, ntemp.dotProduct(max) );	// front of the box	ntemp = util::CVector3d(0,0,1);	sides[1] = util::CPlane( ntemp, ntemp.dotProduct(max) );	// right of the box	ntemp = util::CVector3d(1,0,0);	sides[2] = util::CPlane( ntemp, ntemp.dotProduct(max) );	// bottom of the box	ntemp = util::CVector3d(0,-1,0);	sides[3] = util::CPlane( ntemp, ntemp.dotProduct(min) );	// left of the box	ntemp = util::CVector3d(-1,0,0);	sides[4] = util::CPlane( ntemp, ntemp.dotProduct(min) );	// back of the box	ntemp = util::CVector3d(0,0,-1);	sides[5] = util::CPlane( ntemp, ntemp.dotProduct(min) );	// select the three different planes	int count=0;		// the number of planes to test against	for(int i=0, j=0; (i < 6) && (j < 3); i++){		// If the dot product is less than 0, this indicates that		// the vectors are going in roughly opposite directions, which		// is what we want, because with woo's method, the back-facing faces		// are discarded (faces with normals in roughly the same direction		// as the ray).		// This also ignores planes if they are perpendicular to the ray.		if( d.dotProduct(sides.m_normal) < 0 ){			can[j] = sides;			j++;			count++;		}	}	// By this point we have at most 3 planes to test against.		// Woo's method consists of choosing the 3 best planes 	// (or however many we can get, if less than 3),	// and we find the intersection distances between the ray and	// the planes. The largest of these distances corresponds to a	// potential hit. If a potential hit is found, the intersection	// point is computed. If it is within the AABB, then it is a	// real hit.	float largest = 0.0f;	float dist = 0.0f;	int hit = 0;	for(i=0; i < count; i++){		// get the distance to this plane from the ray origin along the ray		if( can.distance(o,d,dist) ){			if( dist > largest ){				largest = dist;		// new largest				hit = i;			}		}	}	// By this point we should have a single plane which is the best potential	// hit, according to Woo's method. Now, we calculate the intersection point,	// and if it is within the aabbox, then we have an actual hit, and the coords	// of the hit.	util::CVector3d hit_point;	// get the intersection point between the ray and the specified plane	if( can[hit].intersectionPoint( o, d, hit_point ) ){		// if the intersection point is within the AABB		if( checkPoint( hit_point ) ){			intersection = hit_point;	// set the intersection point (a real hit)			return true;		}	}	return false;}

int main(){		util::aabbox box;	box.addPoint(util::CVector3d(-1,-1,-1) );	box.addPoint(util::CVector3d(2,1,1) );	// intersection right side, with ray. Intersection should be (2,0,0)	util::CVector3d o(3,0,0);	util::CVector3d d(-1,0,0);	util::CVector3d p;	if( box.intersection(o,d,p) ){		printf("\n Right Intersection found: %.3f %.3f %.3f\n", p.x, p.y, p.z);	}	else		printf("\n Right Nothing found \n");	// intersection left side, with ray. Intersection should be (-1,0.5,0)	o = util::CVector3d(-2,0.5f,0);	d = util::CVector3d(1,0,0);	if( box.intersection(o,d,p) ){		printf("\n Left Intersection found: %.3f %.3f %.3f\n", p.x, p.y, p.z);	}	else		printf("\n Left Nothing found \n");return 0;}