#include "CooneySoftBody.h"

namespace Cooney
{
	void SpringEdge::Freeze(std::vector<SpringPoint> *points, std::vector<SpringPointState> *states, float materialStiffness)
	{
		// Get a Vector3 from PointB to PointA
		Cooney::Math::Vec3 edge_ab = (*states)[(*points)[pointBIndex].stateIndex].pointPosition - (*states)[(*points)[pointAIndex].stateIndex].pointPosition;
		// Determine the restlength
		restlength = Cooney::Math::Mag3(edge_ab);
		// Determine the stiffness so that the shorter the edge, the more tense the edge.
		// While there are more issues when trying to simulate a material than distance
		// between sample points, this works well in aiding the issue of a uniform material.
		stiffness = materialStiffness / restlength;
	}

	void SpringEdge::CalculateForces(Cooney::Math::Vec3 &outForceA, Cooney::Math::Vec3 &outForceB,
									 Cooney::Math::Vec3 &outDampingA, Cooney::Math::Vec3 &outDampingB,
									 std::vector<SpringPoint> *points, std::vector<SpringPointState> *states)
	{
		// Get information about the actual edge state.
		Vec3 edge_ab = (*states)[(*points)[pointBIndex].stateIndex].pointPosition - (*states)[(*points)[pointAIndex].stateIndex].pointPosition;
		float edge_length = Mag3(edge_ab);
		if(edge_length <= 0.0f)
		{
			outForceA = outForceB = outDampingA = outDampingB = Vec3();
			return;
		}
		Vec3 unit_ab = edge_ab / edge_length;

		// Simple, to calculate force, it's the difference from rest length * stiffness.
		outForceA = stiffness * (edge_length - restlength) * unit_ab;
		outForceB = -outForceA; // for every action, there is an equal and opposite reaction.
		
		// Get velocity for damping
		Vec3 velocityA = (*states)[(*points)[pointAIndex].stateIndex].pointVelocity;
		Vec3 velocityB = (*states)[(*points)[pointBIndex].stateIndex].pointVelocity;
		
		// Damping is a scaled negative of the difference in velocities.
		outDampingA = -damping * (Dot3(velocityA, unit_ab) - Dot3(velocityB, unit_ab)) * unit_ab;
		outDampingB = -outDampingA;
	}

	void SpringCornerBrace::Freeze(std::vector<SpringPoint> *points, std::vector<SpringPointState> *states)
	{
		SpringPoint &pointA = (*points)[pointAIndex];
		SpringPoint &pointB = (*points)[pointBIndex];
		SpringPoint &pointC = (*points)[pointCIndex];
		
		SpringPointState &stateA = (*states)[pointA.stateIndex];
		SpringPointState &stateB = (*states)[pointB.stateIndex];
		SpringPointState &stateC = (*states)[pointC.stateIndex];
		
		Vec3 edge_ab = stateB.pointPosition - stateA.pointPosition;
		MakeNormalized3(edge_ab);
		
		Vec3 edge_ac = stateC.pointPosition - stateA.pointPosition;
		MakeNormalized3(edge_ac);
		
		// calculate the angle between.
		float cosCorner = Dot3(edge_ab, edge_ac);
		float radCorner = ACos(cosCorner);
		
		// Store the rest angle in radians.
		restAngle = radCorner;
	}

	void SpringCornerBrace::CalculateForces(Cooney::Math::Vec3 &outForceA, Cooney::Math::Vec3 &outForceB, Cooney::Math::Vec3 &outForceC,
											Cooney::Math::Vec3 &outDampingA, Cooney::Math::Vec3 &outDampingB, Cooney::Math::Vec3 &outDampingC,
											std::vector<SpringPoint> *points, std::vector<SpringPointState> *states)
	{
		SpringPoint &pointA = (*points)[pointAIndex];
		SpringPoint &pointB = (*points)[pointBIndex];
		SpringPoint &pointC = (*points)[pointCIndex];
		
		SpringPointState &stateA = (*states)[pointA.stateIndex];
		SpringPointState &stateB = (*states)[pointB.stateIndex];
		SpringPointState &stateC = (*states)[pointC.stateIndex];
		
		// Get information about the edge states.

		// a to b
		Vec3 edge_ab = stateB.pointPosition - stateA.pointPosition;
		Vec3 unit_ab = Normalized3(edge_ab);
		float length_ab = Mag3(edge_ab);
		
		// a to c
		Vec3 edge_ac = stateC.pointPosition - stateA.pointPosition;
		Vec3 unit_ac = Normalized3(edge_ac);
		float length_ac = Mag3(edge_ac);
		
		// Calculate the perpendicular vector that
		// represents the hinge around rotation.
		Vec3 hinge = Normalized3(Cross3(unit_ab,unit_ac));
		
		// Calculate tangential vectors for applying angular
		// forces about the hinge.
		Vec3 normal_b = Normalized3(Cross3(hinge, unit_ab));
		Vec3 normal_c = Normalized3(Cross3(unit_ac, hinge));
		
		// Determine the angle between both edges.
		float cosCorner = Dot3(unit_ab, unit_ac);
		float radCorner = ACos(cosCorner);
		
		// Calculate the difference from the desired angle.
		float radTheta = radCorner - restAngle;
		
		Vec3 force_a, force_b, force_c;
		
		// Determine the forces on the edge end points.
		force_b = ((stiffness * radTheta) / length_ab) * normal_b;
		force_c = ((stiffness * radTheta) / length_ac) * normal_c;
		force_a = -(force_b + force_c); // for every action, there is an equal and opposite reaction.
		
		outForceA = force_a;
		outForceB = force_b;
		outForceC = force_c;
		
		Vec3 damping_a, damping_b, damping_c;

		// Determine damping as a scaled inverse differential velocity of the brace points.
		damping_b = ((-damping * (Dot3(stateB.pointVelocity, normal_b)/length_ab - Dot3(stateC.pointVelocity, normal_c)/length_ac))) * normal_b;
		damping_c = ((-damping * (Dot3(stateC.pointVelocity, normal_c)/length_ac - Dot3(stateB.pointVelocity, normal_b)/length_ab))) * normal_c;
		damping_a = -(damping_b + damping_c);
		
		outDampingA = damping_a;
		outDampingB = damping_b;
		outDampingC = damping_c;
	}

