//-stl STLFiles/cube.stl 0.001	<-Using sphere distance checking

#include "STL_Distance.h"
#include "ADFCreator.h"
#include <math.h>
#include <time.h>
#include <string.h>

#include "AxisAlignedBox.h"
#include "DebugSphere.h"
#include "Utils.h"

#include "ISDF.h"
#include "JitteringStlSdfAble.h"
#include "ConvexUmbrellaSdfAlgoFactory.h"
#include "PseudoNormalSdfAlgoFactory.h"
#include "CUvsPN_CompareSdfAlgo.h"
using namespace SDF;

#include "testcases.h"

int numCubes = 0;

int IN = 1;
int ON = 0;
int OUT = -1;

bool SPHERES_ON = true;
bool MESH_ON = true;

/* multiplication factors for input interaction:			*/
/*  (these are known from previous experience)				*/

const float ANGFACT = { 1. };
const float SCLFACT = { 0.005f };
/* minimum allowable scale factor:					*/

const float MINSCALE = { 0.05f };

/* able to use the left mouse for either rotation or scaling,		*/
/* in case have only a 2-button mouse:					*/
#define ROTATE		0
#define SCALE		1


/* active mouse buttons (or them together):				*/
const int LEFT   = { 4 };
const int MIDDLE = { 2 };
const int RIGHT  = { 1 };


/**
** non-constant global variables:
**/

int	ActiveButton;		/* current button that is down		*/
int	AxesList;		/* list to hold the axes		*/
int	AxesOn;			/* ON or OFF				*/
int	Debug;			/* ON means print debug info		*/
int	DepthCueOn;		/* TRUE means to use intensity depth cueing */
int	Detail;			/* 0 means low detail -- higher numbers mean more detail */
int	GluiWindow;		/* the glut id for the glui window	*/
int	HighLists[9];		/* high-detail object display lists	*/
int	LeftButton;		/* either ROTATE or SCALE		*/
int	LowLists[9];		/* low-detail object display lists	*/
int	MainWindow;		/* window id for main graphics window	*/
int	MidLists[9];		/* mid-detail object display lists	*/
float	RotMatrix[4][4];	/* set by glui rotation widget		*/
float	Scale, Scale2;		/* scaling factors			*/
int	TimerInterrupted;	/* TRUE means to not process the timer event */
int	WhichColor;		/* index into Colors[]			*/
int	WhichObject;		/* object index to display		*/
int	WhichProjection;	/* ORTHO or PERSP			*/
int	Xmouse, Ymouse;		/* mouse values				*/
float	Xrot, Yrot;		/* rotation angles in degrees		*/
float	TransXYZ[3];		/* set by glui translation widgets	*/
float collide[3];		/*XYZ of the collision test cube  */
float collide_closest[3];  /*XYZ of the closest point on the surface*/
int collide_TF=0;  /* Did it Collide?  */
int SpeedTriangleSolve= FALSE;
int WhatToShow = ON;

bool draw_internal_nodes = true;

/* Function Prototypes */
void	MouseButton( int, int, int, int );
void	MouseMotion( int, int );
void drawSTL ();
void Keyboard( unsigned char c, int x, int y );
void renderCollider();
Vector GetNodeGradient(octreeNode *CollideNode, Point *CollidePoint,float);


CLEDSGeometry *STL = NULL;

// Call Program.exe -stl <STL Filename> 

//<-stl, -sphere> <STL Filename> <-td, -bu, -m> <-f, -r, -o> <ADF Filename> <errorLimit, depthLimit>
octreeNode *ADF = NULL;

// (Steve) Here's where you decide which SDF algorithm to use.
// Instantiate the appropriate factory!
//SdfAlgoFactory *sdf_algo_factory = new ConvexUmbrellaSdfAlgoFactory();
SdfAlgoFactory *sdf_algo_factory = new PseudoNormalSdfAlgoFactory();
//SdfAlgoFactory *sdf_algo_factory = new CUvsPN_SdfAlgoFactory();


// (Steve) Don't use this to keep track of number of nodes.
// Use ADF->count_offspring()+1 instead
// int totalNumOfNodes = 0;

octreeNode* CollideNode;
Point* CollidePoint;
Point* Testpoint;
octreeVtx* CollideError;


/*
Absolute Value for Double Values Function
*/
double abs_d (double num) {
	if (num >= 0) {
		return num;
	}
	else {
		return -num;
	}
}

void printADFUsage()
{
	printf( "Usage: ADFCreator.exe { -sphere | -none | -load } file percent_error max_levels\n" );
	printf( "   where \n" );
	printf( "          -sphere = use sphere tree optimization\n" );
	printf( "          -none = use no optimization\n" );
	printf( "          -load = load the given ADF file\n" );
	printf( "          file = the adf or stl file\n" );
	printf( "          percent_error = the minimum percentage that the interpolated distance can be of real distance\n" );
	printf( "                (e.g. interDist / realDistance >= (percent_error/100) for all the leaf nodes)\n" );
	printf( "          max_levels = the maximum number of levels that should be traversed\n" );
}

void test_ADF_with_random_points(double error)
{
	double t1,t2;
	
	int NUM_TEST_PTS=1000;
	int count = 0;
	printf("Starting %d Random Test Points\n",NUM_TEST_PTS);
	t1=clock();
	for(int bar=0; bar<NUM_TEST_PTS;bar++)
	{
		if( RandomPointCollider(error) )
			count++;
		
	}
	t2=clock();
	printf("Done Testing %d Random Points in %f msec\n",NUM_TEST_PTS,difftime(t2,t1));
	printf("Average %f Hz\n",(float)(1000.0*(float)NUM_TEST_PTS)/difftime(t2,t1));
	printf("Passed %d out of %d tests. Thats %f percent!\n" , count , NUM_TEST_PTS , 100.0 * (float)count / (float)NUM_TEST_PTS );
}


octreeNode *create_arbitrary_bounding_box(const Point &origin,
										  double edge_length, SDF::ISDF *model);

// (Steve) The simpler algorithm is probably more robust! UPDATE - but..wrong.
//TriMeshSdfAlgorithm *sdf_algo = new SimplerConvexUmbrellaSdfAlgo();


void run_ADF_generation( int argc, char* argv[] )
{
	// (Steve) initializing these vars to sensible values
	double t1 = 0, t2 = 0, SphereTime = 0;
	char* STLArgs[2];
	
	char stlFilename[100];
	strcpy(stlFilename, argv[2]);
	
	bool using_sphere_tree = FALSE;

	
	STLArgs[0] = argv[0];
	STLArgs[1] = argv[2];

	printf("Opening File: %s\n",argv[2]); 
	// Parse the Input STL File and Build Up LEDS Data Structure
	STL = ParseSTL(2, STLArgs);


#if DEBUG_MINDISTSTL_WITH_POISON_PT
	debug_just_one_point();
	return;
#endif
	
	cout << "Determining Bounding Box..";
#if DEBUG_USE_SPHERE_INSTEAD_OF_STL
	DebugSphere model;
	ADF = create_arbitrary_bounding_box(Point(-150, -150, -150), 300.0, &model);
#else
	ADF = createBoundingBox(STL);
#endif
	cout << "Done!" << endl;
	
	if (strcmp(argv[1], "-sphere") == 0) 
	{
		SpeedTriangleSolve=TRUE;
		using_sphere_tree = TRUE;
		printf("Creating Sphere Tree for Distance Checking\n");
		t1=clock();
		LoadSTL(STLArgs[1] , STL);
		CreateSphereTree();
		t2=clock();
		SphereTime=difftime(t2,t2);
		printf("Sphere tree built in %f msec!\n",SphereTime);
	}
	
	
	const double error = atof(argv[3]) /100.0F;

#if USE_FIXED_DISTANCE_TOLERANCE
	// (Steve) Here we don't use the error - we take arg 3 as a straight up float.
	octreeNode::adf_distance_tolerance = atof(argv[3]);
#else
	octreeNode::adf_error_limit = error;
#endif

	const int maxSubDivLevels = atoi(argv[4]);

	// Use a custom jitter amount if given
	double jitter_box_length = 0.0;
	if(argc >= 6) {
		jitter_box_length = atof(argv[5]);
	}
	else {
		jitter_box_length = JITTER_BOX_HALF_LENGTH * 2.0;
	}

	//--------------------------------
	// Build it!
	//--------------------------------

	cout << "Building ADF" << endl;
	cout << "  - Subdivision limit: " << maxSubDivLevels << endl;
	cout << "  - Error tolerance (in distance units): " << octreeNode::adf_distance_tolerance << endl;
	cout << "  - Using jitter box-length (and gradient epsilon): " << jitter_box_length << endl;
	cout << "  - Bounding box edge length: " << ADF->m_edgeLength << endl;

	//---------------------------------
	//	Create an ISDF wrapper for the STL model
	//	Here is where to decide which wrapper to use!
	//---------------------------------

	// The normal one.  This does nothing too fancy - it just uses the umbrella algo.
	// But this one often returns DONT_KNOW
	//StlSdfAble model(STL);

	// This one jitters if the umbrella algo returns DONT_KNOW
	JitteringStlSdfAble model(
		STL, 
		// The gradient epsilon should be at least the jitter box length
		jitter_box_length,
		// The Point Jitterer takes a HALF length
		new PointJitterer(jitter_box_length * 0.5),
		sdf_algo_factory->create());


	//-----------------------
	//	Run tests that target specific SDF files
	//-----------------------
//	testMeshTriangleVertexPOT();
// (Steve) we can't just run this since this needs trigon to be loaded.  we should have the test case load trigon by itself..
//	testTrigonFlatFoldBug(model);
	

	// testWedgeBigBug(model);
	//testWedgeEdgeBumpiness(model);

	//testTrigonStaller(model);
	//testCrossStaller(model);
	//testFoldLeak(model);

	//testBoxTubeErrors(model);
	// testSTR(model);


	// test fold00 comparing CU vs. PN algos
	//__DBc("running tests\n");
	//model.calc_sdf_from_pt(Point(-55, -3.4375, -5));

	// Exit here if you just want to run a test
	//exit(0);

	//---------------------------------
	//	Build the ADF!
	//---------------------------------
	t1=clock();
	buildTopDownADF(model, ADF, error , maxSubDivLevels);
	t2=clock();

	//--------------------------------
	// Output some stats
	//--------------------------------
	long msecs = SphereTime + difftime(t2,t1);
	cout << endl;
	cout << "ADF Built in " << msecs << " msecs! (" << ((float)msecs)/1000.0 << " seconds)\n";
	cout << "  - Max depth of generated ADF: " << ADF->calc_max_depth() << " (0 means ADF is just one leaf box)\n";
	cout << "  - Depth limit was: " << maxSubDivLevels << " (if this is the same as the ADF depth, then some detail was lost.  Considering redo-ing with a higher limit.)\n";

	//--------------------------------
	// Save ADF to file
	//--------------------------------

	// Get a useful filename

#if DEBUG_USE_SPHERE_INSTEAD_OF_STL
	strcpy(stlFilename, "debug-sphere");
#elif DEBUG_USE_BOX_INSTEAD_OF_STL
	strcpy(stlFilename, "debug-box");
#endif

	char* savedADFFilename =
		Utils::STL_to_ADF_filename(
			stlFilename,
			octreeNode::adf_distance_tolerance,
			maxSubDivLevels,
			using_sphere_tree);

	cout << "Saving to " << savedADFFilename << "..";
	saveADF(savedADFFilename);
	cout << "Done!" << endl;

	// TODO - turn both checks below into cmd line switches

	//--------------------------------
	// CHECK BALANCE
	//--------------------------------
	// (Steve) disabling this for now.  Balancing works pretty well, and this takes a bit of time to check
	// TODO - turns this into a command line switch..or maybe have a mode where we just check balance and nothing else!
	if(FALSE) {
		cout << "Making sure the tree is BALANCED...";
		if( TRUE == ADF->subtree_is_balanced() ) {
			cout << "Tree is balanced.  Sweeet!!\n";
		}
		else {
			cout << "\n\t** WARNING: The generated ADF octree is not properly balanced.  If you are using it with COONS interpolation, you will not get the desired results.\n\n";
		}
	}

	//--------------------------------
	// CHECK W of corner points
	//--------------------------------
	if(FALSE) {
		cout << "Making sure the corners have w=1...";
		if( TRUE == ADF->check_corner_pts() ) {
			cout << "looks good.  Sweeet!!\n";
		}
		else {
			cout << "\n\t** WARNING: Some W of the ADF node vertices is not 1.0f.  This is probably undesirable.\n\n";
		}
	}

	//test_ADF_with_random_points(error);

	//displayADF(&argc, argv);
}

