//=======================================================================//
// File:		sphere.cpp                                                   //
// Project: Architecture                                                 //
// Author:	J.M. Albertson                                               //
//-----------------------------------------------------------------------//
// Copyright (C) 1994, Virtual World Entertainments, All Rights reserved //
// PROPRIETARY AND CONFIDENTIAL                                          //
//=======================================================================//

#include "StuffHeaders.hpp"

Sphere
	Sphere::s_Identity(0.0f, 0.0f, 0.0f, 0.0f);

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
int
	Sphere::ComputeBounds(ReadOnlyArrayOf<Point3D> &points)
{
	Check_Object(&points);

	//
	//-----------------------------------------------------------
	// First we find the two points farthest away from each other
	//-----------------------------------------------------------
	//
	Vector3D delta;
	int first=-1, second=-1;
	Scalar distance=0.0f;
	int i,j;
	int count = points.GetLength();
	for (i=0; i<count-1; ++i)
	{
		for (j=i+1; j<count; ++j)
		{
			delta.Subtract(points[i], points[j]);
			Scalar d = delta.GetLengthSquared();
			if (d > distance)
			{
				distance = d;
				first = i;
				second = j;
			}
		}
	}

	//
	//--------------------------------------------------------
	// Assume the sphere is at the average of these two points
	//--------------------------------------------------------
	//
	radius = Sqrt(distance) * 0.5f;
	center.Lerp(points[first], points[second], 0.5f);

	//
	//----------------------------------------------------------------------
	// Test all the points against the sphere, looking for the one thats has
	// the most error.  If none are out of bounds, return this sphere,
	// otherwise copy it for later reference
	//----------------------------------------------------------------------
	//
	radius += SMALL;
	int third = -1;
	distance = radius*radius;
	for (i=0; i<count; ++i)
	{
		delta.Subtract(points[i], center);
		Scalar d = delta.GetLengthSquared();
		if (d > distance)
		{
			distance = d;
			third = i;
		}
	}
	if (third == -1)
		return 1;
	Sphere chord(*this);

	//
	//------------------------------------------------------------
	// Now we build a circumscribed sphere around the three points
	//------------------------------------------------------------
	//
	Vector3D v1,v2,v3;
	v1.Subtract(points[second], points[first]);
	v2.Subtract(points[third], points[first]);
	v3.Subtract(points[third], points[second]);
	double d1 = v2*v1;
	double d2 = -(v3*v1);
	double d3 = v2*v3;
	double c1 = d2*d3;
	double c2 = d3*d1;
	double c3 = d1*d2;
	double c = c1 + c2 + c3;
	radius = static_cast<Scalar>(0.5f*sqrt((d1+d2)*(d2+d3)*(d3+d1)/c));
	c = 0.5f/c;
	center.x = static_cast<Scalar>((points[first].x*(c2+c3) + points[second].x*(c3+c1) + points[third].x*(c1+c2))*c);
	center.y = static_cast<Scalar>((points[first].y*(c2+c3) + points[second].y*(c3+c1) + points[third].y*(c1+c2))*c);
	center.z = static_cast<Scalar>((points[first].z*(c2+c3) + points[second].z*(c3+c1) + points[third].z*(c1+c2))*c);

	//
	//----------------------------------------------------------------------
	// Test all the points against the sphere, looking for the one thats has
	// the most error.  If none are out of bounds, return this sphere
	//----------------------------------------------------------------------
	//
	radius += SMALL;
	int fourth = -1;
	distance = radius*radius;
	for (i=0; i<count; ++i)
	{
		delta.Subtract(points[i], center);
		Scalar d = delta.GetLengthSquared();
		if (d > distance)
		{
			distance = d;
			fourth = i;
		}
	}
	if (fourth == -1)
		return 2;

	//
	//-------------------------------------------------------------------
	// OK, now we know that neither the circumscribed nor chord sphere is
	// sufficient to hold the points, so we are in a guessing game.  Build
	// a sphere at the centroid to function as an upper clamp for the
	// iteration
	//-------------------------------------------------------------------
	//
	Sphere centroid=s_Identity;
	for (i=0; i<count; ++i)
		centroid.center += points[i];
	centroid.center /= static_cast<Scalar>(i);
	centroid.radius = 0.0f;
	for (i=0; i<count; ++i)
	{
		delta.Subtract(points[i], centroid.center);
		Scalar d = delta.GetLengthSquared();
		if (d > centroid.radius)
			centroid.radius = d;
	}
	centroid.radius = Sqrt(centroid.radius) + SMALL;

	//
	//-----------------------------------------------------------------
	// Grow the two spheres by the biggest points that was out.  If the
	// centroid is now the smallest sphere, return it
	//-----------------------------------------------------------------
	//
Try_Again:
	Scalar chord_distance=0.0f;
	if (third<count)
	{
		chord.Union(points[third]);
		chord_distance=chord.radius*chord.radius;
		third = -1;
	}
	if (fourth<count)
	{
		Union(points[fourth]);
		distance = radius*radius;
		fourth = -1;
	}
	if (centroid.radius<radius && centroid.radius<chord.radius)
	{
		*this = centroid;
		return 3;
	}

	//
	//---------------------------------------------------------
	// Now search for the biggest points that are out of bounds
	//---------------------------------------------------------
	//
	for (i=0; i<count; ++i)
	{
		delta.Subtract(points[i], chord.center);
		Scalar d = delta.GetLengthSquared();
		if (third<i && d>chord_distance)
		{
			third = i;
			chord_distance = d;
		}
		delta.Subtract(points[i], center);
		d = delta.GetLengthSquared();
		if (fourth<i && d>distance)
		{
			fourth = i;
			distance = d;
		}
	}

	//
	//----------------------------------------------------------------------
	// If the chord sphere is done, and its smaller than the current sphere,
	// return it
	//----------------------------------------------------------------------
	//
	if (third == -1 || third == i)
	{
		third = i;
		if (chord.radius < radius)
		{
			*this = chord;
			return 4;
		}
	}

	//
	//------------------------------------------------------------------------
	// If the circumscribed sphere is done, and smaller than the chord sphere,
	// return it
	//------------------------------------------------------------------------
	//
	if (fourth == -1 || fourth == i)
	{
		fourth = i;
		if (chord.radius > radius)
			return 5;
	}
	goto Try_Again;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Sphere&
	Sphere::Union(
		const Sphere& sphere1,
		const Sphere& sphere2
	)
{
	Check_Object(this);
	Check_Object(&sphere1);
	Check_Object(&sphere2);

	//
	//--------------------------------------------------
	// Calculate the length between the sphere midpoints
	//--------------------------------------------------
	//
	Vector3D dist;
	dist.Subtract(sphere1.center, sphere2.center);
	Scalar len = dist.GetLength();

	//
	//------------------------------------------------------
	// If the sphere is contained in the old sphere, move on
	//------------------------------------------------------
	//
	if (len + sphere1.radius <= sphere2.radius)
	{
		*this = sphere2;
		return *this;
	}

	//
	//----------------------------------------------------------
	// If the new sphere contains the old sphere, use it instead
	//----------------------------------------------------------
	//
	if (len + sphere2.radius <= sphere1.radius)
	{
		*this = sphere1;
		return *this;
	}

	//
	//------------------------------
	// Calculate the new centerpoint
	//------------------------------
	//
	len += sphere1.radius + sphere2.radius;
	UnitVector3D direction;
	direction.Normalize(dist);
	len *= 0.5f;
	center.AddScaled(
		sphere2.center,
		direction,
		len - sphere2.radius
	);
	radius = len;
	return *this;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
Sphere&
	Sphere::Union(const Point3D& point)
{
	Vector3D v;
	v.Subtract(point, center);
	Scalar distance = v.GetLengthSquared();
	if (distance <= radius*radius)
		return *this;
	distance = Sqrt(distance);
	radius = (radius + distance) * 0.5f;

	//
	//---------------------------------------------------------------
	// Compute the new center point by lerping between old center and
	// the out point
	//---------------------------------------------------------------
	//
	Scalar t = distance - radius;
	distance = 1.0f / distance;
	center.x = (center.x*radius + point.x*t)*distance;
	center.y = (center.y*radius + point.y*t)*distance;
	center.z = (center.z*radius + point.z*t)*distance;
	radius += SMALL;
	return *this;
}

//
//###########################################################################
//###########################################################################
//
#if !defined(Spew)
	void
		Spew(
			const char* group,
			const Sphere& sphere
		)
	{
		Check_Object(&sphere);

		SPEW((group, "\n\tSphere Centerpoint: +"));
		Spew(group, sphere.center);
		SPEW((group, "\tRadius: %f", sphere.radius));
	}
#endif