	void SoftBody::AddQuad(Cooney::Math::Vec3 pointA, Cooney::Math::Vec3 pointB, Cooney::Math::Vec3 pointC, Cooney::Math::Vec3 pointD,
								int *indexAOut, int *indexBOut, int *indexCOut, int *indexDOut)
	{
		int pointAIndex, pointBIndex, pointCIndex, pointDIndex;
		pointAIndex = pointBIndex = pointCIndex = pointDIndex = -1;
	
		// Check for duplicate points, if they exist, store those indexes
		// in the point*Index.
		for(unsigned int i=0; i<(unsigned int)points.size(); ++i)
		{	
			// Compare positions for duplicates.
			// A
			if( pointA == points[i].initialState.pointPosition )
				pointAIndex = (int)i;
			// B
			if( pointB == points[i].initialState.pointPosition )
				pointBIndex = (int)i;
			// C
			if( pointC == points[i].initialState.pointPosition )
				pointCIndex = (int)i;
			// D
			if( pointD == points[i].initialState.pointPosition )
				pointDIndex = (int)i;
		}
		
		// If pointA is not a duplicate, add it.
		if( pointAIndex < 0 )
		{
			SpringPoint newPoint;
			newPoint.initialState.pointPosition = pointA;
			
			pointAIndex = points.size();
			
			points.push_back(newPoint);
		}
		
		// If pointB is not a duplicate, add it.
		if( pointBIndex < 0 )
		{
			SpringPoint newPoint;
			newPoint.initialState.pointPosition = pointB;
			
			pointBIndex = points.size();
			
			points.push_back(newPoint);
		}
		
		// If pointC is not a duplicate, add it.
		if( pointCIndex < 0 )
		{
			SpringPoint newPoint;
			newPoint.initialState.pointPosition = pointC;
			
			pointCIndex = points.size();
			
			points.push_back(newPoint);
		}
		
		// If pointD is not a duplicate, add it.
		if( pointDIndex < 0 )
		{
			SpringPoint newPoint;
			newPoint.initialState.pointPosition = pointD;
			
			pointDIndex = points.size();
			
			points.push_back(newPoint);
		}
		
		int edgeAIndex, edgeBIndex, edgeCIndex, edgeDIndex;
		edgeAIndex = edgeBIndex = edgeCIndex = edgeDIndex = -1;
		
		// Check for duplicate edges. If any duplicates exist,
		// assign their index to edge*Index.
		for(unsigned int i=0; i<(unsigned int)edges.size(); ++i)
		{
			// Compare the edges' point indexes for duplicates.
			// A
			if( (edges[i].pointAIndex == pointAIndex && edges[i].pointBIndex == pointBIndex)
				||
				(edges[i].pointBIndex == pointAIndex && edges[i].pointAIndex == pointBIndex) )
				edgeAIndex = (int)i;
			// B
			if( (edges[i].pointAIndex == pointBIndex && edges[i].pointBIndex == pointCIndex)
			   ||
			   (edges[i].pointBIndex == pointBIndex && edges[i].pointAIndex == pointCIndex) )
				edgeBIndex = (int)i;
			// C
			if( (edges[i].pointAIndex == pointCIndex && edges[i].pointBIndex == pointDIndex)
			   ||
			   (edges[i].pointBIndex == pointCIndex && edges[i].pointAIndex == pointDIndex) )
				edgeCIndex = (int)i;
			// D
			if( (edges[i].pointAIndex == pointDIndex && edges[i].pointBIndex == pointAIndex)
			   ||
			   (edges[i].pointBIndex == pointDIndex && edges[i].pointAIndex == pointAIndex) )
				edgeDIndex = (int)i;
		}
		
		// If edgeA is not a duplicate, add it.
		if( edgeAIndex < 0 )
		{
			SpringEdge newEdge;
			newEdge.pointAIndex = pointAIndex;
			newEdge.pointBIndex = pointBIndex;
			
			edgeAIndex = edges.size();
			
			edges.push_back(newEdge);
		}
		
		// If edgeB is not a duplicate, add it.
		if( edgeBIndex < 0 )
		{
			SpringEdge newEdge;
			newEdge.pointAIndex = pointBIndex;
			newEdge.pointBIndex = pointCIndex;
			
			edgeBIndex = edges.size();
			
			edges.push_back(newEdge);
		}
		
		// If edgeC is not a duplicate, add it.
		if( edgeCIndex < 0 )
		{
			SpringEdge newEdge;
			newEdge.pointAIndex = pointCIndex;
			newEdge.pointBIndex = pointDIndex;
			
			edgeCIndex = edges.size();
			
			edges.push_back(newEdge);
		}
		
		// If edgeD is not a duplicate, add it.
		if( edgeDIndex < 0 )
		{
			SpringEdge newEdge;
			newEdge.pointAIndex = pointDIndex;
			newEdge.pointBIndex = pointAIndex;
			
			edgeAIndex = edges.size();
			
			edges.push_back(newEdge);
		}
		
		int braceAIndex, braceBIndex, braceCIndex, braceDIndex;
		braceAIndex = braceBIndex = braceCIndex = braceDIndex = -1;
		
		// Check for duplicate braces. If any brace is a duplicate,
		// set its index to brace*Index.
		for(unsigned int i=0; i<(unsigned int)braces.size(); ++i)
		{
			// Compare brace point indexes for duplicates

			// A little tricky to read below, but the check works as follows:
			// If the corner point A is shared,
			// AND EITHER
			// pointB is shared with the point counter-clockwise AND pointC is shared with the point clockwise,
			// OR
			// pointB is shared with the point clockwise AND pointC is shared with the point counter-clockwise,
			// THEN
			// This is a duplicate brace

			// A
			if	( braces[i].pointAIndex == pointAIndex &&
					(
					(braces[i].pointBIndex == pointBIndex && braces[i].pointCIndex == pointDIndex)
					||
					(braces[i].pointCIndex == pointBIndex && braces[i].pointBIndex == pointDIndex)
					)
				)
				braceAIndex = i;
			// B
			if	( braces[i].pointAIndex == pointBIndex &&
				 (
				  (braces[i].pointBIndex == pointCIndex && braces[i].pointCIndex == pointAIndex)
				  ||
				  (braces[i].pointCIndex == pointCIndex && braces[i].pointBIndex == pointAIndex)
				  )
				 )
				braceBIndex = i;
			// C
			if	( braces[i].pointAIndex == pointCIndex &&
				 (
				  (braces[i].pointBIndex == pointDIndex && braces[i].pointCIndex == pointBIndex)
				  ||
				  (braces[i].pointCIndex == pointDIndex && braces[i].pointBIndex == pointBIndex)
				  )
				 )
				braceCIndex = i;
			// D
			if	( braces[i].pointAIndex == pointDIndex &&
				 (
				  (braces[i].pointBIndex == pointAIndex && braces[i].pointCIndex == pointCIndex)
				  ||
				  (braces[i].pointCIndex == pointAIndex && braces[i].pointBIndex == pointCIndex)
				  )
				 )
				braceDIndex = i;
		}
		
		// If braceA is not duplicate, add it.
		if( braceAIndex < 0 )
		{
			SpringCornerBrace newBrace;
			newBrace.pointAIndex = pointAIndex;
			newBrace.pointBIndex = pointBIndex;
			newBrace.pointCIndex = pointDIndex;
			
			braceAIndex = braces.size();
			
			braces.push_back(newBrace);
		}
		
		// If braceB is not duplicate, add it.
		if( braceBIndex < 0 )
		{
			SpringCornerBrace newBrace;
			newBrace.pointAIndex = pointBIndex;
			newBrace.pointBIndex = pointCIndex;
			newBrace.pointCIndex = pointAIndex;
			
			braceBIndex = braces.size();
			
			braces.push_back(newBrace);
		}
		
		// If braceC is not duplicate, add it.
		if( braceCIndex < 0 )
		{
			SpringCornerBrace newBrace;
			newBrace.pointAIndex = pointCIndex;
			newBrace.pointBIndex = pointDIndex;
			newBrace.pointCIndex = pointBIndex;
			
			braceCIndex = braces.size();
			
			braces.push_back(newBrace);
		}
		
		// If braceD is not duplicate, add it.
		if( braceDIndex < 0 )
		{
			SpringCornerBrace newBrace;
			newBrace.pointAIndex = pointDIndex;
			newBrace.pointBIndex = pointAIndex;
			newBrace.pointCIndex = pointCIndex;
			
			braceDIndex = braces.size();
			
			braces.push_back(newBrace);
		}
		
		// Set the outputs to the point indexes.
		// This is a one line check for NULL before
		// setting the output.
		indexAOut ? *indexAOut = pointAIndex : 0;
		indexBOut ? *indexBOut = pointBIndex : 0;
		indexCOut ? *indexCOut = pointCIndex : 0;
		indexDOut ? *indexDOut = pointDIndex : 0;
	}