class GlobalSTLFile;

void run_unit_tests() {
	ConvexUmbrellaTriMeshSdfAlgorithm::Test_all();

	testMeshTriangleEdgePOT();
	testMeshTriangleVertexPOT();
	testMeshTriangleVertexBug();
	testMeshTriangleFaceBug();
	testMeshTriangleClosestPoint();

	testMeshTriangleShouldGetEdgePOT();

	Utils::Test_all();
	// AxisAlignedBox::run_unit_tests();

}

void RayTracer(octreeNode *ADF) 
{
	printf("inside raytracer\n");
	FILE* file;
	if(file=fopen("c:\\ADFCreator\\raytrace.m","w"))
	{
		fprintf(file,"close all;\n hold on;\n");
		fclose(file);
	}
	else printf("Couldn't open raytracer file\n");
	Point eye;
	Point *part;
	part = new Point;
	Vector ray;
	octreeNode *temp = new octreeNode;
	eye.Set(0,0,20,0);
	ray.Set(0,0,-1,0);
	(*part)=eye;
	double conv=3.12159/180;
	double stepsize=.1;
	double i,j;
	int collision=0;
	for(i=-45;i<45;i+=.5)
	{
		for(j=-45;j<45;j+=.5)
		{
			(*part).Set(eye(0),eye(1),eye(2),0);
			ray.Set(sin(conv*j),sin(conv*i),-1.0*cos(conv*i)*cos(conv*j),0);  //particle direction
			while((*part)(2)>-1.5 && collision==0)
			{
				(*part)=(*part)+(stepsize*ray);
				if((*part)(0)<1 && (*part)(0)>-1 && (*part)(1)<1 && (*part)(1)>-1 && (*part)(2)<1 && (*part)(2)>-1)
				{
					temp = findNode(ADF, part);
					temp->vtxArray[0][0][0]->v->Display();
					if(temp->inOnOut==IN)
					{
						collision=1;
					}
					printf("I should collide in here\n");
					part->Display();
					printf("IN=%d ON=%d Out=%d but InOnOut=%d\n",IN,ON,OUT,temp->inOnOut);
					temp->vtxArray[0][0][0]->v->Display();
				}
			}
			if(collision==1)
			{
				part->Display();
				if(file=fopen("c:\\ADFCreator\\raytrace.m","a"))
				{
					fprintf(file,"plot3(%f,%f,%f,'r*');\n",(*part)(0),(*part)(1),(*part)(2));
					fclose(file);
				}
				else printf("Still can't open raytracer file!\n");
			}
			collision=0;
		}
		printf("i=%f\n",i);
	}
}



/*
Leaf Node Predicate

  Determines whether the octree node, *node, is a leaf node.
*/

bool isLeafNode (octreeNode *node) 
{
	return ((node->subnodeArray)[0][0][0] == NULL);
}



/*
Are Children Subdivided Predicate

  Determines whether the children of octree node, *node, are subdivided.
*/

bool areChildrenSub (octreeNode *node) 
{
	int i, j, k;
	
	for (i = 0; i < 2; i += 1) 
	{
		for (j = 0; j < 2; j += 1) 
		{
			for (k = 0; k < 2; k += 1) 
			{
				if (!(isLeafNode((node->subnodeArray)[i][j][k]))) 
				{
					return true;
				}
			}
		}
	}
	
	return false;
}



/*
Is Node Containing Point In, On, or Out "Predicate"

  Determines whether point, *pt, lies in a node of octree, *node, 
  which is in, on, or out.
*/

int isInOnOut (octreeNode *node, Point *pt) 
{
	int inOnOut = node->inOnOut;
	
	if (inOnOut == IN || inOnOut == OUT || isLeafNode(node)) {
		return inOnOut;
	}
	else {
		return isInOnOut(whichNode(node, pt), pt);
	}
	
}



/*
Find Node at Next Subdivision Level Which Contains Point

  Determines which octree node contains the point, *pt, by 
  comparing point coordinates to center of parent the parent
  octree node, *node.
*/

octreeNode *whichNode (octreeNode *node, Point *pt) 
{
	Point p0 = *((node->vtxArray[0][0][0])->v);
	double halfLength = node->m_edgeLength / 2;
	bool lessX, lessY, lessZ;
	
	lessX = (p0[0] + halfLength) > (*pt)[0];
	lessY = (p0[1] + halfLength) > (*pt)[1];
	lessZ = (p0[2] + halfLength) > (*pt)[2];
	
	if (lessX) {
		if (lessY) {
			if (lessZ) {
				return (node->subnodeArray)[0][0][0];
			}
			else {
				return (node->subnodeArray)[0][0][1];
			}
		}
		else {
			if (lessZ) {
				return (node->subnodeArray)[0][1][0];
			}
			else {
				return (node->subnodeArray)[0][1][1];
			}
		}
	}
	else {
		if (lessY) {
			if (lessZ) {
				return (node->subnodeArray)[1][0][0];
			}
			else {
				return (node->subnodeArray)[1][0][1];
			}
		}
		else {
			if (lessZ) {
				return (node->subnodeArray)[1][1][0];
			}
			else {
				return (node->subnodeArray)[1][1][1];
			}
		}
	}
}



/*
Find Smallest Node Which Contains Point

  Recursively determines which octree node, *octreeNode, contains the
  point, *p.
*/
octreeNode *findNode (octreeNode *octreeNode, Point *p) {
	
	if (isLeafNode(octreeNode)) {
		return octreeNode;
	}
	else {
		return findNode(whichNode(octreeNode, p), p);
	}
}



/*	
Trilinear Interpolation

  Uses trilinear interpolation to find the approximate distance value at a
  given point, *pt, from the distance values at the corners of the octree 
  node, *node.
  
*/

double findInterpDist(const octreeNode *node, const Point *pt) {
	Point normPt;
	double x, y, z;
	double dist;
	
	normPt = *pt - *(((node->vtxArray)[0][0][0])->v);
	normPt /= node->m_edgeLength;
	
	x = (double) normPt[0];
	y = (double) normPt[1];
	z = (double) normPt[2];
	
	dist = (
		((node->vtxArray)[0][0][0])->dist * (1 - x) * (1 - y) * (1 - z) +
		((node->vtxArray)[1][0][0])->dist * x * (1 - y) * (1 - z) +
		((node->vtxArray)[0][1][0])->dist * (1 - x) * y * (1 - z) +
		((node->vtxArray)[0][0][1])->dist * (1 - x) * (1 - y) * z +
		((node->vtxArray)[1][0][1])->dist * x * (1 - y) * z +
		((node->vtxArray)[0][1][1])->dist * (1 - x) * y * z +
		((node->vtxArray)[1][1][0])->dist * x * y * (1 - z) +
		((node->vtxArray)[1][1][1])->dist * x * y * z
		);
	
	return dist;
}



