#include "stdafx.h"
#include <math.h>
#include <stdlib.h>
#include <assert.h>

// Quick-and-dirty minimal "vector" class
struct vector
{
	vector()													{}
	vector(float _x, float _y, float _z) : x(_x), y(_y), z(_z)	{}

	void	Normalize()
			{
				float M = x*x + y*y + z*z;
				if(M)
				{
					M = 1.0f / sqrtf(M);
					x *= M;
					y *= M;
					z *= M;
				}
			}

	vector		operator+(const vector& p)		const		{ return vector(x + p.x, y + p.y, z + p.z);	}
	vector		operator-(const vector& p)		const		{ return vector(x - p.x, y - p.y, z - p.z);	}
	float		operator*(const vector& p)		const		{ return x*p.x + y*p.y + z*p.z;				}
	vector		operator*(float s)				const		{ return vector(x * s,   y * s,   z * s );	}

	float x,y,z;
};

// Vanilla ray-sphere intersection function, found online.

// determines a collision between a ray and a sphere
//
// ray_start:	the start pos of the ray
// ray_dir:		the normalized direction of the ray
// length:		length of ray to check
// sphere_pos:	sphere position
// sphere_rad:	sphere redius
// hit_time:	(OUT) if a collision, contains the distance from ray.pos
// hit_pos:		(OUT) if a collision, contains the world point of the collision
//
// returns: true if a collision occurred
//
static bool collide_ray_sphere( const vector &ray_start, const vector &ray_dir, float length, const vector &sphere_pos, float sphere_rad, float &hit_time, vector &hit_pos )
{
	// get the offset vector
	const vector offset = sphere_pos - ray_start;

	// get the distance along the ray to the center point of the sphere
	const float ray_dist = ray_dir * offset;
	if( ray_dist <= 0 || (ray_dist - length) > sphere_rad) {
		// moving away from object or too far away
		return false;
	}

	// get the squared distances
	const float off2 = offset * offset;
	const float rad2 = sphere_rad * sphere_rad;
	if( off2 <= rad2 ) {
		// we're in the sphere
		hit_pos = ray_start;
		hit_time = 0;
		return true;
	}

	// find hit distance squared
	const float d = rad2 - (off2 - ray_dist * ray_dist);
	if( d < 0 ) {
		// ray passes by sphere without hitting
		return false;
	}

	// get the distance along the ray
	hit_time = (float)(ray_dist - sqrt( d ));
	if( hit_time > length ) {
		// hit point beyond length
		return false;
	}

	// sort out the details
	hit_pos = ray_start + ray_dir * hit_time;
	hit_time /= length;
	return true;
}

// Modified version calls the previous function, but moves the ray origin closer to the sphere. The test accuracy is
// greatly improved as a result. This is an idea proposed on the GD-Algorithms list by Eddie Edwards.
static bool collide_ray_sphere_robust(const vector& origin, const vector& dir, float length, const vector& center, float radius, float& dist, vector& hit_pos)
{
	const vector x = origin - center;
	float l = sqrtf(x*x) - radius - 10.0f;
	if(l<0.0f)
		l=0.0f;
	const vector NewOrigin = origin + dir*l;

	bool Status = collide_ray_sphere( NewOrigin, dir, length - l, center, radius, dist, hit_pos );
	if(Status)
		dist += l/length;
	return Status;
}

#define ONE_OVER_RAND_MAX		(1.0f / float(RAND_MAX))	//!< Inverse of the max possible value returned by rand()
static float UnitRandomFloat()
{
	return float(rand()) * ONE_OVER_RAND_MAX;
}

//#define USE_ROBUST_TEST

int main(int argc, char* argv[])
{
	// The test:
	// - we compute a random ray (origin + direction)
	// - we locate a unit sphere N units along the ray direction, starting from the ray origin.
	// - we increase N until the test fails. (In theory of course, it should never fail)
	// With the original code the test can fail when N is only ~2000 units. With the modified code, it never fails.

	const float SphereRadius = 1.0f;

	while(1)
	{
		// Use a random origin in the [-500 ; +500] cube
		// UnitRandomFloat() returns a random value in [0;1]
		vector Orig;
		Orig.x = (UnitRandomFloat()-0.5f)*1000.0f;
		Orig.y = (UnitRandomFloat()-0.5f)*1000.0f;
		Orig.z = (UnitRandomFloat()-0.5f)*1000.0f;

		// Use a random direction
		vector Dir;
		Dir.x = UnitRandomFloat()-0.5f;
		Dir.y = UnitRandomFloat()-0.5f;
		Dir.z = UnitRandomFloat()-0.5f;
		Dir.Normalize();

		float SphereDist = SphereRadius;
		bool Intersect = true;
		while(Intersect)
		{
			SphereDist += SphereRadius;
			const float SegmentLength = SphereDist + SphereRadius * 10.0f;

			// Locate the sphere exactly on the ray, "SphereDist" units from the origin
			vector SpherePos = Orig + Dir*SphereDist;

			// Segment-sphere raycast
			float dist;
			vector hit_pos;
#ifdef USE_ROBUST_TEST
			Intersect = collide_ray_sphere_robust( Orig, Dir, SegmentLength, SpherePos, SphereRadius, dist, hit_pos );
#else
			Intersect = collide_ray_sphere( Orig, Dir, SegmentLength, SpherePos, SphereRadius, dist, hit_pos );
#endif
		}
		printf("Sphere distance on failure: %f\n", SphereDist);
	}
	return 0;
}