	void SoftBody::AddTriangle(Cooney::Math::Vec3 pointA, Cooney::Math::Vec3 pointB, Cooney::Math::Vec3 pointC,
									int *indexAOut, int *indexBOut, int *indexCOut)
	{
		int pointAIndex, pointBIndex, pointCIndex;
		pointAIndex = pointBIndex = pointCIndex = -1;
		
		// Check for duplicate points. If any points are duplicates
		// store their indexes in point*Index.
		for(unsigned int i=0; i<(unsigned int)points.size(); ++i)
		{	
			// Duplicate checks are point position comparisons.
			// A
			if( pointA == points[i].initialState.pointPosition )
				pointAIndex = (int)i;
			// B
			if( pointB == points[i].initialState.pointPosition )
				pointBIndex = (int)i;
			// C
			if( pointC == points[i].initialState.pointPosition )
				pointCIndex = (int)i;
		}
		
		// If pointA is not a duplicate, add it.
		if( pointAIndex < 0 )
		{
			SpringPoint newPoint;
			newPoint.initialState.pointPosition = pointA;
			
			pointAIndex = points.size();
			
			points.push_back(newPoint);
		}
		
		// If pointB is not a duplicate, add it.
		if( pointBIndex < 0 )
		{
			SpringPoint newPoint;
			newPoint.initialState.pointPosition = pointB;
			
			pointBIndex = points.size();
			
			points.push_back(newPoint);
		}
		
		// If pointC is not a duplicate, add it.
		if( pointCIndex < 0 )
		{
			SpringPoint newPoint;
			newPoint.initialState.pointPosition = pointC;
			
			pointCIndex = points.size();
			
			points.push_back(newPoint);
		}
		
		int edgeAIndex, edgeBIndex, edgeCIndex;
		edgeAIndex = edgeBIndex = edgeCIndex = -1;
		
		// Check for duplicate edges. If any edges are duplicate,
		// set edge*Index to their index.
		for(unsigned int i=0; i<(unsigned int)edges.size(); ++i)
		{
			// Duplicate checks are performed by compare edge indexes.
			// A
			if( (edges[i].pointAIndex == pointAIndex && edges[i].pointBIndex == pointBIndex)
			   ||
			   (edges[i].pointBIndex == pointAIndex && edges[i].pointAIndex == pointBIndex) )
				edgeAIndex = (int)i;
			// B
			if( (edges[i].pointAIndex == pointBIndex && edges[i].pointBIndex == pointCIndex)
			   ||
			   (edges[i].pointBIndex == pointBIndex && edges[i].pointAIndex == pointCIndex) )
				edgeBIndex = (int)i;
			// C
			if( (edges[i].pointAIndex == pointCIndex && edges[i].pointBIndex == pointAIndex)
			   ||
			   (edges[i].pointBIndex == pointCIndex && edges[i].pointAIndex == pointAIndex) )
				edgeCIndex = (int)i;
		}
		
		// If edgeA is not a duplicate, add it.
		if( edgeAIndex < 0 )
		{
			SpringEdge newEdge;
			newEdge.pointAIndex = pointAIndex;
			newEdge.pointBIndex = pointBIndex;
			
			edgeAIndex = edges.size();
			
			edges.push_back(newEdge);
		}
		
		// If edgeB is not a duplicate, add it.
		if( edgeBIndex < 0 )
		{
			SpringEdge newEdge;
			newEdge.pointAIndex = pointBIndex;
			newEdge.pointBIndex = pointCIndex;
			
			edgeBIndex = edges.size();
			
			edges.push_back(newEdge);
		}
		
		// If edgeB is not a duplicate, add it.
		if( edgeCIndex < 0 )
		{
			SpringEdge newEdge;
			newEdge.pointAIndex = pointCIndex;
			newEdge.pointBIndex = pointAIndex;
			
			edgeCIndex = edges.size();
			
			edges.push_back(newEdge);
		}
		
		int braceAIndex, braceBIndex, braceCIndex;
		braceAIndex = braceBIndex = braceCIndex = -1;
		
		// Check for duplicate corner braces. If any braces
		// are duplicate, set brace*Index to the index of
		// the original brace.
		for(unsigned int i=0; i<(unsigned int)braces.size(); ++i)
		{
			// Check for duplicates by referenced point index comparisons.

			// A little tricky to read below, but the check works as follows:
			// If the corner point A is shared,
			// AND EITHER
			// pointB is shared with the point counter-clockwise AND pointC is shared with the point clockwise,
			// OR
			// pointB is shared with the point clockwise AND pointC is shared with the point counter-clockwise,
			// THEN
			// This is a duplicate brace

			// A
			if	( braces[i].pointAIndex == pointAIndex &&
				 (
				  (braces[i].pointBIndex == pointBIndex && braces[i].pointCIndex == pointCIndex)
				  ||
				  (braces[i].pointCIndex == pointBIndex && braces[i].pointBIndex == pointCIndex)
				  )
				 )
				braceAIndex = i;
			// B
			if	( braces[i].pointAIndex == pointBIndex &&
				 (
				  (braces[i].pointBIndex == pointCIndex && braces[i].pointCIndex == pointAIndex)
				  ||
				  (braces[i].pointCIndex == pointCIndex && braces[i].pointBIndex == pointAIndex)
				  )
				 )
				braceBIndex = i;
			// C
			if	( braces[i].pointAIndex == pointCIndex &&
				 (
				  (braces[i].pointBIndex == pointAIndex && braces[i].pointCIndex == pointBIndex)
				  ||
				  (braces[i].pointCIndex == pointAIndex && braces[i].pointBIndex == pointBIndex)
				  )
				 )
				braceCIndex = i;
		}
		
		// if braceA is not a duplicate, add it.
		if( braceAIndex < 0 )
		{
			SpringCornerBrace newBrace;
			newBrace.pointAIndex = pointAIndex;
			newBrace.pointBIndex = pointBIndex;
			newBrace.pointCIndex = pointCIndex;
			
			braceAIndex = braces.size();
			
			braces.push_back(newBrace);
		}
		
		// if braceB is not a duplicate, add it.
		if( braceBIndex < 0 )
		{
			SpringCornerBrace newBrace;
			newBrace.pointAIndex = pointBIndex;
			newBrace.pointBIndex = pointCIndex;
			newBrace.pointCIndex = pointAIndex;
			
			braceBIndex = braces.size();
			
			braces.push_back(newBrace);
		}
		
		// if braceC is not a duplicate, add it.
		if( braceCIndex < 0 )
		{
			SpringCornerBrace newBrace;
			newBrace.pointAIndex = pointCIndex;
			newBrace.pointBIndex = pointAIndex;
			newBrace.pointCIndex = pointBIndex;
			
			braceCIndex = braces.size();
			
			braces.push_back(newBrace);
		}
		
		// Set the output parameters to the point indexes.
		// This is a one-line check for NULL and assignment.
		indexAOut ? *indexAOut = pointAIndex : 0;
		indexBOut ? *indexBOut = pointBIndex : 0;
		indexCOut ? *indexCOut = pointCIndex : 0;
	}