/*
  Classify a Vertex as In, On, or Out Function

  For a given octree vertex, *vtx, classify it as in, on, or out.

  (Steve) This uses the convention that a postivie distance means the vertex
  is INSIDE.

*/

int classifyVertex (octreeVtx *vtx) {
	double dist = vtx->dist;
	
	if (dist == 0) {
		return ON;
	}
	else if (dist < 0) {
		return OUT;
	}
	else {
		return IN;
	}
}



/*
Classify Octree Node as In, On, or Out Function
*/

int classifyNode (octreeNode *node)
{
	int i, j, k;
	int inOnOut;
	int nextTest;
	
	// Determine if First Vertex is In, On, or Out
	inOnOut = classifyVertex((node->vtxArray)[0][0][0]);

	// (Steve) If any vertex is on, then the whole cube is on.
	if(inOnOut == ON)
	{
		node->inOnOut = ON;
		return node->inOnOut;
	}
	
	// Compare values for the Remaining Vertices
	// (Steve) Shouldn't we start from i=0?  Skipping to i=1 ignores a buncha verts.. like (0,1,1)
	// for (i = 1; i < 2; i += 1) {
	for(i = 0; i < 2; i++) {
		for (j = 0; j < 2; j += 1) {
			for (k = 0; k < 2; k += 1)
			{
				nextTest = classifyVertex((node->vtxArray)[i][j][k]);
								
				if (nextTest != inOnOut)
				{
					// This is different from the corner vertex's classification
					// So, in any case, the cube must be on if it's vertices very in class
					node->inOnOut = ON;
					return node->inOnOut;
				}
			}
		}
	}
	
	// All the verts have the same class.  So use that as the node's.
	node->inOnOut = inOnOut;
	return node->inOnOut;
}



/*
Classify Octree Nodes as In, On, or Out Function

  For a given octree, *node, classify each node as in, on, or out starting with leaf
  nodes and propagating upwards.
*/

int classifyOctree (octreeNode *node) {
	int i, j, k;
	int inOnOut;
	int nextTest;
	
	if (isLeafNode(node)) {
		return classifyNode(node);
	}
	else {
		// Determine if First Node is In, On, or Out
		inOnOut = classifyOctree((node->subnodeArray)[0][0][0]);
		
		if (inOnOut == ON) {
			node->inOnOut = ON;
			
			return inOnOut;
		}
		
		for (i = 1; i < 2; i += 1) {
			for (j = 0; j < 2; j += 1) {
				for (k = 0; k < 2; k += 1) {
					nextTest = classifyOctree((node->subnodeArray)[i][j][k]);
					
					if (nextTest != inOnOut) {
						node->inOnOut = ON;
						
						return inOnOut;
					}
				}
			}
		}
		
		node->inOnOut = inOnOut;
		
		return inOnOut;
	}
}

/*
Construct a Cubical Bounding Box for Given STL

  Determine the maximum x, y, and z distance for a given STL and return the
  maximum cubical counding box for the points with the vertex distances
  stored.
*/

octreeNode *createBoundingBox (CLEDSGeometry *STL) {
	CListIter<CLEDSliteFace *, UINT_8> firstCListIterIntFace;
	CLEDSliteFace *currentFace;
	CLEDSliteFaceIter firstFaceIter;
	CLEDSliteVertex *currentFace1stVtx, *currentFaceVtx;
	Point currentFacePt;
	Point *pt;
	octreeVtx *vtx;
	
	octreeNode *boundingBox = new octreeNode;
	
	double maxXDist = 0;
	double maxYDist = 0;
	double maxZDist = 0;
	
	double maxDist = 0;
	
	float x = 0;
	float y = 0;
	float z = 0;
	
	float xMin = INFINITY;
	float xMax = -INFINITY;
	float yMin = INFINITY;
	float yMax = -INFINITY;
	float zMin = INFINITY;
	float zMax = -INFINITY;
	
	// Initialize First Face List Iterator
	firstCListIterIntFace.Init(STL->GetCListFaces());
	
	// Pull Out the First Face
	currentFace = firstCListIterIntFace.PeekFirst();
	
	while (currentFace) { // While the Face is Valid
		// Initialize First Face Iterator
		firstFaceIter.Init(currentFace);
		
		// Get First Vertex in Face
		currentFaceVtx = currentFace1stVtx = (CLEDSVertex *) firstFaceIter.PeekFirstCVertex();
		
		do {
			// Pull the Coordinate Point Information Out of the Vertex
			currentFacePt = currentFaceVtx->GetPoint();
			
			x = currentFacePt[0];
			y = currentFacePt[1];
			z = currentFacePt[2];
			
			if (x < xMin) {
				xMin = x;
			}
			
			if (x > xMax) {
				xMax = x;
			}
			
			if (y < yMin) {
				yMin = y;
			}
			
			if (y > yMax) {
				yMax = y;
			}
			
			if (z < zMin) {
				zMin = z;
			}
			
			if (z > zMax) {
				zMax = z;
			}
			
			// Get Next Vertex in Face
			currentFaceVtx = (CLEDSVertex *) firstFaceIter.PeekNextCVertex();
			
		} while (currentFaceVtx != currentFace1stVtx); // Until You're Back to the Start
		
		// Get Next Face
		currentFace = firstCListIterIntFace.PeekNext();
	}
	
	maxXDist = xMax - xMin;
	maxYDist = yMax - yMin;
	maxZDist = zMax - zMin;
	
	// Find the max distance
	// since the root node needs to be a square, so we can't
	// have a different length per dimension
	maxDist = max (maxXDist, maxYDist, maxZDist);

	// (Steve) Buffer maxDist a little
	// We only need to change maxDist, since the root node
	// corners are calculated based on maxDist and the model's center.
	// So we don't need to change the x/y/zMin's.
	maxDist *= 1.1;	// 10%

	//---------------------------------
	//	Now that we have the bounds of the model,
	//	create the root node with distance fields
	//---------------------------------
	
	// Calculate the 000-corner of the node such that its centered
	// on the object
	x = xMin + (maxXDist) / 2 - maxDist / 2;
	y = yMin + (maxYDist) / 2 - maxDist / 2;
	z = zMin + (maxZDist) / 2 - maxDist / 2;

	// (Steve) Make a wrapper for this part
	JitteringStlSdfAble model(
		STL, 
		JITTER_BOX_HALF_LENGTH * 2.0,
		new PointJitterer(JITTER_BOX_HALF_LENGTH),
		sdf_algo_factory->create()
		);
	
	for (int i = 0; i < 2; i += 1) {
		for (int j = 0; j < 2; j += 1) {
			for (int k = 0; k < 2; k += 1) {
				pt = new Point;
				pt->Set(x + i * maxDist, y + j * maxDist, z + k * maxDist);

				// (Steve) Old code
				// vtx = minDistSphere(pt);
				//vtx = minDistSTL(STL, pt);				
				
				// (Steve) get the min distance
				SdfValue sdf = model.calc_sdf_from_pt(*pt);

				// (Steve) now build a new octreeVtx object
				vtx = new octreeVtx;
				vtx->dist = sdf.getSignedDistance();
				vtx->v = pt;
				vtx->Pc = new Point(sdf.closest_point);
				vtx->grad = new Vector(sdf.gradient);

				boundingBox->vtxArray[i][j][k] = vtx;
			}
		}
	}
	
	boundingBox->m_edgeLength = maxDist;
	
	return boundingBox;
}

// (Steve)
octreeNode *create_arbitrary_bounding_box(const Point &origin,
										  double edge_length, SDF::ISDF *model) {
	octreeVtx *vtx;
	octreeNode *box = new octreeNode;

	box->m_edgeLength = edge_length;
	
	for (int i = 0; i < 2; i += 1)
	for (int j = 0; j < 2; j += 1)
	for (int k = 0; k < 2; k += 1)
	{
		vtx = new octreeVtx;
		vtx->v = new Point(
			origin(0) + i * edge_length, 
			origin(1) + j * edge_length, 
			origin(2) + k * edge_length,
			1.0);

		SDF::SdfValue sdf = model->calc_sdf_from_pt(*(vtx->v));
		vtx->dist = sdf.getSignedDistance();
		vtx->Pc = new Point(sdf.closest_point);
		vtx->grad = new Vector(sdf.gradient);

		box->vtxArray[i][j][k] = vtx;
	}
	
	return box;
}

// Just for fun
int printSpinner(int step) {
	cout << "\r";	// clear previous spin character
	switch(step) {
		case 0: cout << "|"; break;
		case 1: cout << "/"; break;
		case 2: cout << "-"; break;
		case 3:	/* fall thru */
		default: cout << "\\"; break;
	}

	return (step+1) % 4;
}


/*
Adaptively Sampled Distance Field Octree Top-Down Construction Function

  Construct an ADF using a top-down method from the given octreeNode, *node,
  given an error tolerance, errorLimit.
  
	STL - the geometry
	node - the octree node
	errorLimit - the error limit within cells - see MeetTolerance
	maxLevels - maximum number of branches you can go on
*/
	
