#include "stdafx.h"
#include "IceHelpers.h"

// Misc functions borrowed & adapted from ICE

void RotX(Matrix3x3& m, float angle)
{
	float Cos = cosf(angle);
	float Sin = sinf(angle);
	m.Identity();
	m.m[1][1] = m.m[2][2] = Cos;
	m.m[2][1] = -Sin;
	m.m[1][2] = Sin;
}

void RotY(Matrix3x3& m, float angle)
{
	float Cos = cosf(angle);
	float Sin = sinf(angle);
	m.Identity();
	m.m[0][0] = m.m[2][2] = Cos;
	m.m[2][0] = Sin;
	m.m[0][2] = -Sin;
}

void RotZ(Matrix3x3& m, float angle)
{
	float Cos = cosf(angle);
	float Sin = sinf(angle);
	m.Identity();
	m.m[0][0] = m.m[1][1] = Cos;
	m.m[1][0] = -Sin;
	m.m[0][1] = Sin;
}

bool SegmentSphere(const Point& origin, const Point& dir, float length, const Point& center, float radius, float& dist, Point& hit_pos)
{
	// get the offset vector
	Point offset = center - origin;

	// get the distance along the ray to the center point of the sphere
	float ray_dist = dir | offset;

	// get the squared distances
	float off2 = offset | offset;
	float rd2 = radius * radius;
	if(off2 <= rd2)
	{
		// we're in the sphere
		hit_pos	= origin;
		dist	= 0.0f;
		return true;
	}

	if(ray_dist <= 0 || (ray_dist - length) > radius)
	{
		// moving away from object or too far away
		return false;
	}

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

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

	// sort out the details
	hit_pos = origin + dir * dist;
	return true;
}

bool /*Ctc::*/RayAABB2(const Point& min, const Point& max, const Point& origin, const Point& dir, Point& coord)
{
	BOOL Inside = TRUE;
	Point MaxT;
	MaxT.x=MaxT.y=MaxT.z=-1.0f;

	// Find candidate planes.
	for(udword i=0;i<3;i++)
	{
		if(origin[i] < min[i])
		{
			coord[i]	= min[i];
			Inside		= FALSE;

			// Calculate T distances to candidate planes
			if(IR(dir[i]))	MaxT[i] = (min[i] - origin[i]) / dir[i];
		}
		else if(origin[i] > max[i])
		{
			coord[i]	= max[i];
			Inside		= FALSE;

			// Calculate T distances to candidate planes
			if(IR(dir[i]))	MaxT[i] = (max[i] - origin[i]) / dir[i];
		}
	}

	// Ray origin inside bounding box
	if(Inside)
	{
		coord = origin;
		return true;
	}

	// Get largest of the maxT's for final choice of intersection
	udword WhichPlane = 0;
	if(MaxT[1] > MaxT[WhichPlane])	WhichPlane = 1;
	if(MaxT[2] > MaxT[WhichPlane])	WhichPlane = 2;

	// Check final candidate actually inside box
	if(IR(MaxT[WhichPlane])&0x80000000) return false;

	for(udword i=0;i<3;i++)
	{
		if(i!=WhichPlane)
		{
			coord[i] = origin[i] + MaxT[WhichPlane] * dir[i];
#ifdef RAYAABB_EPSILON
			if(coord[i] < min[i] - RAYAABB_EPSILON || coord[i] > max[i] + RAYAABB_EPSILON)	return false;
#else
			if(coord[i] < min[i] || coord[i] > max[i])	return false;
#endif
		}
	}
	return true;	// ray hits box
}

static const bool gNormalize = true;