	void SoftBody::AddEdge(Cooney::Math::Vec3 pointA, Cooney::Math::Vec3 pointB, bool bHidden)
	{
		int pointAIndex, pointBIndex;
		pointAIndex = pointBIndex  = -1;
		
		// Check to make sure this is actually an edge.
		if( pointA == pointB )
			return;
		
		// Check for duplicate points. If any point is a 
		// duplicate, assign its index to point*Index.
		for(unsigned int i=0; i<(unsigned int)points.size(); ++i)
		{	
			if( pointA == points[i].initialState.pointPosition )
				pointAIndex = (int)i;
			
			if( pointB == points[i].initialState.pointPosition )
				pointBIndex = (int)i;
		}

		// Check for duplicate edges. If any edges are duplicate
		// then we don't have anything else to do.
		if( pointAIndex != -1 && pointBIndex != -1 )
			for(unsigned int i=0; i<(unsigned int)edges.size(); ++i)
			{
				if( (edges[i].pointAIndex == pointAIndex && edges[i].pointBIndex == pointBIndex) ||
					(edges[i].pointBIndex == pointBIndex && edges[i].pointBIndex == pointAIndex) )
					return; // edge exists, exit.
			}
		
		// If pointA is not a duplicate, add it.
		if( pointAIndex < 0 )
		{
			SpringPoint newPoint;
			newPoint.initialState.pointPosition = pointA;
			pointAIndex = points.size();
			points.push_back(newPoint);
		}
		
		// If pointB is not a duplicate, add it.
		if( pointBIndex < 0 )
		{
			SpringPoint newPoint;
			newPoint.initialState.pointPosition = pointB;
			pointBIndex = points.size();
			points.push_back(newPoint);
		}
		
		// Add the new edge.
		SpringEdge newEdge;
		newEdge.pointAIndex = pointAIndex;
		newEdge.pointBIndex = pointBIndex;
		newEdge.bHidden = bHidden;
		edges.push_back(newEdge);
	}

	void SoftBody::AddEdge(const unsigned int pointAIndex, const unsigned int pointBIndex, bool bHidden)
	{
		// Check for invalid point indexes.
		if( pointAIndex == pointBIndex ||
		   pointAIndex < 0 || pointBIndex < 0 ||
		   pointAIndex >= (unsigned int)points.size() || pointBIndex >= (unsigned int)points.size() )
			return;
		
		// Check for a duplicate edge.
		for(unsigned int i=0; i<(unsigned int)edges.size(); ++i)
		{
			if( (edges[i].pointAIndex == pointAIndex && edges[i].pointBIndex == pointBIndex) ||
			   (edges[i].pointBIndex == pointAIndex && edges[i].pointBIndex == pointBIndex) )
				return; // edge exists, exit
		}
		
		// Add the new edge.
		SpringEdge newEdge;
		newEdge.pointAIndex = pointAIndex;
		newEdge.pointBIndex = pointBIndex;
		newEdge.bHidden = bHidden;
		edges.push_back(newEdge);
	}