void buildTopDownADF (const ISDF &model, octreeNode *node, double errorLimit, int maxLevels)
{
	static int spinner = 0;
	spinner = printSpinner(spinner);

	// add to the running count of nodes
	// (Steve) undesirable way of keeping track...makes things very hard to extend.
	// We no longer use this when saving the ADF - I use count_offspring instead.
	// Slower, but less side-effect-ish.
	// totalNumOfNodes++;

	octreeVtx *funcDist[3][3][3];	// (Steve) These are the distances for the octnode corners, with the midpoints as well

	if( maxLevels > 0 && node->isLeafNode() )
	{
		bool should_subdivide = shouldSubNodeFrisken(model, node, funcDist, errorLimit);
		
		// If outside of error tolerance limits, recursively subdivide.
		if(should_subdivide) {
			//cout << ":";	// stands for subdivision

			// Subdivide it, passing the computed vertices
			// (Steve) THe two lines below do the subdividing - one maintains balance, one doesn't
			// You must NOT use the balancing one, then use the other one.  Your balance
			// will not be maintained.
			// TODO - it is probably better to take these subdivide methods out of
			// the octree class into a Strategy pattern
#if MAINTAIN_BALANCED_ADF
			node->subdivide_for_balanced_ADF(funcDist, model);
#else
			node->subdivide_for_ADF(funcDist, model);
#endif
			
		}
		else {
			// TODO - keep track of this in a global var
			//cout << "!"; // stands for a leaf where you simply meet all the tolerances
		}
	}
	else {
		// TODO - keep track of this in a global var
		//cout << "?"; // stands for a leaf where you have maxed out on levels
	}
	
	// If there are child nodes, traverse them
	if( !node->isLeafNode() )
	{
		// Now recurse on each child, building the ADF
		for(int i = 0; i < 2; i++)
			for(int j = 0; j < 2; j++)
				for(int k = 0; k < 2; k++)
				{
					octreeNode *child = node->subnodeArray[i][j][k];
					buildTopDownADF(model, child, errorLimit, maxLevels - 1);
				}
	}

	classifyNode(node);
}



/**
* Returns whether or not the cube contains this point
*/
bool cubeContainsPoint(const Point *cubeOrigin, double cubeLength , const Point *point)
{
	Point tempPoint;
	tempPoint = *point - *cubeOrigin;
	
	return (
		( 0 <= tempPoint[0] && tempPoint[0] <= cubeLength ) &&
		( 0 <= tempPoint[1] && tempPoint[1] <= cubeLength ) &&
		( 0 <= tempPoint[2] && tempPoint[2] <= cubeLength ));
	
}


/*
Subdivide ADF Node Predicate Function (Frisken et al Method)

  Determine whether an ADF node should be subdivided using the nineteen points
  described in Frisken et al to check the error.
*/


bool shouldSubNodeFrisken (const SDF::ISDF &model, octreeNode *node, octreeVtx *funcDist[3][3][3], double errorLimit)
{

	// OPTIMIZATION - if we can determine that this node
	// contains no surface, then we can ignore it.

#if USE_CONTAINS_SURFACE_OPTIMIZATION
	if( !node->contains_surface() )
		return FALSE;
#endif

	// Use the Frisken et. al. method of checking test pts.
	
	// Have the node calculate the child-corners
	octreeNode::calculate_child_corner_verts(node, model, funcDist);

	// Then, see if those are good enough
	bool node_is_good_enough = octreeNode::all_child_corner_verts_meet_tolerance(funcDist, node);

	return !node_is_good_enough;	// If it's good enough, return false (no need to subdivide further)
}


/**
* Returns true if the actual distance calculated minus the interpolated distance 
* is within the error limit. 
*/
bool MeetsTolerance(double actualDistance , double interpDist , double errorLimit)
{
	double deviation = abs_d( abs_d(actualDistance) - abs_d(interpDist) );
	

	return( /*(sign(actualDistance) == sign(interpDist))  && */ 
		(deviation < (errorLimit * abs_d(actualDistance))  )
		);
}

octreeVtx *sdf_value_to_octreeVtx(const SDF::SdfValue &sdf, Point *from) {
	octreeVtx *vtx = new octreeVtx;
	vtx->dist = sdf.getSignedDistance();
	vtx->v = from;
	vtx->Pc = new Point(sdf.closest_point);

	return vtx;
}

/*
Subdivide ADF Node Predicate Function (Random Displacement)

  Determine whether an ADF node should be subdivided using a grid of randomly
  displaced points to check the error.
*/

bool shouldSubNodeRand (const SDF::ISDF &model, octreeNode *node, octreeVtx *funcDist[3][3][3], double errorLimit) {
	Point *p0 = ((node->vtxArray)[0][0][0])->v;
	double halfLength = (node->m_edgeLength)/2;
	
	int i, j, k;
	int l;
	
	Point *pt;
	
	double interpDist;
	double error;
	
	bool subdivide = false;
	
	// Initialize distance array to NULL.
	for (i = 0; i < 3; i += 1) {
		for (j = 0; j < 3; j += 1) {
			for (k = 0; k < 3; k += 1) {
				funcDist[i][j][k] = NULL;
			}
		}
	}
	
	// Initialize distance array for known values.
	for (i = 0; i < 2; i += 1) {
		for (j = 0; j < 2; j += 1) {
			for (k = 0; k < 2; k += 1) {
				funcDist[2 * i][2 * j][2 * k] = (node->vtxArray)[i][j][k];
			}
		}
	}
	
	// Check error or randomly displaced points with interpolated distance.
	for (i = 0; i < 3; i += 1) {
		for (j = 0; j < 3; j += 1) {
			for (k = 0; k < 3; k += 1) {
				if (funcDist[i][j][k] == NULL) {
					pt = new Point;
					
					pt->Set(i, j, k);
					
					for (l = 0; l < 3; l += 1) {
						(*pt)[l] += ((rand() - (RAND_MAX / 2)) / RAND_MAX);
					}
					
					*pt *= halfLength;
					*pt += *p0;
					
					// (Steve) get the min distance
					// (Steve) Use the model isntead of minDistSTL
					SdfValue sdf = model.calc_sdf_from_pt(*pt);
					funcDist[i][j][k] = sdf_value_to_octreeVtx(sdf, pt);;
					
					interpDist = findInterpDist(node, pt);
					error = abs_d((funcDist[i][j][k])->dist) - abs_d(interpDist);
					
					if ((error > errorLimit) || (error < -errorLimit)) {
						subdivide = true;
					}
				}
			}
		}
	}
	
	
	if (subdivide) {
		// Fill in remaining values for distance array
		for (i = 0; i < 3; i += 1) {
			for (j = 0; j < 3; j += 1) {
				for (k = 0; k < 3; k += 1) {
					if (funcDist[i][j][k] == NULL) {
						pt = new Point;
						
						pt->Set(i, j, k);
						*pt *= halfLength;
						*pt += *p0;
						
						// (Steve) get the min distance
						// (Steve) Use the model isntead of minDistSTL
						SdfValue sdf = model.calc_sdf_from_pt(*pt);
						funcDist[i][j][k] = sdf_value_to_octreeVtx(sdf, pt);;
					}
				}
			}
		}
	}
	return subdivide;
}



/*
Implicit Distance of Sphere Function

  Find the distance from a sphere to point *pt.
*/
octreeVtx *minDistSphere (Point *pt) {
	octreeVtx *vtx = new octreeVtx;
	Point *Pc = new Point;
	
	(*pt)[3] = 0;
	vtx->dist = DEBUG_SPHERE_RADIUS - sqrt (pt->Dot(*pt));
	
	*Pc = *pt;
	*Pc = ((*Pc/(Pc->Magnitude())) * vtx->dist) + *Pc;
	
	vtx->Pc = Pc;
	vtx->v = pt;
	
	return vtx;
}

/* (Steve) 2005-12-10 Unused
octreeVtx *min_dist_cube( Point *pt )
{
	AxisAlignedBox box( Point(-1, -1, -1), Vector(2,2,2));
	octreeVtx *vtx = new octreeVtx;
	Point *Pc = new Point;

	vtx->dist = box.min_distance_from( *pt, *Pc );
	vtx->Pc = Pc;
	vtx->v = pt;

	return vtx;
}
*/

double norm (Vector pt) {
	// Exists to return the magnitude of the difference of two points
	return pt.Magnitude();
}



int sign(float x)
{
	if(x<0) return(-1);
	else return(1);
}

int maxInd (Vector l) {
	// Returns the index of the vector component with the largest magnitude
	float value = abs_d(l[0]); // Assume its x
	int index = 0;
	
	if (value < abs_d(l[1])) { // But maybe its y
		value = abs_d(l[1]);
		index = 1;
	}
	
	if (value < abs_d(l[2])) { // Or maybe its z
		index = 2;
	}
	
	return index;
}



Vector proj (Vector A, Vector B) {
	// Projection of A onto B
	return (A.Dot(B) / B.Dot(B)) * B;
}



float Dot (Vector A, Vector B) {
	return (A[0] * B[0] + A[1] * B[1] + A[2] * B[2]);
}

/*
Returns the signed, shortest distance from from_pt to the infinite plane specified
by norm and a point on the plane, on_plane.
*/
double dist_to_plane( const Vector &norm, const Point &on_plane, const Point &from_pt )
{
	const Point p = on_plane;
	const double d = -p(0)*norm(0) - p(1)*norm(1) - p(2)*norm(2);

	const double dist =
		from_pt(0)*norm(0)
		+ from_pt(1)*norm(1)
		+ from_pt(2)*norm(2)
		+ d;

	return dist;
}

bool point_behind_plane( const Vector &norm, const Point &on_plane, const Point &test_pt)
{
	return dist_to_plane(norm, on_plane, test_pt) < 0.0;
}