udword /*Ctc::*/RayCapsuleOverlap(const Point& origin, const Point& dir, const LSS& capsule, float s[2])
{
	// set up quadratic Q(t) = a*t^2 + 2*b*t + c
	Point kU, kV, kW, capsDir;
	capsule.ComputeDirection(capsDir);
	kW = capsDir;
	
	float fWLength = kW.Magnitude();
	kW.Normalize();
	
	// generate orthonormal basis
	
    float fInvLength;
	if ( fabsf(kW.x) >= fabsf(kW.y) )
    {
        // W.x or W.z is the largest magnitude component, swap them
		fInvLength = 1.0f/sqrtf(kW.x*kW.x + kW.z*kW.z);
        kU.x = -kW.z*fInvLength;
        kU.y = 0.0f;
        kU.z = +kW.x*fInvLength;
    }
    else
    {
        // W.y or W.z is the largest magnitude component, swap them
        fInvLength = 1.0f/sqrtf(kW.y*kW.y + kW.z*kW.z);
        kU.x = 0.0f;
        kU.y = +kW.z*fInvLength;
        kU.z = -kW.y*fInvLength;
    }
    kV = kW^kU;
kU.Normalize();
	if(gNormalize)
	kV.Normalize();

	// compute intersection

	Point kD(kU|dir, kV|dir, kW|dir);
	float fDLength = kD.Magnitude();
	kD.Normalize();

	float fInvDLength = 1.0f/fDLength;
	Point kDiff = origin - capsule.mP0;
	Point kP(kU|kDiff, kV|kDiff, kW|kDiff);
	float fRadiusSqr = capsule.mRadius*capsule.mRadius;

	float fInv, fA, fB, fC, fDiscr, fRoot, fT, fTmp;

	// Is the velocity parallel to the capsule direction? (or zero)
	if ( fabsf(kD.z) >= 1.0f - FLT_EPSILON || fDLength < FLT_EPSILON )
		{

		float fAxisDir = dir|capsDir;

		fDiscr = fRadiusSqr - kP.x*kP.x - kP.y*kP.y;
		if ( fAxisDir < 0 && fDiscr >= 0.0f )
			{
			// Velocity anti-parallel to the capsule direction
			fRoot = sqrtf(fDiscr);
			s[0] = (kP.z + fRoot)*fInvDLength;
			s[1] = -(fWLength - kP.z + fRoot)*fInvDLength;
			return 2;
			}
		else if ( fAxisDir > 0  && fDiscr >= 0.0f )
			{
			// Velocity parallel to the capsule direction
			fRoot = sqrtf(fDiscr);
			s[0] = -(kP.z + fRoot)*fInvDLength;
			s[1] = (fWLength - kP.z + fRoot)*fInvDLength;
			return 2;
			}
		else
			{
			// sphere heading wrong direction, or no velocity at all
			return 0;
			}   
		}

	// test intersection with infinite cylinder
	fA = kD.x*kD.x + kD.y*kD.y;
	fB = kP.x*kD.x + kP.y*kD.y;
	fC = kP.x*kP.x + kP.y*kP.y - fRadiusSqr;
	fDiscr = fB*fB - fA*fC;
	if ( fDiscr < 0.0f )
		{
		// line does not intersect infinite cylinder
		return 0;
		}

	int iQuantity = 0;

	if ( fDiscr > 0.0f )
		{
		// line intersects infinite cylinder in two places
		fRoot = sqrtf(fDiscr);
		fInv = 1.0f/fA;
		fT = (-fB - fRoot)*fInv;
		fTmp = kP.z + fT*kD.z;
		if ( 0.0f <= fTmp && fTmp <= fWLength )
			s[iQuantity++] = fT*fInvDLength;

		fT = (-fB + fRoot)*fInv;
		fTmp = kP.z + fT*kD.z;
		if ( 0.0f <= fTmp && fTmp <= fWLength )
			s[iQuantity++] = fT*fInvDLength;

		if ( iQuantity == 2 )
			{
			// line intersects capsule wall in two places
			return 2;
			}
		}
	else
		{
		// line is tangent to infinite cylinder
		fT = -fB/fA;
		fTmp = kP.z + fT*kD.z;
		if ( 0.0f <= fTmp && fTmp <= fWLength )
			{
			s[0] = fT*fInvDLength;
			return 1;
			}
		}

	// test intersection with bottom hemisphere
	// fA = 1
	fB += kP.z*kD.z;
	fC += kP.z*kP.z;
	fDiscr = fB*fB - fC;
	if ( fDiscr > 0.0f )
		{
		fRoot = sqrtf(fDiscr);
		fT = -fB - fRoot;
		fTmp = kP.z + fT*kD.z;
		if ( fTmp <= 0.0f )
			{
			s[iQuantity++] = fT*fInvDLength;
			if ( iQuantity == 2 )
				return 2;
			}

		fT = -fB + fRoot;
		fTmp = kP.z + fT*kD.z;
		if ( fTmp <= 0.0f )
			{
			s[iQuantity++] = fT*fInvDLength;
			if ( iQuantity == 2 )
				return 2;
			}
		}
	else if ( fDiscr == 0.0f )
		{
		fT = -fB;
		fTmp = kP.z + fT*kD.z;
		if ( fTmp <= 0.0f )
			{
			s[iQuantity++] = fT*fInvDLength;
			if ( iQuantity == 2 )
				return 2;
			}
		}

	// test intersection with top hemisphere
	// fA = 1
	fB -= kD.z*fWLength;
	fC += fWLength*(fWLength - 2.0f*kP.z);

	fDiscr = fB*fB - fC;
	if ( fDiscr > 0.0f )
		{
		fRoot = sqrtf(fDiscr);
		fT = -fB - fRoot;
		fTmp = kP.z + fT*kD.z;
		if ( fTmp >= fWLength )
			{
			s[iQuantity++] = fT*fInvDLength;
			if ( iQuantity == 2 )
				return 2;
			}

		fT = -fB + fRoot;
		fTmp = kP.z + fT*kD.z;
		if ( fTmp >= fWLength )
			{
			s[iQuantity++] = fT*fInvDLength;
			if ( iQuantity == 2 )
				return 2;
			}
		}
	else if ( fDiscr == 0.0f )
		{
		fT = -fB;
		fTmp = kP.z + fT*kD.z;
		if ( fTmp >= fWLength )
			{
			s[iQuantity++] = fT*fInvDLength;
			if ( iQuantity == 2 )
				return 2;
			}
		}

	return iQuantity;
}