	bool SoftBody::ConstructSpringyGrid(unsigned int divisionsX, unsigned int divisionsY, unsigned int divisionsZ, float scale, 
								  float stiffness, float damping, float cornerstiffness, float cornerdamping,
								  Cooney::Math::Mat4 TransformationMat, bool bInterConnect)
	{
		if( scale <= 0.0f || (stiffness == 0 && damping == 0 && cornerstiffness == 0 && cornerdamping == 0) )
			return false;

		// *Step represents the size of each grid-cell.
		float xStep, yStep, zStep;
		xStep = scale / (float)divisionsX;
		yStep = scale / (float)divisionsY;
		zStep = scale / (float)divisionsZ;
		
		// Laying out the grid points manually ahead
		// of the actuall edges so that the points
		// are layed out predictably in memory and they
		// can be used elsewhere.
		points.resize((divisionsX+1)*(divisionsY+1)*(divisionsZ+1));
		for(unsigned int x=0; x<divisionsX+1; ++x)
		{
			for(unsigned int y=0; y<divisionsY+1; ++y)
			{
				for(unsigned int z=0; z<divisionsZ+1; ++z)
				{
					// Add the point.
					// To get the point position, go back half the size of the grid
					// and then step upwards from there.
					SpringPoint newPoint;
					newPoint.initialState.pointPosition = -Vector3(scale/2.0f, scale/2.0f, scale/2.0f) + Vector3(xStep * x, yStep * y, zStep * z);
					points[x + y*(divisionsX+1) + z*(divisionsX+1)*(divisionsY+1)] = newPoint;
				}
			}
		}
		 
		// Step through each grid cell block and construct
		// a springy cube. If the grid is requested to be
		// interconnected, the cube's corners are linked to
		// all of the other cell cube's corners. Cubes do not
		// interconnect to neighboring cubes except at
		// connecting walls.
		for(unsigned int x=0; x<divisionsX; ++x)
		{
			for(unsigned int y=0; y<divisionsY; ++y)
			{
				for(unsigned int z=0; z<divisionsZ; ++z)
				{
					// Get the bottom left of the cell block and
					// work from there.
					Vec3 rootPosition = Vector3(xStep * x, yStep * y, zStep * z) - Vector3(scale/2.0f, scale/2.0f, scale/2.0f);
					
					// Since the memory is already layed out, this will not
					// create duplicate points. It will create new edges and
					// braces though.

					// -Y wall.
					AddQuad(rootPosition + Vector3(0,		0,			0),
							rootPosition + Vector3(xStep,	0,			0),
							rootPosition + Vector3(xStep,	0,			zStep),
							rootPosition + Vector3(0,		0,			zStep));
					
					// Y wall.
					AddQuad(rootPosition + Vector3(0,		yStep,		0),
							rootPosition + Vector3(xStep,	yStep,		0),
							rootPosition + Vector3(xStep,	yStep,		zStep),
							rootPosition + Vector3(0,		yStep,		zStep));
					
					// -Z wall.
					AddQuad(rootPosition + Vector3(0,		0,			0),
							rootPosition + Vector3(xStep,	0,			0),
							rootPosition + Vector3(xStep,	yStep,		0),
							rootPosition + Vector3(0,		yStep,		0));
					
					// Z wall.
					AddQuad(rootPosition + Vector3(0,		0,			zStep),
							rootPosition + Vector3(xStep,	0,			zStep),
							rootPosition + Vector3(xStep,	yStep,		zStep),
							rootPosition + Vector3(0,		yStep,		zStep));
					
					// -X wall.
					AddQuad(rootPosition + Vector3(0,		0,			0),
							rootPosition + Vector3(0,		0,			zStep),
							rootPosition + Vector3(0,		yStep,		zStep),
							rootPosition + Vector3(0,		yStep,		0));
					
					// X wall.
					AddQuad(rootPosition + Vector3(xStep,	0,			0),
							rootPosition + Vector3(xStep,	0,			zStep),
							rootPosition + Vector3(xStep,	yStep,		zStep),
							rootPosition + Vector3(xStep,	yStep,		0));
					
					// connect every cell block point to every
					// other cell block. (cell, not entire grid).
					if( bInterConnect )
					{
						// total opposites
						AddEdge(rootPosition + Vector3(0,0,0),				rootPosition + Vector3(xStep, yStep, zStep), true);
						AddEdge(rootPosition + Vector3(xStep, 0, zStep),	rootPosition + Vector3(0, yStep, 0), true);
						AddEdge(rootPosition + Vector3(xStep, 0, 0),		rootPosition + Vector3(0, yStep, zStep), true);
						AddEdge(rootPosition + Vector3(0,0,zStep),			rootPosition + Vector3(xStep, yStep, 0), true);
						
						// -X edge to opposites (not all)
						AddEdge(rootPosition + Vector3(0,0,0),				rootPosition + Vector3(xStep, yStep, 0), true);
						AddEdge(rootPosition + Vector3(0,0,0),				rootPosition + Vector3(0, yStep, zStep), true);
						AddEdge(rootPosition + Vector3(0,yStep,0),			rootPosition + Vector3(xStep, 0, 0), true);
						AddEdge(rootPosition + Vector3(0,yStep,0),			rootPosition + Vector3(0, 0, zStep), true);
						
						// +X edge to opposites (not all)
						AddEdge(rootPosition + Vector3(xStep,0,zStep),		rootPosition + Vector3(xStep, yStep, 0), true);
						AddEdge(rootPosition + Vector3(xStep,0,zStep),		rootPosition + Vector3(0, yStep, zStep), true);
						AddEdge(rootPosition + Vector3(xStep,yStep,zStep),	rootPosition + Vector3(xStep, 0, 0), true);
						AddEdge(rootPosition + Vector3(xStep,yStep,zStep),	rootPosition + Vector3(0, 0, zStep), true);
						
						// -Y wall opposites
						AddEdge(rootPosition + Vector3(0,0,0),				rootPosition + Vector3(xStep, 0, zStep), true);
						AddEdge(rootPosition + Vector3(xStep,0,0),			rootPosition + Vector3(0, 0, zStep), true);
						
						// +Y wall opposites
						AddEdge(rootPosition + Vector3(0,yStep,0),			rootPosition + Vector3(xStep, yStep, zStep), true);
						AddEdge(rootPosition + Vector3(xStep,yStep,0),		rootPosition + Vector3(0, yStep, zStep), true);
					}
				}
			}
		}
		
		// Store the materialStiffness. Use this later
		// to automatically determine edge tension.
		materialStiffness = stiffness;
		
		// Transform all of the points by some transformation matrix,
		// which may or may not represent the object's work position.
		for(unsigned int i=0; i<(unsigned int)points.size(); ++i)
		{
			points[i].initialState.pointPosition = ToVector3(TransformationMat * ToVector4(points[i].initialState.pointPosition, 1.0f));
		}
		
		// Assign damping to the edges, stiffness is automatically
		// determined in the later, Initiate() call which calls
		// the individual edge's Freeze() function which determines
		// the stiffness.
		if( stiffness == 0.0f && damping == 0.0f )
		{
			edges.clear();
		}
		else
		{
			for(unsigned int i=0; i<(unsigned int)edges.size(); ++i)
			{
				edges[i].damping = damping;
			}
		}
		
		// Assign damping and stiffness to the corner braces.
		if( cornerstiffness == 0.0f && cornerdamping == 0.0f )
		{
			braces.clear();
		}
		else
		{
			for(unsigned int i=0; i<(unsigned int)braces.size(); ++i)
			{
				braces[i].stiffness = cornerstiffness;
				braces[i].damping = cornerdamping;
			}
		}
		
		// Prepare the grid to be run. This freezes all of the points
		// and prepares the dynamic state-array.
		Initiate();

		return true;
	}