/*
Returns true if the triangle A (QRS) is behind tri B (TUV).
tri_norm should be the normal of triangle B.
IMPORTANT: This assumes tri A and B do not intersect, except maybe share a point or an edge.

  TODO: I guess we don't need UV..just T.
*/
bool tri_A_behind_B( const Point &Q, const Point &R, const Point &S,
					const Point &T, const Point &U, const Point &V,
					const Vector &tri_norm)
{
	// First, calculate the average pt of tri A.  We'll use this point to
	// determine tri A's relative position to B.

	Point avgA = (Q + R + S) * (1/3.0);
	bool behind = point_behind_plane(tri_norm, T, avgA);
	return behind;
}

/**
*	Find the closest point on a triangle to a test point and the minimum signed distance to that point
* Make sure you call freeOctreeVtx when finished with this function to free memory
*/
octreeVtx *minDistTri (Point P1, Point P2, Point P3, const Point &testPoint, Vector &tri_norm) {

	// (Steve) Greg sets w to 0.0 a LOT..shouldn't it be set to 1.0?  Or just leave it alone?

	Point *Pc = new Point;					// The closest point

	//(Steve) Removed w=0.0 set
	Pc->Set(0,0,0);
	Point *P0 = new Point;

	//(Steve) Removed w=0.0 set
	P0->Set( testPoint(0),testPoint(1),testPoint(2));

	Vector C;
	Vector C1, C2, C3;
	Vector V1, V2, V3;
	Point P;
	double s = 0;
	int i;
	int temp=0;
	int temp2=0;
	
	Vector L1, L2, L3;
	
	L1=P2-P1;   //edge 1
	L2=P3-P2;   //edge 2
	L3=P1-P3;   //edge 3
	
	C = L1 * L2;							// The surface normal
	
	P = *P0 + proj((P1 - *P0), C);			// Projection onto the plane of the triangle
	
	C1 = L1 * C;							// The edge normals
	C2 = L2 * C;
	C3 = L3 * C;
	
	V1 = P1 - P;							// Vectors from the plane projection to the triangle vertices
	V2 = P2 - P;							// Vectors from the plane projection to the triangle vertices
	V3 = P3 - P;							// Vectors from the plane projection to the triangle vertices
	
	if (Dot(V1, C1) < 0) {					// Projection is outside triangle and closest to edge #1
		*Pc = P + proj(V1, C1);				// Project onto the edge
		i = maxInd(L1);						// This prevents a division be zero error in the next line
		s = ((*Pc)[i] - P1[i])/L1[i];		// To be used to check if projection is between the vertices
		temp2=1;
	}
	else if (Dot(V2, C2) < 0) {				// Projection is outside triangle and closest to edge #2
		*Pc = P + proj(V2, C2);				// Project onto the edge
		i = maxInd(L2);
		s = ((*Pc)[i] - P2[i])/L2[i];		// To be used to check if projection is between the vertices
		temp2=2;
	}
	else if (Dot(V3, C3) < 0) {				// Projection is outside triangle and closest to edge #3
		*Pc = P + proj(V3, C3);				// Project onto the edge
		i = maxInd(L3);
		s = ((*Pc)[i] - P3[i])/L3[i];		// To be used to check if projection is between the vertices
		temp2=3;
	}
	else {									// Projection is inside the triangle boundary
		*Pc = P;
		(*Pc).Set((*Pc)(0),(*Pc)(1),(*Pc)(2),0);
		temp2=4;
	}
	
	// s comes from the equation for a line: x1+s(x2-x1)=x.
	// If s>1 or s<0 then the projection onto the line is out of bounds,
	// therefore, pt is closest to one of the vertices
	
	if (s<0 || s>1) {
		*Pc = P1;							// Figure out which vertex pt is closest to
		if (norm(*Pc-P) > norm(P2-P)) {
			*Pc = P2;
		}
		
		if (norm(*Pc-P) > norm(P3-P)) {
			*Pc = P3;
		}
	}
	
	octreeVtx *V = new octreeVtx;
	V->v = P0;
	V->Pc = Pc;

	// Steven - OK, the sign calculated here causes some artifacts with
	// models containing right-angle edges.
	//
	// V->dist = norm(*Pc - *P0)*sign(Dot((*Pc - *P0),C));	// Greg's way

	// Steven's modified way:
	// Use the surface normal instead to figure out the sign
	const Vector testpt_to_surfpt = *Pc - *P0;
	const double testpt_to_surfpt_dist = testpt_to_surfpt.Magnitude();
	int dist_sign = 0;
	const double v_dot_normal = Dot(testpt_to_surfpt, C);

	if( v_dot_normal < 0.0 ) dist_sign = -1;
	else if( v_dot_normal > 0.0 ) dist_sign = 1;
	else dist_sign = 0;

	Vector test_to_surf_dir = testpt_to_surfpt;
	test_to_surf_dir.Normalize();
	Vector surf_norm = C;
	surf_norm.Normalize();

	// Different attempts..
	//abs_dot = Utils::abs_d( Dot(test_to_surf_dir, surf_norm) );
	//abs_dist_to_faceplane = Utils::abs_d(dist_to_plane(surf_norm, P1, *P0 ));

	tri_norm = surf_norm;

	/* DEBUG 
	if( dist_sign == 1.0 )
	{
		const double v_dist = testpt_to_surfpt.Magnitude();
		if( 55.0 < v_dist && v_dist < 56.0 )
		{
			printf("breakpt");
		}
	}
*/
	//printf( "%f\n", dist_sign );



	if( dist_sign == 0 && testpt_to_surfpt_dist != 0.0 )
	{
		// We got some distance, but the test point is on the face plane/
		// This distance value cannot be trusted. 
		// Return NULL, for invalid.
		return NULL;
	}
	else {
		V->dist = testpt_to_surfpt.Magnitude() * dist_sign;
	}

	return V;
}


octreeVtx *min_dist_to_origin_sphere( Point &pt)
{
	return minDistSphere(&pt);
}

/*
Find the closest point on a solid to a test point and the minimum signed distance to that point.
The solid model is defined by an STL file, the name of which was given as an argument to the program
*/

bool DEBUG_TEST_POINT = FALSE;

//--------------------------------
// (Steve) This is the original function made by Greg, modified by me.
// I took this function and made min_distance_to_STL_using_a_trimesh_algorithm()
// so I can more easily experiment with various SDF algorithms.
//--------------------------------
// (Steve) We really should not be using this anymore..only the random point-colliding functions use this now
// None of the ADF creation code should use this.

octreeVtx *minDistSTL(CLEDSGeometry *STL, Point *pt)
{

#if DEBUG_USE_SPHERE_INSTEAD_OF_STL
	return min_dist_to_origin_sphere(*pt);
#elif DEBUG_USE_BOX_INSTEAD_OF_STL
	return min_dist_cube(pt);
#else

	// (Steve) Wrap the STL into wrapper
	JitteringStlSdfAble model(STL,
		JITTER_BOX_HALF_LENGTH * 2.0,
		new PointJitterer(JITTER_BOX_HALF_LENGTH),
		sdf_algo_factory->create());

	// Get the distance from that
	SdfValue sdf = model.calc_sdf_from_pt(*pt);
	return sdf_value_to_octreeVtx(sdf, pt);

#endif

}


/*
Clear a File Function
Clear the file, filename, and begin file for use with MATLAB.
*/

void clearFile (char* filename) {
	FILE *file;
	file = fopen(filename,"w");
	
	if(file) {
		fclose(file);
	}
	else {
		printf("ERROR: The file %s could not be opened.\n", filename);
	}
	
	file = fopen(filename,"a");
	
}