	bool SoftBody::ConstructSpringyCube(float scale, float stiffness, float damping,
														float cornerstiffness, float cornerdamping,
														Cooney::Math::Mat4 TransformationMat)
	{	
		if( scale <= 0.0f || stiffness < 0.0f || damping < 0.0f )
			return false;
		
		materialStiffness = stiffness;
		
		float halfScale = scale / 2.0f;
		
		// Construct the cube.

		AddQuad(Vector3(-halfScale, -halfScale, halfScale),
				Vector3(halfScale, -halfScale, halfScale),
				Vector3(halfScale, halfScale, halfScale),
				Vector3(-halfScale, halfScale, halfScale));
		AddQuad(Vector3(-halfScale, -halfScale, -halfScale),
				Vector3(halfScale, -halfScale, -halfScale),
				Vector3(halfScale, halfScale, -halfScale),
				Vector3(-halfScale, halfScale, -halfScale));
		AddQuad(Vector3(-halfScale, -halfScale, -halfScale),
				Vector3(-halfScale, -halfScale, halfScale),
				Vector3(-halfScale, halfScale, halfScale),
				Vector3(-halfScale, halfScale, -halfScale));
		AddQuad(Vector3(halfScale, -halfScale, -halfScale),
				Vector3(halfScale, -halfScale, halfScale),
				Vector3(halfScale, halfScale, halfScale),
				Vector3(halfScale, halfScale, -halfScale));
		AddQuad(Vector3(-halfScale, halfScale, halfScale),
				Vector3(halfScale, halfScale, halfScale),
				Vector3(halfScale, halfScale, -halfScale),
				Vector3(-halfScale, halfScale, -halfScale));
		AddQuad(Vector3(-halfScale, -halfScale, halfScale),
				Vector3(halfScale, -halfScale, halfScale),
				Vector3(halfScale, -halfScale, -halfScale),
				Vector3(-halfScale, -halfScale, -halfScale));
		
		
		// Transform all of the points by the transformation matrix.
		for(unsigned int i=0; i<(unsigned int)points.size(); ++i)
		{
			points[i].initialState.pointPosition = ToVector3(TransformationMat * ToVector4(points[i].initialState.pointPosition, 1.0f));
		}
		
		// Manually set edge dampening.
		// Edge stiffness is automatically set in Initiate
		// as a consequence of materialStiffness.
		for(unsigned int i=0; i<(unsigned int)edges.size(); ++i)
			edges[i].damping = damping;
		
		// Manually set corner stiffness and damping.
		for(unsigned int i=0; i<(unsigned int)braces.size(); ++i)
		{
			braces[i].stiffness = cornerstiffness;
			braces[i].damping = cornerdamping;
		}
		
		// Initiate the state array and freeze the mesh into
		// its rest state.
		Initiate();
		
		return true;
	}

	bool SoftBody::ConstructSpringyTriCube(float scale, float stiffness, float damping,
														   float cornerstiffness, float cornerdamping,
														   Cooney::Math::Mat4 TransformationMat)
	{	
		if( scale <= 0.0f || stiffness < 0.0f || damping < 0.0f )
			return false; // bad parameters.
		
		materialStiffness = stiffness;
		
		float halfScale = scale / 2.0f;

		// construct the TriCube
		
		AddTriangle(Vector3(-halfScale, -halfScale, halfScale),
					Vector3(halfScale, -halfScale, halfScale),
					Vector3(-halfScale, halfScale, halfScale));
		AddTriangle(Vector3(halfScale, -halfScale, halfScale),
					Vector3(halfScale, halfScale, halfScale),
					Vector3(-halfScale, halfScale, halfScale));
		
		AddTriangle(Vector3(-halfScale, -halfScale, -halfScale),
					Vector3(halfScale, -halfScale, -halfScale),
					Vector3(-halfScale, halfScale, -halfScale));
		AddTriangle(Vector3(halfScale, -halfScale, -halfScale),
					Vector3(halfScale, halfScale, -halfScale),
					Vector3(-halfScale, halfScale, -halfScale));
		
		AddTriangle(Vector3(-halfScale, -halfScale, -halfScale),
					Vector3(-halfScale, -halfScale, halfScale),
					Vector3(-halfScale, halfScale, -halfScale));
		AddTriangle(Vector3(-halfScale, -halfScale, halfScale),
					Vector3(-halfScale, halfScale, halfScale),
					Vector3(-halfScale, halfScale, -halfScale));
		
		AddTriangle(Vector3(halfScale, -halfScale, -halfScale),
					Vector3(halfScale, -halfScale, halfScale),
					Vector3(halfScale, halfScale, -halfScale));
		AddTriangle(Vector3(halfScale, -halfScale, halfScale),
					Vector3(halfScale, halfScale, halfScale),
					Vector3(halfScale, halfScale, -halfScale));
		
		AddTriangle(Vector3(-halfScale, halfScale, halfScale),
					Vector3(halfScale, halfScale, halfScale),
					Vector3(-halfScale, halfScale, -halfScale));
		AddTriangle(Vector3(halfScale, halfScale, halfScale),
					Vector3(halfScale, halfScale, -halfScale),
					Vector3(-halfScale, halfScale, -halfScale));
		
		AddTriangle(Vector3(-halfScale, -halfScale, halfScale),
					Vector3(halfScale, -halfScale, halfScale),
					Vector3(-halfScale, -halfScale, -halfScale));
		AddTriangle(Vector3(halfScale, -halfScale, halfScale),
					Vector3(halfScale, -halfScale, -halfScale),
					Vector3(-halfScale, -halfScale, -halfScale));
		
		// Transform all of the points by the transformation matrix.
		for(unsigned int i=0; i<(unsigned int)points.size(); ++i)
		{
			points[i].initialState.pointPosition = ToVector3(TransformationMat * ToVector4(points[i].initialState.pointPosition, 1.0f));
		}
		
		// Manually set edge dampening.
		// Edge stiffness is automatically set in Initiate
		// as a consequence of materialStiffness.
		for(unsigned int i=0; i<(unsigned int)edges.size(); ++i)
			edges[i].damping = damping;
		
		// Manually set corner stiffness and damping.
		for(unsigned int i=0; i<(unsigned int)braces.size(); ++i)
		{
			braces[i].stiffness = cornerstiffness;
			braces[i].damping = cornerdamping;
		}
		
		// Initiate the state array and freeze the mesh into
		// its rest state.
		Initiate();
		
		return true;
	}

	void SoftBody::FloodSettings(float stiffness, float damping, float cornerStiffness, float cornerDamping)
	{
		materialStiffness = stiffness;
		
		// Calculate edge-parameters
		for(unsigned int i=0; i<(unsigned int)edges.size(); ++i)
		{
			edges[i].stiffness = materialStiffness / edges[i].restlength;
			edges[i].damping = damping;
		}
		
		// Calculate brace-parameters.
		for(unsigned int i=0; i<(unsigned int)braces.size(); ++i)
		{	
			braces[i].stiffness = cornerStiffness;
			braces[i].damping = cornerDamping;
		}
	}

	void SoftBody::Initiate()
	{	
		// Resize the dynamic states array to the
		// number of points we have.
		currentStates.resize(points.size());
		for(unsigned int i=0; i<(unsigned int)currentStates.size(); ++i)
		{
			// The initial state is stored in each point, set that now.
			currentStates[i] = points[i].initialState;
			points[i].stateIndex = i;
		}

		// Freeze() all of the edges to set their
		// rest-states and stiffness.
		for(unsigned int i=0; i<(unsigned int)edges.size(); ++i)
		{
			if( !edges[i].VerifyIntegrity(&points, &currentStates) )
				continue;
			
			edges[i].Freeze(&points, &currentStates, materialStiffness);
		}

		// Freeze() all of the braces to set their rest-angles.
		for(unsigned int i=0; i<(unsigned int)braces.size(); ++i)
		{
			if( !braces[i].VerifyIntegrity(&points, &currentStates) )
				continue;
			
			braces[i].Freeze(&points, &currentStates);
		}
		
		// Debug
		std::cout << "Springy Mesh Info:\n";
		std::cout << "Num Points: " << points.size() << "\n";
		std::cout << "Num Edges: " << edges.size() << "\n";
		std::cout << "Num Braces: " << braces.size() << "\n";
	}

	void SoftBody::AddVelocity(Cooney::Math::Vec3 velocity)
	{
		// Loop through all of the points and add velocity.
		for(unsigned int i=0; i<(unsigned int)points.size(); ++i)
		{
			if( !points[i].VerifyIntegrity(&currentStates) )
				continue;
			
			currentStates[points[i].stateIndex].pointVelocity += velocity;
		}
	}

	std::vector<Cooney::Math::Vec3> SoftBody::CalculateForces(std::vector<SpringPointState> *states, float timeStep)
	{
		std::vector<Cooney::Math::Vec3> outForces;
		
		if( !states )
			return outForces;
		
		// Resize the outForces array to be as big as
		// the number of states we're analyzing.
		outForces.resize((*states).size()); // Forces match 1 to 1 with state indexes.
		
		// Loop through each edge and calculate force.
		for(unsigned int i=0; i<(unsigned int)edges.size(); ++i)
		{
			SpringEdge &edge = edges[i];
			
			// Useless edge.
			if( edge.stiffness == 0.0f && edge.damping == 0.0f )
				continue;
			
			// Invalid edge.
			if( !edge.VerifyIntegrity(&points, states) )
				continue;
			
			Cooney::Math::Vec3 forceA, forceB, dampingA, dampingB;

			// All force-calculation is done inside the edge class.
			edge.CalculateForces(forceA, forceB, dampingA, dampingB, &points, states);
			
			// Accumulate the forces.
			outForces[points[edge.pointAIndex].stateIndex] += forceA + dampingA;
			outForces[points[edge.pointBIndex].stateIndex] += forceB + dampingB;
		}
		
		// Loop through each corner brace and calculate force.
		for(unsigned int i=0; i<(unsigned int)braces.size(); ++i)
		{
			SpringCornerBrace &brace = braces[i];
			
			// Useless brace.
			if( brace.stiffness == 0.0f && brace.damping == 0.0f )
				continue;

			// Invalid brace.
			if( !brace.VerifyIntegrity(&points, states) )
				continue;
			
			Cooney::Math::Vec3 forceA, forceB, forceC, dampingA, dampingB, dampingC;

			// All force calculation is done inside the brace object.
			brace.CalculateForces(forceA, forceB, forceC, dampingA, dampingB, dampingC, &points, states);
			
			// Accumulate the forces.
			outForces[points[brace.pointAIndex].stateIndex] += forceA + dampingA;
			outForces[points[brace.pointBIndex].stateIndex] += forceB + dampingB;
			outForces[points[brace.pointCIndex].stateIndex] += forceC + dampingC;
		}
		
		// Apply uniform forces to all the points.
		for(unsigned int i=0; i<(unsigned int)points.size(); ++i)
		{
			SpringPoint &point = points[i];
			if( !point.VerifyIntegrity(states) )
				continue;
			
			if( point.bLocked )
			{
				// Locked points have no forces. Clear any
				// forces on a locked point.
				outForces[point.stateIndex] = Vector3(0,0,0);
				continue;
			}
			
			// Gravity.
			outForces[point.stateIndex] += Cooney::Math::Vector3(0.0f, -COONEY_PHYSICS_GRAVITY, 0.0f);
			// Wind.
			outForces[point.stateIndex] += Vector3(COONEY_PHYSICS_WIND);
		}
		
		return outForces;
	}