/*
Print the Octree Node to a File Function

  Print the necessary plot commands for representing the ADF nodes
  to a file, filename, for use with MATLAB.
*/
void printNode (octreeNode* node, char* filename) {
	FILE *file;
	file = fopen(filename, "a");
	
	numCubes += 1;
	
	if(file) {
		fprintf(file,"\nNew_Cube = %d;\n\n\n", numCubes);
		if(node->inOnOut==ON)
		{
			fprintf(file,"c='r';\n");
		}
		else if(node->inOnOut==OUT)
			fprintf(file,"c='b';\n");
		else fprintf(file,"c='g';\n");
		
		/*  The x-aligned edges  */
		fprintf(file,"x=[%f %f];\n",(*(node->vtxArray[0][0][0])->v)[0],(*(node->vtxArray[1][0][0])->v)[0]);
		fprintf(file,"y=[%f %f];\n",(*(node->vtxArray[0][0][0])->v)[1],(*(node->vtxArray[1][0][0])->v)[1]);
		fprintf(file,"z=[%f %f];\n",(*(node->vtxArray[0][0][0])->v)[2],(*(node->vtxArray[1][0][0])->v)[2]);
		fprintf(file,"plot3(x,y,z,c)\n");
		
		fprintf(file,"x=[%f %f];\n",(*(node->vtxArray[0][1][0])->v)[0],(*(node->vtxArray[1][1][0])->v)[0]);
		fprintf(file,"y=[%f %f];\n",(*(node->vtxArray[0][1][0])->v)[1],(*(node->vtxArray[1][1][0])->v)[1]);
		fprintf(file,"z=[%f %f];\n",(*(node->vtxArray[0][1][0])->v)[2],(*(node->vtxArray[1][1][0])->v)[2]);
		fprintf(file,"plot3(x,y,z,c)\n");
		
		fprintf(file,"x=[%f %f];\n",(*(node->vtxArray[0][0][1])->v)[0],(*(node->vtxArray[1][0][1])->v)[0]);
		fprintf(file,"y=[%f %f];\n",(*(node->vtxArray[0][0][1])->v)[1],(*(node->vtxArray[1][0][1])->v)[1]);
		fprintf(file,"z=[%f %f];\n",(*(node->vtxArray[0][0][1])->v)[2],(*(node->vtxArray[1][0][1])->v)[2]);
		fprintf(file,"plot3(x,y,z,c)\n");
		
		fprintf(file,"x=[%f %f];\n",(*(node->vtxArray[0][1][1])->v)[0],(*(node->vtxArray[1][1][1])->v)[0]);
		fprintf(file,"y=[%f %f];\n",(*(node->vtxArray[0][1][1])->v)[1],(*(node->vtxArray[1][1][1])->v)[1]);
		fprintf(file,"z=[%f %f];\n",(*(node->vtxArray[0][1][1])->v)[2],(*(node->vtxArray[1][1][1])->v)[2]);
		fprintf(file,"plot3(x,y,z,c)\n");
		
		/*  The y-aligned edges */
		fprintf(file,"x=[%f %f];\n",(*(node->vtxArray[0][0][0])->v)[0],(*(node->vtxArray[0][1][0])->v)[0]);
		fprintf(file,"y=[%f %f];\n",(*(node->vtxArray[0][0][0])->v)[1],(*(node->vtxArray[0][1][0])->v)[1]);
		fprintf(file,"z=[%f %f];\n",(*(node->vtxArray[0][0][0])->v)[2],(*(node->vtxArray[0][1][0])->v)[2]);
		fprintf(file,"plot3(x,y,z,c)\n");
		
		fprintf(file,"x=[%f %f];\n",(*(node->vtxArray[1][0][0])->v)[0],(*(node->vtxArray[1][1][0])->v)[0]);
		fprintf(file,"y=[%f %f];\n",(*(node->vtxArray[1][0][0])->v)[1],(*(node->vtxArray[1][1][0])->v)[1]);
		fprintf(file,"z=[%f %f];\n",(*(node->vtxArray[1][0][0])->v)[2],(*(node->vtxArray[1][1][0])->v)[2]);
		fprintf(file,"plot3(x,y,z,c)\n");
		
		fprintf(file,"x=[%f %f];\n",(*(node->vtxArray[0][0][1])->v)[0],(*(node->vtxArray[0][1][1])->v)[0]);
		fprintf(file,"y=[%f %f];\n",(*(node->vtxArray[0][0][1])->v)[1],(*(node->vtxArray[0][1][1])->v)[1]);
		fprintf(file,"z=[%f %f];\n",(*(node->vtxArray[0][0][1])->v)[2],(*(node->vtxArray[0][1][1])->v)[2]);
		fprintf(file,"plot3(x,y,z,c)\n");
		
		fprintf(file,"x=[%f %f];\n",(*(node->vtxArray[1][0][1])->v)[0],(*(node->vtxArray[1][1][1])->v)[0]);
		fprintf(file,"y=[%f %f];\n",(*(node->vtxArray[1][0][1])->v)[1],(*(node->vtxArray[1][1][1])->v)[1]);
		fprintf(file,"z=[%f %f];\n",(*(node->vtxArray[1][0][1])->v)[2],(*(node->vtxArray[1][1][1])->v)[2]);
		fprintf(file,"plot3(x,y,z,c)\n");
		
		/*  The z-aligned edges */
		fprintf(file,"x=[%f %f];\n",(*(node->vtxArray[0][0][0])->v)[0],(*(node->vtxArray[0][0][1])->v)[0]);
		fprintf(file,"y=[%f %f];\n",(*(node->vtxArray[0][0][0])->v)[1],(*(node->vtxArray[0][0][1])->v)[1]);
		fprintf(file,"z=[%f %f];\n",(*(node->vtxArray[0][0][0])->v)[2],(*(node->vtxArray[0][0][1])->v)[2]);
		fprintf(file,"plot3(x,y,z,c)\n");
		
		fprintf(file,"x=[%f %f];\n",(*(node->vtxArray[1][0][0])->v)[0],(*(node->vtxArray[1][0][1])->v)[0]);
		fprintf(file,"y=[%f %f];\n",(*(node->vtxArray[1][0][0])->v)[1],(*(node->vtxArray[1][0][1])->v)[1]);
		fprintf(file,"z=[%f %f];\n",(*(node->vtxArray[1][0][0])->v)[2],(*(node->vtxArray[1][0][1])->v)[2]);
		fprintf(file,"plot3(x,y,z,c)\n");
		
		fprintf(file,"x=[%f %f];\n",(*(node->vtxArray[0][1][0])->v)[0],(*(node->vtxArray[0][1][1])->v)[0]);
		fprintf(file,"y=[%f %f];\n",(*(node->vtxArray[0][1][0])->v)[1],(*(node->vtxArray[0][1][1])->v)[1]);
		fprintf(file,"z=[%f %f];\n",(*(node->vtxArray[0][1][0])->v)[2],(*(node->vtxArray[0][1][1])->v)[2]);
		fprintf(file,"plot3(x,y,z,c)\n");
		
		fprintf(file,"x=[%f %f];\n",(*(node->vtxArray[1][1][0])->v)[0],(*(node->vtxArray[1][1][1])->v)[0]);
		fprintf(file,"y=[%f %f];\n",(*(node->vtxArray[1][1][0])->v)[1],(*(node->vtxArray[1][1][1])->v)[1]);
		fprintf(file,"z=[%f %f];\n",(*(node->vtxArray[1][1][0])->v)[2],(*(node->vtxArray[1][1][1])->v)[2]);
		fprintf(file,"plot3(x,y,z,c)\n");
		fclose(file);
		
	}
	else {
		printf("ERROR: The file %s could not be opened.\n", filename);
	}
}




/*
Maximum Value for Double Values Function
*/
double max (double num1, double num2, double num3) {
	double maxValue = num1;
	
	if (maxValue < num2) {
		maxValue = num2;
	}
	
	if (maxValue < num3) {
		maxValue = num3;
	}
	
	return maxValue;
}


void renderNode (octreeNode *node)
{
	double halfLength = node->m_edgeLength / 2.0;
	Vector halfPoint;
	halfPoint.Set(halfLength, halfLength, halfLength);
	Point center = *((node->vtxArray[0][0][0])->v) + halfPoint;
	
	GLfloat red = 0;
	GLfloat blue = 0;
	GLfloat green = 0;
	
	// Determine Color for Node
	if (node->inOnOut == IN) {
		red = 1.0;
	}
	else if (node->inOnOut == ON) {
		green = 1.0;
	}
	else {
		blue = 1.0;
	}
	
	
	//if (node->inOnOut == WhatToShow) {
	glPushMatrix();
	
	// Set Current Color
	glColor3f (red, green, blue);
	
	// Translate to Cube Position
	glTranslatef ((GLfloat) center[0], (GLfloat) center[1], (GLfloat) center[2]);
	
	// Draw Cube
	glutWireCube((GLdouble) node->m_edgeLength);
	
	glPopMatrix();
	//}
}

void renderOctree (octreeNode *node)
{
	if( node == NULL )
		return;
	
	if( draw_internal_nodes || isLeafNode(node) )
	{
		renderNode(node);
	}

	for (int i = 0; i < 2; i += 1) {
		for (int j = 0; j < 2; j += 1) {
			for (int k = 0; k < 2; k += 1) {
				renderOctree (node->subnodeArray[i][j][k]);
			}
		}
	}
}

void renderCollider()
{
	glPushMatrix();
	{
		glTranslatef((GLfloat) collide[0], (GLfloat) collide[1], (GLfloat) collide[2]);
		glColor3f(1,0,0);
		glutSolidCube(.1);
	}
	glPopMatrix();
	
	glLineWidth(4.0);
	if(collide_TF==1) 
		glColor3f(1,0,1); // purple if it collides
	else 
		glColor3f(1,1,0);
	
	glBegin(GL_LINES);
	{
		glVertex3f((GLfloat) collide[0], (GLfloat) collide[1], (GLfloat) collide[2]);
		glVertex3f((GLfloat) collide_closest[0], (GLfloat) collide_closest[1], (GLfloat) collide_closest[2]);
	}
	glEnd();
	
	glLineWidth(1.0);
}

void renderADF () {
	renderOctree(ADF);
	renderCollider();
	if( SPHERES_ON )
		renderSphereTree();
	
	glutSwapBuffers();
}

void initGL (int width, int height) {
	collide[0]=collide[1]=collide[2]=0;
	collide_closest[0]=collide_closest[1]=collide_closest[2]=0;
	
	ActiveButton = 0;
	DepthCueOn = FALSE;
	Detail = 0;
	LeftButton = ROTATE;
	Scale  = 1.0;
	Scale2 = 0.0;		/* because add 1. to it in Display()	*/
	TimerInterrupted = FALSE;
	Xrot = Yrot = 0.;
	TransXYZ[0] = TransXYZ[1] = TransXYZ[2] = 0.;
	
	RotMatrix[0][1] = RotMatrix[0][2] = RotMatrix[0][3] = 0.;
	RotMatrix[1][0]                   = RotMatrix[1][2] = RotMatrix[1][3] = 0.;
	RotMatrix[2][0] = RotMatrix[2][1]                   = RotMatrix[2][3] = 0.;
	RotMatrix[3][0] = RotMatrix[3][1] = RotMatrix[3][3]                   = 0.;
	RotMatrix[0][0] = RotMatrix[1][1] = RotMatrix[2][2] = RotMatrix[3][3] = 1.;
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	
	gluPerspective(45, (GLfloat) width / (GLfloat) height, 0.1, 100);
	gluLookAt(0, 0, -10, 0, 0, 0, 0, 1, 0);
	
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}