	void SoftBody::SubStep(std::vector<SpringPointState> *thisstates, std::vector<SpringPointState> *nextstates, std::vector<Cooney::Math::Vec3> *forces, float timeStep)
	{
		// Loop through every point and do an euler step.
		for(unsigned int i=0; i<(unsigned int)points.size(); ++i)
		{
			SpringPoint &point = points[i];
			if( !point.VerifyIntegrity(thisstates) )
				continue; // Invalid point.
			
			SpringPointState &thisstate = (*thisstates)[point.stateIndex]; // in
			SpringPointState &nextstate = (*nextstates)[point.stateIndex]; // out
			
			// F = MA -> A = F/M
			Cooney::Math::Vec3 acceleration = ((*forces)[point.stateIndex] / point.mass);
			
			// V1 = V0 + A0*dT
			nextstate.pointVelocity = thisstate.pointVelocity + acceleration * timeStep;
			// P1 = P0 + V0*dT
			nextstate.pointPosition += thisstate.pointPosition + thisstate.pointVelocity * timeStep;
		}
	}

	void SoftBody::Step(float timeStep)
	{
		std::vector<SpringPointState> nextstates;
		nextstates.resize(currentStates.size());
		
		switch(2)
		{
			case 1: // RK2
			{
				// RK2 integration
				// K1 = hF(x)
				// K2 = hF(x + (1/2)K1)
				// xnext = x + K2
				
				// Calculate the standard euler forces.
				std::vector<Cooney::Math::Vec3> k1Forces = CalculateForces(&currentStates, timeStep);

				// Do a pretend step, but only halfway.
				std::vector<SpringPointState> k1HalfStates;
				k1HalfStates.resize(currentStates.size());
				SubStep(&currentStates, &k1HalfStates, &k1Forces, timeStep/2.0f);
				
				// Determine the forces that the halfway pretend step would have on its next turn.
				std::vector<Cooney::Math::Vec3> k2Forces = CalculateForces(&k1HalfStates, timeStep/2.0f);
				
				// Use those pretend halfway forces for this whole step.
				SubStep(&currentStates, &nextstates, &k2Forces, timeStep);
			}
				break;
			case 2: // RK4
			{
				// RK4
				// K1 = hF(x, t)
				// K2 = hF(x + (1/2)K1, t + h/2)
				// K3 = hF(x + (1/2)k2, t + h/2)
				// K4 = hF(x + K3, t+ h)
				// xnext = x + 1/6(K1 + 2*k2 + 2*K3 + K4)
				
				// Calculate a full-step's forces.
				std::vector<Cooney::Math::Vec3> k1Forces = CalculateForces(&currentStates, timeStep);
				
				// Do a half-step.
				std::vector<SpringPointState> k1HalfStates;
				k1HalfStates.resize(currentStates.size());
				SubStep(&currentStates, &k1HalfStates, &k1Forces, timeStep/2.0f);
				// Use that half-step's state to calculate k2 forces from that position.
				std::vector<Cooney::Math::Vec3> k2Forces = CalculateForces(&k1HalfStates, timeStep/2.0f);
				
				// Do a half-step based on the k2 forces.
				std::vector<SpringPointState> k2HalfStates;
				k2HalfStates.resize(currentStates.size());
				SubStep(&currentStates, &k2HalfStates, &k2Forces, timeStep/2.0f);
				// Use the k2 half-step to generate k3 forces.
				std::vector<Cooney::Math::Vec3> k3Forces = CalculateForces(&k2HalfStates, timeStep/2.0f);
				
				// Do a full step based on the k3 forces.
				std::vector<SpringPointState> k3States;
				k3States.resize(currentStates.size());
				SubStep(&currentStates, &k3States, &k3Forces, timeStep);
				// Use the k3 step to determine the k4 forces.
				std::vector<Cooney::Math::Vec3> k4Forces = CalculateForces(&k3States, timeStep);
				
				// Blend all of the forces together.
				std::vector<Cooney::Math::Vec3> RK4Forces;
				RK4Forces.resize(k1Forces.size());
				for(unsigned int i=0; i<(unsigned int)RK4Forces.size(); ++i)
				{
					// RK4 = 1/6(K1 + 2*K2 + 2*K3 + K4)
					RK4Forces[i] = (k1Forces[i] + 2*k2Forces[i] + 2*k3Forces[i] + k4Forces[i]) / 6.0f;
				}
				
				// Do a full step with the RK4Forces.
				SubStep(&currentStates, &nextstates, &RK4Forces, timeStep);
				
			}
				break;
			default: // case 0: Euler
			{
				// Determine forces.
				std::vector<Cooney::Math::Vec3> stepForces = CalculateForces(&currentStates, timeStep);
				// Do a full step with the forces.
				SubStep(&currentStates, &nextstates, &stepForces, timeStep);
			}
				break;
		}
		
		// Basic collision. (Floor)
		// TODO: In a more robust system, implement full collision support.
		for(unsigned int i=0; i<(unsigned int)nextstates.size(); ++i)
		{
			SpringPointState &nextstate = nextstates[i];
			if( nextstate.pointPosition.y < 0.0f )
			{
				nextstate.pointPosition.y = 0.0f;
				nextstate.pointVelocity.y *= 0.0f;
				nextstate.pointVelocity.z -= nextstate.pointVelocity.z * 30.0f * timeStep;
				nextstate.pointVelocity.x -= nextstate.pointVelocity.x * 30.0f * timeStep;
			}
		}
		
		// Finished.
		currentStates = nextstates;
	}

	void SoftBody::PushGL()
	{
		// Render all of the edges first.
		glBegin(GL_LINES);
		glColor3f(1,0,0); // Edges are red.
		for(unsigned int i=0; i<(unsigned int)edges.size(); ++i)
		{
			if( edges[i].bHidden )
				continue; // Hidden edges are not rendered.
			if( !edges[i].VerifyIntegrity(&points, &currentStates) )
				continue; // Invalid point.
			
			Cooney::Math::Vec3 posA, posB;
			posA = currentStates[points[edges[i].pointAIndex].stateIndex].pointPosition;
			posB = currentStates[points[edges[i].pointBIndex].stateIndex].pointPosition;
			// Draw the line.
			glVertex3f(posA.x, posA.y, posA.z);
			glVertex3f(posB.x, posB.y, posB.z);
		}
		glEnd();
		
		// Render all of the points.
		glPointSize(5);
		glBegin(GL_POINTS);
		for(unsigned int i=0; i<(unsigned int)currentStates.size(); ++i)
		{
			// Locked points are drawn slightly
			// more dim than unlocked points.
			if( !points[i].bLocked )
				glColor3f(1,1,1);
			else
				glColor3f(1.0f, 0.5f, 0.5f);
			
			glVertex3f(currentStates[i].pointPosition.x, currentStates[i].pointPosition.y, currentStates[i].pointPosition.z);
		}
		glEnd();
	}
}