void displayADF (int *argc, char *argv[]) {
	int width = 1024/2;
	int height = 768;
	
	glutInit(argc, argv);
	glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
	glutInitWindowSize(width, height);
	glutInitWindowPosition(0, 0);
	
	MainWindow = glutCreateWindow("ADF Grid");
	
	initGL(width, height);
	
	glShadeModel( GL_FLAT );
	glEnable(GL_LIGHTING);
	
	float Light_Ambient[]=  { 0.1f, 0.1f, 0.1f, 1.0f };
	float Light_Diffuse[]=  { 1.0f, 1.0f, 1.0f, 1.0f }; 
	float Light_Position[]= { 0.0f, 5.0f, -5.0f, 1.0f };
	float Light_Position2[]= { -5.0f, -5.0f, -5.0f, 0.0f };
	
	glLightfv(GL_LIGHT1, GL_POSITION, Light_Position);
	glLightfv(GL_LIGHT1, GL_AMBIENT,  Light_Ambient);
	glLightfv(GL_LIGHT1, GL_DIFFUSE,  Light_Diffuse); 
	glLightfv(GL_LIGHT2, GL_POSITION, Light_Position2);
	glLightfv(GL_LIGHT2, GL_AMBIENT,  Light_Ambient);
	glLightfv(GL_LIGHT2, GL_DIFFUSE,  Light_Diffuse); 
	glEnable (GL_LIGHT1);
	//  glEnable (GL_LIGHT2);
	
	glColorMaterial(GL_FRONT_AND_BACK,GL_AMBIENT_AND_DIFFUSE);
	glEnable(GL_COLOR_MATERIAL);
	
	
	
	//	GLfloat light_position[] = {0.0, 0.0, -10.0, 0.0};
	//	glLightfv (GL_LIGHT0, GL_SPOT_DIRECTION, light_position);
	
	//glutDisplayFunc(&renderADF);
	glutDisplayFunc(&drawSTL);
	glutMouseFunc( MouseButton );
	glutMotionFunc( MouseMotion );
	//glutReshapeFunc(&resizeView);
	glutKeyboardFunc(Keyboard);
	//glutMouseFunc(&mouseInput);
	
	glutMainLoop();
}

/**
** called when the mouse button transitions down or up:
**/

void
MouseButton
(
 int button,		/* GLUT_*_BUTTON			*/
 int state,		/* GLUT_UP or GLUT_DOWN			*/
 int x, int y		/* where mouse was when button hit	*/
 )
{
	int b;			/* LEFT, MIDDLE, or RIGHT		*/
	
	if( Debug )
		fprintf( stderr, "MouseButton: %d, %d, %d, %d\n", button, state, x, y );
	
	
	/* get the proper button bit mask:				*/
	
	switch( button )
	{
	case GLUT_LEFT_BUTTON:
		b = LEFT;		break;
		
	case GLUT_MIDDLE_BUTTON:
		b = MIDDLE;		break;
		
	case GLUT_RIGHT_BUTTON:
		b = RIGHT;		break;
		
	default:
		b = 0;
		fprintf( stderr, "Unknown mouse button: %d\n", button );
	}
	
	
	/* button down sets the bit, up clears the bit:			*/
	
	if( state == GLUT_DOWN )
	{
		Xmouse = x;
		Ymouse = y;
		ActiveButton |= b;		/* set the proper bit	*/
		Detail = 0;
		TimerInterrupted = TRUE;
	}
	else
	{
		ActiveButton &= ~b;		/* clear the proper bit	*/
		TimerInterrupted = FALSE;
	}
}



/**
** called when the mouse moves while a button is down:
**/

void
MouseMotion
(
 int x, int y		/* mouse coords				*/
 )
{
	int dx, dy;		/* change in mouse coordinates		*/
	
	if( Debug )
		fprintf( stderr, "MouseMotion: %d, %d\n", x, y );
	
	
	dx = x - Xmouse;		/* change in mouse coords	*/
	dy = y - Ymouse;
	
	if( ActiveButton & LEFT )
	{
		switch( LeftButton )
		{
		case ROTATE:
			Xrot -= ( ANGFACT*dy );
			Yrot += ( ANGFACT*dx );
			break;
			
		case SCALE:
			Scale += SCLFACT * (float) ( dx - dy );
			if( Scale < MINSCALE )
				Scale = MINSCALE;
			break;
		}
	}
	
	
	if( ActiveButton & MIDDLE )
	{
		Scale += SCLFACT * (float) ( dx - dy );
		
		/* keep object from turning inside-out or disappearing:	*/
		
		if( Scale < MINSCALE )
			Scale = MINSCALE;
	}
	
	Xmouse = x;			/* new current position		*/
	Ymouse = y;
	Detail = 0;			/* use lowest level of detail	*/
	
	glutSetWindow( MainWindow );
	glutPostRedisplay();
}


/*
Construct a Cubical Bounding Box for Given STL

  Determine the maximum x, y, and z distance for a given STL and return the
  maximum cubical counding box for the points with the vertex distances
  stored.
*/

void drawSTL () {
	CListIter<CLEDSliteFace *, UINT_8> firstCListIterIntFace;
	CLEDSliteFace *currentFace;
	CLEDSliteFaceIter firstFaceIter;
	CLEDSliteVertex *currentFace1stVtx, *currentFaceVtx;
	Point currentFacePt;
	/* erase the background:					*/
	
	glDrawBuffer( GL_BACK );
	glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
	glEnable( GL_DEPTH_TEST );
	
	/* place the object into the viewing volume:			*/
	
	glMatrixMode( GL_MODELVIEW );
	glLoadIdentity();
	gluLookAt( 0., 0., 3.,     0., 0., 0.,     0., 1., 0. );
	
	
	/* translate the object:					*/
	/* note the minus sign on the z value				*/
	/* this is to make the appearance of the glui z translate	*/
	/* widget more intuitively match the translate behavior		*/
	
	glTranslatef( TransXYZ[0], TransXYZ[1], -TransXYZ[2] );
	
	
	/* rotate the scene:						*/
	
	glRotatef( Yrot, 0., 1., 0. );
	glRotatef( Xrot, 1., 0., 0. );
	glMultMatrixf( (const GLfloat *) RotMatrix );
	
	
	/* scale the scene:						*/
	
	glScalef( Scale, Scale, Scale );
	float scale2 = 1. + Scale2;		/* because glui translation starts at 0. */
	if( scale2 < MINSCALE )
		scale2 = MINSCALE;
	glScalef( scale2, scale2, scale2 ); 
	
	float x = 0;
	float y = 0;
	float z = 0;
	
	if( MESH_ON )
	{
		// Initialize First Face List Iterator
		firstCListIterIntFace.Init(STL->GetCListFaces());
		
		// Pull Out the First Face
		currentFace = firstCListIterIntFace.PeekFirst();
		
		while (currentFace) { // While the Face is Valid
			// Initialize First Face Iterator
			firstFaceIter.Init(currentFace);
			
			// Get First Vertex in Face
			currentFaceVtx = currentFace1stVtx = (CLEDSVertex *) firstFaceIter.PeekFirstCVertex();
			
			glColor3f (0.0, 0.0, 1.0);
			
			glBegin(GL_TRIANGLES);
			
			do {
				// Pull the Coordinate Point Information Out of the Vertex
				currentFacePt = currentFaceVtx->GetPoint();
				
				x = currentFacePt[0];
				y = currentFacePt[1];
				z = currentFacePt[2];
				
				glVertex3f (x, y, z);
				
				// Get Next Vertex in Face
				currentFaceVtx = (CLEDSVertex *) firstFaceIter.PeekNextCVertex();
				
			} while (currentFaceVtx != currentFace1stVtx); // Until You're Back to the Start
			
			glEnd();
			
			
			// Get Next Face
			currentFace = firstCListIterIntFace.PeekNext();
		}
	}
	
	renderADF();
	
	glutSwapBuffers();
}


void saveADF (char* filename)
{
	clearFile (filename);
	
	FILE *ADFFile = fopen (filename, "a");
	
	if (ADFFile == NULL) {
		std::cerr << "Could Not Save ADF to File";
	}
	else {
		// (Steve) To make it more robust, I'm using the
		// count offspring function instead of a global var.
		// This will make life easier for extending the subdivision routines.
		// fprintf(ADFFile, "%d\n\n", totalNumOfNodes);
		fprintf(ADFFile, "%d\n\n", ADF->count_offspring()+1 );	// The +1, to count the root node itself.  count_offspring just returns number of OFFSPRING, not itself.

		writeNodes(ADFFile, ADF , 0 , 1 , 0);
	}

	fclose(ADFFile);
}


void printIndent(FILE* file,int indent)
{
	for(int i=0;i<indent;i++)
		fprintf(file,"\t");
}



// this is to keep track of indexing in a rather dumb manner
int stupidIndex = 2;

void writeNodes (FILE* file, octreeNode *node,int parentId,int thisId,int octant) {
	
	int i, j, k;
	Point currentVtx;
	// keeps track of the indices of this node's kids
	int childrenItr = stupidIndex;
	
	fprintf(file, "octree_node\n");
	
	fprintf(file,"id: %d\n",thisId); 
	fprintf(file,"pid: %d\n",parentId);
	
	
	for (i = 0; i < 2; i++) {
		for (j = 0; j < 2; j++) {
			for (k = 0; k < 2; k++) {
				if (isLeafNode(node)) 
					fprintf( file , "c%d%d%d: %d\n" , k , j , i , 0);
				else
				{
					// prints out indices of kids and actually adds their indices to a variable
					fprintf( file , "c%d%d%d: %d\n" , k , j , i , stupidIndex);
					stupidIndex++;
				}
			}
		}
	}
	
	currentVtx = *((node->vtxArray[0][0][0])->v);
	fprintf(file,"v: %f %f %f\n", currentVtx[0] , currentVtx[1], currentVtx[2]); 
	fprintf(file,"len: %f\n",node->m_edgeLength );
	
	
	/*
	 * SDFs
	 */
	for (i = 0; i < 2; i++) {
		for (j = 0; j < 2; j++) {
			for (k = 0; k < 2; k++) {
				fprintf( file , "d%d%d%d: %f\n" , k , j , i , node->vtxArray[k][j][i]->dist );
			}
		}
	}

#if OUTPUT_GRADIENT_VECTORS
	/*
	 * Gradient vectors
	 */
	if(true) {
		Vector *grad = NULL;
		for (i = 0; i < 2; i++) {
			for (j = 0; j < 2; j++) {
				for (k = 0; k < 2; k++) {
					grad = node->vtxArray[k][j][i]->grad;
					fprintf( file , "g%d%d%d: %f %f %f\n",
							k, j, i,
							(*grad)[0], (*grad)[1], (*grad)[2]);
				}
			}
		}
	}
#endif
	
	fprintf( file , "octant: %d\n\n" , octant);

	/*
	 * Recurse on children
	 */
	octant = 0;
	if (!isLeafNode(node)) {
		for(i = 0; i < 2; i++) 
			for(j = 0; j < 2; j++) 
				for(k = 0; k < 2; k++) {
					octant++;
					writeNodes(file, node->subnodeArray[k][j][i] , thisId , childrenItr,octant );
					childrenItr++;
				}
	}
}





/*
void loadADF (char* filename) {
	std::ifstream ADFFile;
	ADFFile.open(filename);
	
	if (!ADFFile) {
		cerr << "Could Not Load ADF from File";
	}
	else {
		ADF = readNodes (ADFFile, ADF);
	}
}


octreeNode *readNodes (ifstream file, octreeNode *node) {
	char fileInput[256];
	char* ptr;
	
	int i, j, k;
	
	octreeNode *newNode;
	octreeVtx *currentVtx;
	Point *currentPt;
	
	if (!file.eof()) {
		file.getline(fileInput, 256);
		node->inOnOut = atof(fileInput);
		
		file.getline (fileInput, 256);
		node->m_edgeLength = atof(fileInput);
		
		for (i = 0; i < 2; i += 1) {
			for (j = 0; j < 2; j += 1) {
				for (k = 0; k < 2; k += 1) {
					currentVtx = new octreeVtx;
					currentPt = new Point;
					
					file.getline (fileInput, 256);
					(*currentPt)[0] = atof(fileInput);
					
					ptr = strpbrk (fileInput, " ");
					(*currentPt)[1] = atof(ptr + 1);
					
					ptr = strpbrk (ptr, " ");
					(*currentPt)[2] = atof(ptr + 1);
					
					currentVtx->v = currentPt;
					
					currentPt = new Point;
					
					file.getline (fileInput, 256);
					(*currentPt)[0] = atof(fileInput);
					
					ptr = strpbrk (fileInput, " ");
					(*currentPt)[1] = atof(ptr + 1);
					
					ptr = strpbrk (ptr, " ");
					(*currentPt)[2] = atof(ptr + 1);
					
					currentVtx->Pc = currentPt;
					
					file.getline (fileInput, 256);
					currentVtx->dist = atof(fileInput);
					
					node->vtxArray[i][j][k] = currentVtx;
				}
			}
		}
		
		file.getline(fileInput, 256);
		
		if (strcmp(fileInput, "False") != 0) {
			for (i = 0; i < 2; i += 1) {
				for (j = 0; j < 2; j += 1) {
					for (k = 0; k < 2; k += 1) {
						newNode = createOctreeNode();
						newNode = readNodes(file, newNode);
						node->subnodeArray[i][j][k] = newNode;
					}
				}
			}
		}
	}
	
	return node;
}
*/

Vector GetNodeGradient(octreeNode *CollideNode, Point *CollidePoint, float dist)
{
	Vector Vect2Collide;
	float vect_terms[3];
	float delta=(CollideNode->m_edgeLength)/10.0;
	Testpoint->Set((collide[0]+delta),collide[1],collide[2]);
	vect_terms[0]=-(findInterpDist(CollideNode, Testpoint)-dist)/delta;
	
	Testpoint->Set(collide[0],(collide[1]+delta),collide[2]);
	vect_terms[1]=-(findInterpDist(CollideNode, Testpoint)-dist)/delta;
	
	Testpoint->Set(collide[0],collide[1],(collide[2]+delta));
	vect_terms[2]=-(findInterpDist(CollideNode, Testpoint)-dist)/delta;
	Vect2Collide.Set(vect_terms[0],vect_terms[1],vect_terms[2],0.0);
	float length=sqrt((Vect2Collide[0]*Vect2Collide[0])+(Vect2Collide[1]*Vect2Collide[1])+(Vect2Collide[2]*Vect2Collide[2]));
	float foo=dist/length;
	Vect2Collide=Vect2Collide*foo;
	return(Vect2Collide);
}

/**
** the keyboard callback:
**/

void printKeyMapping()
{
	printf("Key Mappings:\n");
	printf(" [ = toggles sphere tree display\n");
	printf(" ] = toggles mesh display\n");
	printf(" w s a d r f = moves collider around\n");
	printf(" i = show nodes that are inside the object\n");
	printf(" o = show nodes that are outside the object\n");
	printf(" n = show nodes that are on the object\n");
	printf(" , . = zoom in and out\n");
	printf(" + = change sphere tree level\n");
	printf(" * = print out info on the collider\n");
}

void Keyboard( unsigned char c, int x, int y )
{
	switch( c )
	{
	case '[':
		SPHERES_ON = !SPHERES_ON; break;
	case ']':
		MESH_ON = !MESH_ON; break;
	case 'w':
	case 'W':
		WhichProjection = collide[1]+=.02;;
		break;
	case 's':
	case 'S':
		WhichProjection = collide[1]-=.02;;
		break;
	case 'a':
	case 'A':
		WhichProjection = collide[0]+=.02;;
		break;
	case 'd':
	case 'D':
		WhichProjection = collide[0]-=.02;;
		break;
	case 'r':
	case 'R':
		WhichProjection = collide[2]+=.02;;
		break;
	case 'f':
	case 'F':
		WhichProjection = collide[2]-=.02;;
		break;
	case 'i':
	case 'I':
		WhatToShow=IN;
		break;
	case 'l':
		draw_internal_nodes = !draw_internal_nodes;
	case 'o':
	case 'O':
		WhatToShow=OUT;
		break;
	case 'n':
	case 'N':
		WhatToShow=ON;
		break;
	case ',':
		Scale += SCLFACT;
		if( Scale < MINSCALE )
			Scale = MINSCALE;
		break;
	case '.':
		Scale -= SCLFACT;
		if( Scale < MINSCALE )
			Scale = MINSCALE;
		break;
	case '+':
		changeSphereRenderLevel();
		printf( "%u\n" , getSphereRenderLevel() );
		break;
	case '*':
		break;
	default:
		printf( "\n" );
		printf( "Don't know what to do with keyboard hit:: '%c' (0x%0x)\n", c, c );
		printKeyMapping();
		printf( "\n" );
	}
	
	CollidePoint->Set(collide[0],collide[1],collide[2]);
	CollideNode=findNode(ADF,CollidePoint);
	float dist = findInterpDist(CollideNode, CollidePoint);
	Vector Vect2Collide;
	Vect2Collide=GetNodeGradient(CollideNode, CollidePoint, dist);
	Point Nearest;
	Nearest=(*CollidePoint)+Vect2Collide;
	collide_closest[0]=Nearest[0];
	collide_closest[1]=Nearest[1];
	collide_closest[2]=Nearest[2];
	if(dist>0) collide_TF=1;
	else collide_TF=0;
	
	if( c == '*' )
	{
		CollideError=minDistSTL(STL,CollidePoint);
		printf( "\n" );
		
		printf("Interpolated distance from surface = %f\n",dist);
		printf("Actual distance from the surface = %f\n" , CollideError->dist );
		printf("Distance error = %f percent off from the expected value\n", (  abs_d(abs_d(dist)-abs_d(CollideError->dist))/abs_d(dist) * 100  ));
		
		delete CollideError;
		printf("I'm at %f %f %f\n and the nearest point is %f %f %f\n",collide[0],collide[1],collide[2],collide_closest[0],collide_closest[1],collide_closest[2]);
		printf( "\n" );
	}
	
	
	
	/* force a call to Display():					*/
	
	glutSetWindow( MainWindow );
	glutPostRedisplay();
}


/**
* Generates a random location and determines if it is within the 
* given error limit.
*/
bool RandomPointCollider(double errorLimit)
{
	
	float x,y,z;
	x=(ADF->m_edgeLength*(((float)rand())/((float)RAND_MAX)));
	y=(ADF->m_edgeLength*(((float)rand())/((float)RAND_MAX)));
	z=(ADF->m_edgeLength*(((float)rand())/((float)RAND_MAX)));
	
	CollidePoint->Set(x,y,z,0);
	CollideNode = findNode(ADF,CollidePoint);
	
	float dist = findInterpDist(CollideNode, CollidePoint);
	octreeVtx *realdist = minDistSTL( STL , CollidePoint );
	
	Vector Vect2Collide;
	Vect2Collide=GetNodeGradient(CollideNode, CollidePoint, dist);
	
	Point Nearest;
	Nearest=(*CollidePoint)+Vect2Collide;
	collide_closest[0]=Nearest[0];
	collide_closest[1]=Nearest[1];
	collide_closest[2]=Nearest[2];
	
	if(dist>0) collide_TF=1;
	else collide_TF=0;
	
	
	bool metTolerance = MeetsTolerance(  realdist->dist , dist , errorLimit);
	
	delete realdist;
	
	
	return metTolerance;
}