public class FabrikStructure3D implements FabrikStructure<FabrikChain3D,Vec3f>, Serializable
{
	private static final long serialVersionUID = 1L;
	
	// ---------- Private Properties ----------
	
	private static final String NEW_LINE = System.lineSeparator();
	
	/** The string name of this FabrikStructure3D - can be used for creating Maps, if required. */
	private String mName;
	
	/**
	 * The main substance of a FabrikStructure3D is a List of FabrikChain3D objects.
	 * <p>
	 * Each FabrikChain3D in the mChains vector is independent of all others, but shares the same target location as any/all other chains
	 * which exist in this structure.
	 */
	private List<FabrikChain3D> mChains = new ArrayList<>();

	// --------- Public Methods ----------

	/** Default constructor. */
	public FabrikStructure3D() { }
	
	/**
	 * Naming constructor.
	 * <p>
	 * Name lengths are truncated to a maximum of 100 characters, if necessary.
	 *  
	 *  @param	name	The name you wish to call the structure.
	 */
	public FabrikStructure3D(String name)
	{
		mName = Utils.getValidatedName(name);
	}

	/**
	 * Solve the structure for the given target location.
	 * <p>
	 * All chains in this structure are solved for the given target location EXCEPT those which have embedded targets enabled, which are
	 * solved for the target location embedded in the chain.
	 * <p>
	 * After this method has been executed, the configuration of all IK chains attached to this structure will have been updated.
	 *  
	 * @param   newTargetLocation	The location of the target for which we will attempt to solve all chains attached to this structure.
	 */
	@Override
	public void solveForTarget(Vec3f newTargetLocation)
	{
		int numChains = mChains.size();
		int connectedChainNumber;
		
		// Loop over all chains in this structure...
		for (int loop = 0; loop < numChains; ++loop)
		{
			// Get this chain, and get the number of the chain in this structure it's connected to (if any)
			FabrikChain3D thisChain = mChains.get(loop);
			connectedChainNumber    = thisChain.getConnectedChainNumber();
			
			// If this chain isn't connected to another chain then update as normal...
			if (connectedChainNumber == -1)
			{	
				thisChain.solveForTarget(newTargetLocation);
			}
			else // ...however, if this chain IS connected to another chain...
			{	
				// ... get the host chain and bone which this chain is connected to
				FabrikChain3D hostChain = mChains.get(connectedChainNumber);
				FabrikBone3D hostBone   = hostChain.getBone( thisChain.getConnectedBoneNumber() );
				if (hostBone.getBoneConnectionPoint() == BoneConnectionPoint.START) { thisChain.setBaseLocation( hostBone.getStartLocation() ); }
				else                                                                  { thisChain.setBaseLocation( hostBone.getEndLocation()   ); }
				
				// Now that we've clamped the base location of this chain to the start or end point of the bone in the chain we are connected to, it's
				// time to deal with any base bone constraints...
				
				// What type of base bone constraint is this (connected to another) chain using? 
				BaseboneConstraintType3D constraintType = thisChain.getBaseboneConstraintType();
				switch (constraintType)
				{
					// None or global basebone constraints? Nothing to do, because these will be handled in FabrikChain3D.solveIK() as we do not
					// need information from another chain to handle them.
					case NONE:         // Nothing to do because there's no basebone constraint
					case GLOBAL_ROTOR: // Nothing to do because the basebone constraint is not relative to bones in other chains in this structure
					case GLOBAL_HINGE: // Nothing to do because the basebone constraint is not relative to bones in other chains in this structure
						break;
						
					// If we have a local rotor or hinge constraint then we must calculate the relative basebone constraint before calling updateTarget
					case LOCAL_ROTOR:
					case LOCAL_HINGE: {
						// Get the direction of the bone this chain is connected to and create a rotation matrix from it.
						Mat3f connectionBoneMatrix = Mat3f.createRotationMatrix( hostBone.getDirectionUV() );
						
						// We'll then get the basebone constraint UV and multiply it by the rotation matrix of the connected bone 
						// to make the basebone constraint UV relative to the direction of bone it's connected to.
						Vec3f relativeBaseboneConstraintUV = connectionBoneMatrix.times( thisChain.getBaseboneConstraintUV() ).normalised();
							
						// Update our basebone relative constraint UV property
						thisChain.setBaseboneRelativeConstraintUV(relativeBaseboneConstraintUV);
						
						// Updat the relative reference constraint UV if we hav a local hinge
						if (constraintType == BaseboneConstraintType3D.LOCAL_HINGE)
						{
							thisChain.setBaseboneRelativeReferenceConstraintUV( connectionBoneMatrix.times( thisChain.getBone(0).getJoint().getHingeReferenceAxis() ) );
						}
						break;
					}	
					
					// No need for a default - constraint types are enums and we've covered them all.
				}
						
				// NOTE: If the base bone constraint type is NONE then we don't do anything with the base bone constraint of the connected chain.
				
				// Finally, update the target and solve the chain
				// Update the target and solve the chain
				if ( !thisChain.getEmbeddedTargetMode() )
				{
					thisChain.solveForTarget(newTargetLocation);	
				}
				else
				{
					thisChain.solveForEmbeddedTarget();
				}
				
			} // End of if chain is connected to another chain section
			
		} // End of loop over chains

	} // End of updateTarget method

	/**
	 * Add a FabrikChain3D to this FabrikStructure3D.
	 * <p>
	 * In effect, the chain is added to the mChains list of FabrikChain3D objects, and the mNumChains property is incremented.
	 * <p>
	 * Adding a chain using this method adds the chain to the structure, but does not connect it to any existing chain
	 * in the structure. If you wish to connect a chain, use one of the connectChain methods instead.
	 * <p>
	 * All chains in a structure share the same target, and all chains in the structure can be solved for the target location
	 * via a single call to updateTarget. 
	 *  
	 * @param	chain	(FabrikChain3D)	The FabrikChain3D to add to this structure.
	 * @see   #connectChain(FabrikChain3D, int, int)
	 * @see		#connectChain(FabrikChain3D, int, int, BoneConnectionPoint)
	 **/
	@Override
	public void addChain(FabrikChain3D chain)
	{
		mChains.add(chain);		
	}
	
	/**
	 * Remove a FabrikChain3D from this FabrikStructure3D by its index.
	 * <p>
	 * In effect, the chain is removed from the mChains list of FabrikChain3D objects, and the mNumChains property is decremented.
	 * 
	 * @param	chainIndex	The index of the chain to remove from the mChains list of FabrikChain3D objects.
	 **/
	public void removeChain(int chainIndex)
	{
		mChains.remove(chainIndex);		
	}
	
	/**
	 * Connect a chain to an existing chain in this structure.
	 * <p>
	 * Both chains and bones are are zero indexed.
	 * <p>
	 * If the existingChainNumber or existingBoneNumber specified to connect to does not exist in this structure
	 * then an IllegalArgumentExeception is thrown.
	 * 
	 * @param	newChain			The chain to connect to this structure
	 * @param	existingChainNumber	The index of the chain to connect the new chain to.
	 * @param	existingBoneNumber	The index of the bone to connect the new chain to within the existing chain.
	 */
	@Override
	public void connectChain(FabrikChain3D newChain, int existingChainNumber, int existingBoneNumber)
	{	
		// Does this chain exist? If not throw an IllegalArgumentException
		if (existingChainNumber > this.mChains.size())
		{
			throw new IllegalArgumentException("Cannot connect to chain " + existingChainNumber + " - no such chain (remember that chains are zero indexed).");
		}
		
		// Do we have this bone in the specified chain? If not throw an IllegalArgumentException
		if (existingBoneNumber > mChains.get(existingChainNumber).getNumBones() )
		{
			throw new IllegalArgumentException("Cannot connect to bone " + existingBoneNumber + " of chain " + existingChainNumber + " - no such bone (remember that bones are zero indexed).");
		}
			
		// Make a copy of the provided chain so any changes made to the original do not affect this chain
		FabrikChain3D relativeChain = new FabrikChain3D(newChain);
		
		// Connect the copy of the provided chain to the specified chain and bone in this structure
		relativeChain.connectToStructure(this, existingChainNumber, existingBoneNumber);
		
		// The chain as we were provided should be centred on the origin, so we must now make it
		// relative to the start location of the given bone in the given chain.
		
		
		// Get the connection point so we know to connect at the start or end location of the bone we're connecting to
		BoneConnectionPoint connectionPoint = this.getChain(existingChainNumber).getBone(existingBoneNumber).getBoneConnectionPoint();
		Vec3f connectionLocation;
		if (connectionPoint == BoneConnectionPoint.START)
		{
			connectionLocation = mChains.get(existingChainNumber).getBone(existingBoneNumber).getStartLocation();
		}
		else // If it's BoneConnectionPoint.END then we set the connection point to be the end location of the bone we're connecting to
		{
			connectionLocation = mChains.get(existingChainNumber).getBone(existingBoneNumber).getEndLocation();
		}		
		relativeChain.setBaseLocation(connectionLocation);
		
		// When we have a chain connected to a another 'host' chain, the chain is which is connecting in
		// MUST have a fixed base, even though that means the base location is 'fixed' to the connection
		// point on the host chain, rather than a static location.
		relativeChain.setFixedBaseMode(true);
		
		// Translate the chain we're connecting to the connection point
		for (int loop = 0; loop < relativeChain.getNumBones(); ++loop)
		{
			Vec3f origStart = relativeChain.getBone(loop).getStartLocation();
			Vec3f origEnd   = relativeChain.getBone(loop).getEndLocation();
			
			Vec3f translatedStart = origStart.plus(connectionLocation);
			Vec3f translatedEnd   = origEnd.plus(connectionLocation);
			
			relativeChain.getBone(loop).setStartLocation(translatedStart);
			relativeChain.getBone(loop).setEndLocation(translatedEnd);
		}
		
		this.addChain(relativeChain);
	}
	
	/**
	 * Connect a chain to an existing chain in this structure.
	 * <p>
	 * Both chains and bones are are zero indexed.
	 * <p>
	 * If the existingChainNumber or existingBoneNumber specified to connect to does not exist in this structure
	 * then an IllegalArgumentExeception is thrown.
	 * 
	 * @param	newChain		The chain to connect to this structure
	 * @param	existingChainNumber	The index of the chain to connect the new chain to.
	 * @param	existingBoneNumber	The index of the bone to connect the new chain to within the existing chain.
	 * @param	boneConnectionPoint	Whether the new chain should connect to the START or END of the specified bone in the specified chain.
	 */
	@Override
	public void connectChain(FabrikChain3D newChain, int existingChainNumber, int existingBoneNumber, BoneConnectionPoint boneConnectionPoint)
	{
		// Does this chain exist? If not throw an IllegalArgumentException
		if (existingChainNumber > this.mChains.size())
		{
			throw new IllegalArgumentException("Cannot connect to chain " + existingChainNumber + " - no such chain (remember that chains are zero indexed).");
		}
		
		// Do we have this bone in the specified chain? If not throw an IllegalArgumentException
		if (existingBoneNumber > mChains.get(existingChainNumber).getNumBones() )
		{
			throw new IllegalArgumentException("Cannot connect to bone " + existingBoneNumber + " of chain " + existingChainNumber + " - no such bone (remember that bones are zero indexed).");
		}
			
		// Make a copy of the provided chain so any changes made to the original do not affect this chain
		FabrikChain3D relativeChain = new FabrikChain3D(newChain);
		
		// Connect the copy of the provided chain to the specified chain and bone in this structure
		relativeChain.connectToStructure(this, existingChainNumber, existingBoneNumber);
		
		// The chain as we were provided should be centred on the origin, so we must now make it
		// relative to the start location of the given bone in the given chain.
		
		
		// Set the connection point and use it to get the connection location
		this.getChain(existingChainNumber).getBone(existingBoneNumber).setBoneConnectionPoint(boneConnectionPoint);
		Vec3f connectionLocation;
		if (boneConnectionPoint == BoneConnectionPoint.START)
		{
			connectionLocation = mChains.get(existingChainNumber).getBone(existingBoneNumber).getStartLocation();
		}
		else // If it's BoneConnectionPoint.END then we set the connection point to be the end location of the bone we're connecting to
		{
			connectionLocation = mChains.get(existingChainNumber).getBone(existingBoneNumber).getEndLocation();
		}		
		relativeChain.setBaseLocation(connectionLocation);
		
		// When we have a chain connected to a another 'host' chain, the chain is which is connecting in
		// MUST have a fixed base, even though that means the base location is 'fixed' to the connection
		// point on the host chain, rather than a static location.
		relativeChain.setFixedBaseMode(true);
		
		// Translate the chain we're connecting to the connection point
		for (int loop = 0; loop < relativeChain.getNumBones(); ++loop)
		{
			Vec3f origStart = relativeChain.getBone(loop).getStartLocation();
			Vec3f origEnd   = relativeChain.getBone(loop).getEndLocation();
			
			Vec3f translatedStart = origStart.plus(connectionLocation);
			Vec3f translatedEnd   = origEnd.plus(connectionLocation);
			
			relativeChain.getBone(loop).setStartLocation(translatedStart);
			relativeChain.getBone(loop).setEndLocation(translatedEnd);
		}
		
		this.addChain(relativeChain);
	}
	
	/**
	 * Return the number of chains in this structure.
	 *
	 * @return	The number of chains in this structure.
	 */
	@Override
	public int getNumChains() { return this.mChains.size(); }
	
	/**
	 * Return the specified chain from this structure.
	 * <p>
	 * Chain numbers are zero indexed. If the specified chain does not exist in this structure
	 * then an IllegalArgumentException is thrown.
	 * 
	 * @param	chainNumber	The specified chain from this structure.
	 * @return	The specified FabrikChain3D from this chain.
	 */
	@Override
	public FabrikChain3D getChain(int chainNumber) { return mChains.get(chainNumber); }
	
	/**
	 * Set the fixed base mode on all chains in this structure.
	 *
	 * @param	fixedBaseMode	Whether all chains should operate in fixed base mode (true) or not (false).
	 */
	public void setFixedBaseMode(boolean fixedBaseMode)
	{
		for (int loop = 0; loop < this.mChains.size(); ++loop)
		{
			mChains.get(loop).setFixedBaseMode(fixedBaseMode);
		}
	}
	
	/** 
	 * Set the name of this structure, capped to 100 characters if required.
	 * 
	 * @param	name	The name to set.
	 */
	@Override
	public void setName(String name) { mName = Utils.getValidatedName(name); }
	
	@Override
	public String getName() {
		return this.mName;
	}

	/**
	 * Return a concise, human readable description of this FabrikStructure3D. 
	 * <p>
	 * If further details on a specific chain are required, then you should get and print each chain individually.
	 *
	 * @return A concise, human readable description of this FabrikStructure3D.
	 */
	@Override
	public String toString()
	{
		StringBuilder sb = new StringBuilder();
		sb.append("----- FabrikStructure3D: " + mName + " -----" + NEW_LINE);
		
		sb.append("Number of chains: " + this.mChains.size() + NEW_LINE);

		for (int loop = 0; loop < this.mChains.size(); ++loop)
		{
			sb.append(mChains.get(loop).toString() );
		}
 
		return sb.toString();
	}

  @Override
  public int hashCode() {
    final int prime = 31;
    int result = 1;
    result = prime * result + ((mChains == null) ? 0 : mChains.hashCode());
    result = prime * result + ((mName == null) ? 0 : mName.hashCode());
    return result;
  }

  @Override
  public boolean equals(Object obj) {
    if (this == obj) {
      return true;
    }
    if (obj == null) {
      return false;
    }
    if (getClass() != obj.getClass()) {
      return false;
    }
    FabrikStructure3D other = (FabrikStructure3D) obj;
    if (mChains == null) {
      if (other.mChains != null) {
        return false;
      }
    } else if (!mChains.equals(other.mChains)) {
      return false;
    }
    if (mName == null) {
      if (other.mName != null) {
        return false;
      }
    } else if (!mName.equals(other.mName)) {
      return false;
    }
    return true;
  }

} // End of FabrikStructure3D class
 public abstract class CalikoDemoStructure3D implements CalikoDemoStructure {
	
	public static final Vec3f X_AXIS = new Vec3f(1.0f, 0.0f, 0.0f);
	public static final Vec3f Y_AXIS = new Vec3f(0.0f, 1.0f, 0.0f);	
	public static final Vec3f Z_AXIS = new Vec3f(0.0f, 0.0f, 1.0f);
	
	protected static Vec3f defaultBoneDirection   = new Vec3f(Z_AXIS).negated();
	protected static float defaultBoneLength      = 10.0f;
	protected static float boneLineWidth          = 5.0f;
	protected static float constraintLineWidth    = 2.0f;	
	protected static float baseRotationAmountDegs = 0.3f;	
	
	protected FabrikStructure3D structure;
	
	public FabrikStructure3D getStructure() {
		return structure;
	}
	

} public final class CalikoDemoStructure3DFactory {
	
	public static CalikoDemoStructure3D makeDemoStructure3D(int demoNumber) throws ReflectiveOperationException {
		CalikoDemoStructure3DEnum aDemo = findEnumForDemoNumber(demoNumber);
		if(aDemo !=null ) {
			return aDemo.getClazz().newInstance();
		}
		return null;
	}
	
	private static CalikoDemoStructure3DEnum findEnumForDemoNumber(int demoNumber) {
		for(CalikoDemoStructure3DEnum aDemo : CalikoDemoStructure3DEnum.values()) {
			if(aDemo.getDemoNumber() == demoNumber) {
				return aDemo;
			}
		}
		return null;
	}
	
	public enum CalikoDemoStructure3DEnum {
		UNCONSTRAINED_BONES(UnconstrainedBones.class),
		ROTORBALL_CONSTRAINED_BONES(RotorBallJointConstrainedBones.class),
		ROTOR_CONSTRAINED_BASEBONES(RotorConstrainedBaseBones.class),
		FREELY_ROTATING_GLOBAL_HINGES(FreelyRotatingGlobalHinges.class),
		GLOBAL_HINGES_WITH_REFERENCE_AXIS_CONSTRAINTS(GlobalHingesWithReferenceAxisConstraints.class),
		FREELY_ROTATING_LOCAL_HINGES(FreelyRotatingLocalHinges.class),
		LOCAL_HINGES_WITH_REFERENCE_AXIS_CONSTRAINTS(LocalHingesWithReferenceAxisConstraints.class),
		CONNECTED_CHAINS(ConnectedChains.class),
		GLOBAL_ROTOR_CONSTRAINED_CONNECTED_CHAINS(GlobalRotorConstrainedConnectedChains.class),
		LOCAL_ROTOR_CONSTRAINED_CONNECTED_CHAINS(LocalRotorConstrainedConnectedChains.class),
		CONNECTED_CHAINS_WITH_FREELY_ROTATING_GLOBAL_HINGES_BASEBONE_CONSTRAINTS(ConnectedChainsWithFreelyRotatingGlobalHingesBaseboneConstraints.class),
		CONNECTED_CHAINS_WITH_EMBEDDED_TARGETS(ConnectedChainsWithEmbeddedTargets.class);
		
		
		Class<? extends CalikoDemoStructure3D> clazz;
		
		private <T extends CalikoDemoStructure3D> CalikoDemoStructure3DEnum(Class<T> clazz) {
			this.clazz = clazz;
		}
		
		public Class<? extends CalikoDemoStructure3D> getClazz() {
			return this.clazz;
		}
		
		public int getDemoNumber() {
			return this.ordinal() + 1;
		}
	}

} public class Axis
{
	// ----- Static Properties -----
	
	// We're drawing 3 lines and each line has 2 vertices, which brings our total to 6 vertices
	private static final int NUM_VERTICES = 6;

	// Each vertex has three positional components - the x, y and z values. 
	private static final int VERTEX_COMPONENTS = 3;
	
	// Each vertex has four colour components - the red, green, blue and alpha values
	private static final int COLOUR_COMPONENTS = 4;

	// In combination, this gives us 7 floats per vertex - three for the XYZ and four for the RGBA
	private static final int COMPONENT_COUNT = VERTEX_COMPONENTS + COLOUR_COMPONENTS;
	
	// A single static ShaderProgram is used to draw all axes
	private static ShaderProgram axisShaderProgram;

	// Vertex shader source
	private static final String VERTEX_SHADER_SOURCE =
			"#version 330"                                                                  + Utils.NEW_LINE +
			"in vec3 vertexLocation;   // Incoming vertex attribute"                        + Utils.NEW_LINE +
			"in vec4 vertexColour;     // Incoming colour value"                            + Utils.NEW_LINE +
			"flat out vec4 fragColour; // Outgoing colour value"                            + Utils.NEW_LINE +
			"uniform mat4 mvpMatrix;   // Combined Model/View/Projection matrix"            + Utils.NEW_LINE +
			"void main(void) {"                                                             + Utils.NEW_LINE +
			"   fragColour = vertexColour;                         // Pass through colour"  + Utils.NEW_LINE +
			"	gl_Position = mvpMatrix * vec4(vertexLocation, 1); // Project our geometry" + Utils.NEW_LINE +
			"}";

	// Fragment shader source
	private static final String FRAGMENT_SHADER_SOURCE =
			"#version 330"                                                   + Utils.NEW_LINE +
			"flat in vec4 fragColour; // Incoming colour from vertex shader" + Utils.NEW_LINE +
			"out vec4 vOutputColour;  // Outgoing colour value"              + Utils.NEW_LINE +
			"void main() {"                                                  + Utils.NEW_LINE +
			"	vOutputColour = fragColour;"                                 + Utils.NEW_LINE +
			"}";

    // Hold id values for the Vertex Array Object (VAO) and Vertex Buffer Object (VBO)
	private static int axisVaoId;
	private static int axisVboId;
	
	/** The FloatBuffer which contains the ModelViewProjection matrix. */
	private static FloatBuffer mvpMatrixFB;
	
	// We'll keep track of and restore the current OpenGL line width, which we'll store in this FloatBuffer.
	// Note: Although we only need a single float for this, LWJGL insists upon a minimum size of 16 floats.
	private static FloatBuffer currentLineWidthFB;
	
	// The FloatBuffer which will contain our vertex data
	private static FloatBuffer vertexFloatBuffer;
	
	// ----- Non-Static Properties -----
	
	// The float array storing the axis vertex (including colour) data
	private float[] axisData;	

	// The line width with which to draw the axis lines
	private float mAxisLineWidth = 1.0f;
	
	static {
		vertexFloatBuffer  = Utils.createFloatBuffer(NUM_VERTICES * COMPONENT_COUNT);
		mvpMatrixFB        = Utils.createFloatBuffer(16);
		currentLineWidthFB = Utils.createFloatBuffer(16);

		// ----- Grid shader program setup -----

		axisShaderProgram = new ShaderProgram();
		axisShaderProgram.initFromStrings(VERTEX_SHADER_SOURCE, FRAGMENT_SHADER_SOURCE);

		// ----- Grid shader attributes and uniforms -----

		// Add the shader attributes and uniforms
		axisShaderProgram.addAttribute("vertexLocation");
		axisShaderProgram.addAttribute("vertexColour");
		axisShaderProgram.addUniform("mvpMatrix");

		// ----- Set up our Vertex Array Object (VAO) to hold the shader attributes -----

		// Create a VAO and bind to it
		axisVaoId = glGenVertexArrays();
		glBindVertexArray(axisVaoId);

		// ----- Vertex Buffer Object (VBO) -----

		// Create a VBO and bind to it
		axisVboId = glGenBuffers();
		glBindBuffer(GL_ARRAY_BUFFER, axisVboId);

		// Note: We do NOT copy the data into the buffer at this time - we do that on draw!
		
		// Vertex attribute configuration
		glVertexAttribPointer(axisShaderProgram.attribute("vertexLocation"), // Vertex location attribute index
				                                          VERTEX_COMPONENTS, // Number of location components per vertex
				                                                   GL_FLOAT, // Data type
				                                                      false, // Normalised?
				                              COMPONENT_COUNT * Float.BYTES, // Stride
				                                                         0); // Offset

		glVertexAttribPointer(axisShaderProgram.attribute("vertexColour"),  // Vertex colour attribute index
				                                        COLOUR_COMPONENTS,  // Number of colour components per vertex
			                                                     GL_FLOAT,  // Data type
				                                                    true,   // Normalised?
				                            COMPONENT_COUNT * Float.BYTES,  // Stride
				                      (long)VERTEX_COMPONENTS * Float.BYTES);  // Offset
		
		// Unbind VBO
		glBindBuffer(GL_ARRAY_BUFFER, 0);

		// Enable the vertex attributes
		glEnableVertexAttribArray(axisShaderProgram.attribute("vertexLocation"));
		glEnableVertexAttribArray(axisShaderProgram.attribute("vertexColour"));

		// Unbind VAO - all the buffer and attribute settings above will now be associated with our VAO
		glBindVertexArray(0);
	}
	
	// ----- Methods -----
	
	/**
	 * Construct an axis which will be draw with a given line length and line width.
	 * <p>
	 * If the axis length is less than zero or the axis line width is less than 1.0f then an IllegalArgumentException is thrown.
	 * 
	 * @param	axisLength		The length of the line representing the X/Y/Z axes.
	 * @param	axisLineWidth	The width of the line representing the X/Y/Z axes.
	 */
	public Axis(float axisLength, float axisLineWidth)
	{
		// Set the axis length and size - these methods throw IllegalArgumentExceptions if required
		setAxisLength(axisLength);
		setLineWidth(axisLineWidth);

		// Note: We cannot just transfer the data into the vertex buffer here once instead of per frame
		// because we may have multiple Axis objects, each with their own axis size.
	}

	/**
	 * Set the line width with which to draw this axis.
	 * <p>
	 * While many OpenGL implementations allow for varying line widths, only a line width of 1.0f is guaranteed to work.
	 * The maximum line width you can use depends on the OpenGL implementation.
	 * <p>
	 * If an axis line width of less than 1.0f is specified then an IllegalArgumentException is thrown.
	 * 
	 * @param	axisLineWidth	The width to draw the lines of the axis in pixels. 
	 */
	public void setLineWidth(float axisLineWidth)
	{
		if (axisLineWidth <  1.0f)
		{
			throw new IllegalArgumentException("Axis line width must be greater than 1.0f.");
		}
		mAxisLineWidth = axisLineWidth;
	}
	
	/**
	 * Set the length of the lines used to draw this axis object.
	 * 
	 * @param	axisLength	The desired axis line length.
	 */
	public void setAxisLength(float axisLength)
	{
		// Specify the length of each line in the axis 
		//                                x,             y,          z,    r,    g,    b,    a
		axisData = new float[] {       0.0f,       	  0.0f,       0.0f, 1.0f, 0.0f, 0.0f, 1.0f,   // Origin vertex - red
			                     axisLength,       	  0.0f,       0.0f, 1.0f, 0.0f, 0.0f, 1.0f,   // +X vertex     - red
		                               0.0f,          0.0f,       0.0f, 0.0f, 1.0f, 0.0f, 1.0f,   // Origin vertex - green
		                               0.0f,    axisLength,       0.0f, 0.0f, 1.0f, 0.0f, 1.0f,   // +Y vertex     - green
						               0.0f,          0.0f,       0.0f, 0.0f, 0.0f, 1.0f, 1.0f,   // Origin vertex - blue
						               0.0f,          0.0f, axisLength, 0.0f, 0.0f, 1.0f, 1.0f }; // +Z vertex     - blue
	}
	
	
	
	/**
	 * Draw an axis given a ModelViewProjection matrix.
	 * 
	 * @param mvpMatrix	The ModelViewProjection matrix used to draw the axis
	 * @see Mat4f#createPerspectiveProjectionMatrix(float, float, float, float)
	 * @see Mat4f#createPerspectiveProjectionMatrix(float, float, float, float, float, float)
	 */
	public void draw(Mat4f mvpMatrix)
	{
		// Enable our shader program and bind to our VAO
		axisShaderProgram.use();
		glBindVertexArray(axisVaoId);

		// Bind to our VBO so we can update the axis data for this particular axis object
		glBindBuffer(GL_ARRAY_BUFFER, axisVboId);

		// Copy the data for this particular axis into the vertex float buffer
		vertexFloatBuffer.put(axisData);
		vertexFloatBuffer.flip();
		glBufferData(GL_ARRAY_BUFFER, vertexFloatBuffer, GL_DYNAMIC_DRAW);

		// Provide the projection matrix uniform
		mvpMatrixFB.put( mvpMatrix.toArray() );
		mvpMatrixFB.flip();
		glUniformMatrix4fv(axisShaderProgram.uniform("mvpMatrix"), false, mvpMatrixFB);

		// Store the current GL_LINE_WIDTH
		// IMPORTANT: We MUST allocate a minimum of 16 floats in our FloatBuffer in LWJGL, we CANNOT just get a FloatBuffer with 1 float!
		// ALSO: glPushAttrib(GL_LINE_BIT); /* do stuff */ glPopAttrib(); should work instead of this in theory - but LWJGL fails with 'function not supported'.
		glGetFloatv(GL_LINE_WIDTH, currentLineWidthFB);

		/// Set the GL_LINE_WIDTH to be the width requested, as passed to the constructor
		glLineWidth(mAxisLineWidth);

		// 	Draw the axis lines
		glDrawArrays(GL_LINES, 0, NUM_VERTICES);

		// Reset the line width to the previous value
		glLineWidth( currentLineWidthFB.get(0) );

		// Unbind from our VBO
		glBindBuffer(GL_ARRAY_BUFFER, 0);

		// Unbind from our VAO
		glBindVertexArray(0);

		// Disable our shader program
		axisShaderProgram.disable();
	}
	
	/**
	 * Method to draw an axis at each bone in a structure.
	 * <p>
	 * The X axis is red, the Y axis is green and the Z axis (along which the axis is aligned with the bone) is blue.
	 * 
	 * @param	structure			The FabrikStructure3D to draw.
	 * @param	viewMatrix			The view matrix, typically extracted from the camera.
	 * @param	projectionMatrix	The projection matrix, typically extracted from the OpenGLWindow.
	 */
	public void draw(FabrikStructure3D structure, Mat4f viewMatrix, Mat4f projectionMatrix)	
	{	
		int numChains = structure.getNumChains();
		for (int chainLoop = 0; chainLoop < numChains; ++chainLoop)
		{	
			FabrikChain3D chain = structure.getChain(chainLoop);
			
			int numBones = chain.getNumBones();
			for (int boneLoop = 0; boneLoop < numBones; ++boneLoop)
			{	
				FabrikBone3D bone = chain.getBone(boneLoop);				
				Mat4f modelMatrix = new Mat4f( Mat3f.createRotationMatrix( bone.getDirectionUV() ), bone.getStartLocation() );				
				Mat4f mvpMatrix   = projectionMatrix.times(viewMatrix).times(modelMatrix);				
				draw(mvpMatrix);
			}
		}	
	}
	
} // End of Axis class public class Camera
{
	// ----- Static properties -----
	
	// Cardinal axes
	private static final Vec3f X_AXIS = new Vec3f(1.0f, 0.0f, 0.0f);
	private static final Vec3f Y_AXIS = new Vec3f(0.0f, 1.0f, 0.0f);
	private static final Vec3f Z_AXIS = new Vec3f(0.0f, 0.0f, 1.0f);
	
	// Conversion factor from degrees to radians
	private static final float DEGS_TO_RADS = (float)Math.PI / 180.0f;
	
	// Used to build up the movement from held keys in the move method
	private static final Vec3f movement = new Vec3f();
	
	// ----- Non-static properties -----
	
	// Our camera controls the view matrix
	private final Mat4f mViewMatrix = new Mat4f(1.0f);
	
	/** How quickly the camera moves.
	 * 
	 * @default 80.0f
	 */
	private float  mMovementSpeedFactor = 80.0f;
	
	/** How sensitive mouse movements affect looking up and down.
	 * 
	 * @default 0.1
	 */
	private double mPitchSensitivity = 0.1;
	
	/** How sensitive mouse movements affect looking left and right.
	 * 
	 * @default 0.1
	 */
	private double mYawSensitivity = 0.1;

	// Current location and orientation of the camera
	private Vec3f mLocation     = new Vec3f();
	private Vec3f mRotationDegs = new Vec3f();

	// Movement flags
	private boolean mHoldingForward     = false;
	private boolean mHoldingBackward    = false;
	private boolean mHoldingLeftStrafe  = false;
	private boolean mHoldingRightStrafe = false;

	// The middle of the screen in window coordinates
	private double mWindowMidX;
	private double mWindowMidY;
	
	// Location of mouse cursor as of last update
	private double mLastMouseX, mLastMouseY;

	// Constructor
	public Camera(Vec3f location, Vec3f rotationDegs, int windowWidth, int windowHeight)
	{
		// Set initial location and rotation values as per constructor arguments
		mLocation.set(location);
		mRotationDegs = rotationDegs;

		// When using the mouse to look around, we place the mouse cursor at the centre of the window each frame
		mWindowMidX = mLastMouseX = windowWidth  / 2.0;
		mWindowMidY = mLastMouseY = windowHeight / 2.0;
	}
	
	public Mat4f getViewMatrix()
	{
		// Reset to identity
		mViewMatrix.setIdentity();
		
		// Rotate to the orientation of the camera		
		mViewMatrix.rotateAboutLocalAxisDegs(mRotationDegs.x, X_AXIS);
		mViewMatrix.rotateAboutLocalAxisDegs(mRotationDegs.y, Y_AXIS);
		
		// Only rotate around Z if we have any Z-axis rotation - in FPS camera controls we do not.
		if (mRotationDegs.z > 0.0f)
		{
			mViewMatrix.rotateAboutLocalAxisDegs(mRotationDegs.z, Z_AXIS);
		}
		
		// Translate to our camera location and return the resulting matrix
		return mViewMatrix.translate( mLocation.negated() );
	}

	public void updateWindowSize(double windowWidth, double windowHeight)
	{
		// When using the mouse to look around, we place the mouse cursor at the centre of the window each frame
		mWindowMidX = mLastMouseX = windowWidth  / 2.0;
		mWindowMidY = mLastMouseY = windowHeight / 2.0;
	}
	
	// Method called when the LMB is released - this stops the camera 'jumping' on first mouse movement after LMB down
	public void resetLastCursorPosition() {	mLastMouseX = mWindowMidX; mLastMouseY = mWindowMidY; }

	// Method to set the movement flags depending on which keys are being pressed or released
	public void handleKeypress(int key, int action)
	{
		// Set the movement flags based on whether the key was pressed or released
		if ( key == GLFW_KEY_W ) { mHoldingForward     = (action == GLFW_PRESS || action == GLFW_REPEAT) ? true : false; }
		if ( key == GLFW_KEY_S ) { mHoldingBackward    = (action == GLFW_PRESS || action == GLFW_REPEAT) ? true : false; }
		if ( key == GLFW_KEY_A ) { mHoldingLeftStrafe  = (action == GLFW_PRESS || action == GLFW_REPEAT) ? true : false; }
		if ( key == GLFW_KEY_D ) { mHoldingRightStrafe = (action == GLFW_PRESS || action == GLFW_REPEAT) ? true : false; }
	}

	/**
	 * Method to deal with mouse position changes.
	 * <p>
	 * The pitch (up and down) and yaw (left and right) sensitivity of the camera can be modified via the setSensitivity method.
	 * 
	 * @param	mouseX	The x location of the mouse cursor.
	 * @param	mouseY	The y location of the mouse cursor.
	 */
	public void handleMouseMove(double mouseX, double mouseY)
	{
		// Calculate our horizontal and vertical mouse movement
		// Note: Swap the mouseX/Y and lastMouseX/Y to invert the direction of movement.
		double horizMouseMovement = (mouseX - mLastMouseX) * mYawSensitivity;
		double vertMouseMovement  = (mouseY - mLastMouseY) * mPitchSensitivity;
		
		// Keep the last mouse cursor location so we can tell the relative movement of the mouse
		// cursor the next time this method is called.
		mLastMouseX = mouseX;
		mLastMouseY = mouseY;
		
		// Apply the mouse movement to our rotation Vec3. The vertical (look up and down) movement is applied on
		// the X axis, and the horizontal (look left and right) movement is applied on the Y Axis
		mRotationDegs.x += vertMouseMovement;
		mRotationDegs.y += horizMouseMovement;

		// Limit looking up and down to vertically up and down
		if (mRotationDegs.x < -90.0f) { mRotationDegs.x = -90.0f; }
		if (mRotationDegs.x >  90.0f) { mRotationDegs.x = 90.0f;  }

		// Looking left and right - keep angles in the range 0.0 to 360.0
		// 0 degrees is looking directly down the negative Z axis "North", 90 degrees is "East", 180 degrees is "South", 270 degrees is "West"
		if (mRotationDegs.y < 0.0f  ) { mRotationDegs.y += 360.0f; }
		if (mRotationDegs.y > 360.0f) { mRotationDegs.y -= 360.0f; }		

		// Note: If you prefer to keep the angles in the range -180 to +180 use this code and comment out the 0 to 360 code above
		//if (rotation.y < -180.0f) { rotation.y += 360.0f; }
		//if (rotation.y >  180.0f) { rotation.y -= 360.0f; }		
	}

	/**
	 * Calculate which direction we need to move the camera and by what amount.
	 * <p>
	 * If you are not calculating frame duration then you may want to pass it 1.0f divided by the refresh rate of the monitor in use.
	 * 
	 * @param	deltaTime	The frame duration in milliseconds.
	 */
	public void move(float deltaTime)
	{
		// Zero our movement vector
		movement.zero();

		// Get the Sine and Cosine of our x and y axes
		// Note: Java's Math.sin/cos/tan etc. expects angles in radians. As we're working in degrees we must convert degrees to radians to get valid results
		float sinXRot = (float)Math.sin( Math.toRadians( mRotationDegs.x ) );
		float cosXRot = (float)Math.cos( Math.toRadians( mRotationDegs.x ) );

		float sinYRot = (float)Math.sin( Math.toRadians( mRotationDegs.y ) );
		float cosYRot = (float)Math.cos( Math.toRadians( mRotationDegs.y ) );

		float pitchLimitFactor = cosXRot; // This cancels out moving on the Z axis when we're looking up or down

		// Move appropriately depending on which key(s) are currently being held
		if (mHoldingForward)     { Vec3f.add(movement, new Vec3f(sinYRot * pitchLimitFactor, -sinXRot, -cosYRot * pitchLimitFactor) ); }
		if (mHoldingBackward)    { Vec3f.add(movement, new Vec3f(-sinYRot * pitchLimitFactor, sinXRot, cosYRot * pitchLimitFactor) );  }
		if (mHoldingLeftStrafe)  { Vec3f.subtract(movement, new Vec3f(cosYRot, 0.0f, sinYRot) );                                       }
		if (mHoldingRightStrafe) { Vec3f.add(movement, new Vec3f( cosYRot, 0.0f, sinYRot) );                                           }

		// If we have any movement at all, then normalise our movement vector
		if (movement.length() > 0.0f) { movement.normalise(); }

		// Apply our framerate-independent factor to our movement vector so that we move at the same speed
		// regardless of our framerate (assuming a correct frame duration is provided to this method).
		Vec3f.times(movement, mMovementSpeedFactor * deltaTime);

		// Finally, apply the movement to our position
		mLocation = mLocation.plus(movement);
	}

	// ----- Getters -----

	public Vec3f getLocation()     { return mLocation;       }
	public float getXLoc()         { return mLocation.x;     }
	public float getYLoc()         { return mLocation.y;     }
	public float getZLoc()         { return mLocation.z;     }

	public Vec3f getRotationDegs() { return mRotationDegs;   }
	public float getXRotDegs()     { return mRotationDegs.x; }
	public float getYRotDegs()     { return mRotationDegs.y; }
	public float getZRotDegs()     { return mRotationDegs.z; }

	public Vec3f getRotationRads() { return new Vec3f( getXRotRads(), getYRotRads(), getZRotRads() );  }
	public float getXRotRads()     { return mRotationDegs.x * Camera.DEGS_TO_RADS;                     }
	public float getYRotRads()     { return mRotationDegs.y * Camera.DEGS_TO_RADS;                     }
	public float getZRotRads()     { return mRotationDegs.z * Camera.DEGS_TO_RADS;                     }
	
	// ----- Setters -----
	
	/**
	 * Set the movement speed of the camera.
	 * <p>
	 * If this is set to zero, then the camera cannot move.
	 * <p>
	 * If a movement speed of less than zero is specified then an IllegalArgumentException is thrown.
	 * 
	 * @param	movementSpeed	The speed which the camera moves forward/back/left/right.
	 */
	public void setMovementSpeed(float movementSpeed)
	{
		if (movementSpeed <= 0.0f) { throw new IllegalArgumentException("The movement speed factor must be a positive value."); }
		mMovementSpeedFactor = movementSpeed;
	}
	
	/**
	 * Set the mouse look sensitivity of the camera.
	 * <p>
	 * Setting a value of zero to the pitch sensitivity will mean the camera cannot look up and down.
	 * Setting a value of zero to the yaw   sensitivity will mean the camera cannot look left and right.
	 * <p>
	 * It is allowable to set negative values to the pitch and yaw sensitivity values, which will result
	 * in the mouse direction 'looking' in the opposite direction - for example, a negative pitch sensitivity
	 * will result in 'flight-simulator' controls.
	 * 
	 * @param	pitchSensitivity	The pitch (up and down) sensitivity.
	 * @param	yawSensitivity		The yaw (left and right) sensitivity.
	 */
	public void setMouseLookSensitivity(double pitchSensitivity, double yawSensitivity)
	{
		mPitchSensitivity = pitchSensitivity;
		mYawSensitivity   = yawSensitivity;
	}

} // End of Camera class public class FabrikConstraint3D
{
	// Constant to convert degrees to radians
	private static final float DEGS_TO_RADS = (float)Math.PI / 180.0f;
	
	// Constraint colours
	private static final Colour4f ANTICLOCKWISE_CONSTRAINT_COLOUR = new Colour4f(1.0f, 0.0f, 0.0f, 1.0f);
	private static final Colour4f CLOCKWISE_CONSTRAINT_COLOUR     = new Colour4f(0.0f, 0.0f, 1.0f, 1.0f);	
	private static final Colour4f BALL_JOINT_COLOUR               = new Colour4f(1.0f, 0.0f, 0.0f, 1.0f);
	private static final Colour4f GLOBAL_HINGE_COLOUR             = new Colour4f(1.0f, 1.0f, 0.0f, 1.0f);
	private static final Colour4f LOCAL_HINGE_COLOUR              = new Colour4f(0.0f, 1.0f, 1.0f, 1.0f);
	private static final Colour4f REFERENCE_AXIS_COLOUR           = new Colour4f(1.0f, 0.0f, 1.0f, 1.0f);
	
	// The drawn length of the rotor cone and the radius of the cone and circle describing the hinge axes
	private static final float CONE_LENGTH_FACTOR  = 0.3f;
	private static final float RADIUS_FACTOR       = 0.25f;	
	private static final int NUM_CONE_LINES        = 12;
	
	private static float rotStep = 360.0f / (float)NUM_CONE_LINES;

	private Circle3D mCircle; // Used to draw hinges
	private Line3D   mLine;   // Used to draw hinge axes, reference axes and ball-joint cones

	private boolean initialised = false;

	/** Default constructor. */
	public FabrikConstraint3D()
	{
		if (!initialised)
		{
			initialised = true;			
			mCircle      = new Circle3D();
			mLine        = new Line3D();
		}
	}	

	/**
	 * Draw the constraint for this FabrikBone3D.
	 * <p>
	 * Ball joints are drawn as a series of lines forming a cone, while global and local hinge joints are drawn
	 * as circles aligned to the hinge rotation axis, with an optional reference axis within the plane of the
	 * circle if required.
	 * <p>
	 * Line widths may commonly be between 1.0f and 32.0f, values outside of this range may result in unspecified behaviour.
	 * 
	 * @param	bone		The FabrikBone3D object to draw the constraint about.
	 * @param	referenceDirection As bones are constrained about the direction relative to the previous bone in the chain, this is the direction of the previous bone.
	 * @param	lineWidth	The width of the line used to draw the rotor constraint in pixels.
	 * @param	colour		The colour of the line.
	 * @param	mvpMatrix	The ModelViewProjection matrix with which to draw the circle.
	 */
	private void draw(FabrikBone3D bone, Vec3f referenceDirection, float lineWidth, Mat4f mvpMatrix)
	{
		float boneLength = bone.length();
		Vec3f lineStart = bone.getStartLocation();
		switch ( bone.getJointType() )
		{
			case BALL: {				
				float constraintAngleDegs = bone.getBallJointConstraintDegs();
				
				// If the ball joint constraint is 180 degrees then it's not really constrained, so we won't draw it
				if ( Utils.approximatelyEquals(constraintAngleDegs, 180.0f, 0.01f) ) { return; }
				
				// The constraint direction is the direction of the previous bone rotated about a perpendicular axis by the constraint angle of this bone
				Vec3f constraintDirection = Vec3f.rotateAboutAxisDegs(referenceDirection, constraintAngleDegs, Vec3f.genPerpendicularVectorQuick(referenceDirection) ).normalised();
				
				// Draw the lines about the the bone (relative to the reference direction)				
				Vec3f lineEnd;
				for (int loop = 0; loop < NUM_CONE_LINES; ++loop)
				{	
					lineEnd = lineStart.plus( constraintDirection.times(boneLength * CONE_LENGTH_FACTOR) );					
					constraintDirection = Vec3f.rotateAboutAxisDegs(constraintDirection, rotStep, referenceDirection).normalised();					
					mLine.draw(lineStart, lineEnd, BALL_JOINT_COLOUR, lineWidth, mvpMatrix);
				}
				
				// Draw the circle at the top of the cone
				float pushDistance = (float)Math.cos(constraintAngleDegs * DEGS_TO_RADS) * boneLength;
				float radius       = (float)Math.sin(constraintAngleDegs * DEGS_TO_RADS) * boneLength;				
				Vec3f circleCentre = lineStart.plus( referenceDirection.times(pushDistance * CONE_LENGTH_FACTOR) );
				mCircle.draw(circleCentre, referenceDirection, radius * CONE_LENGTH_FACTOR, BALL_JOINT_COLOUR, lineWidth, mvpMatrix);
				break;
			}

			case GLOBAL_HINGE: {
				// Get the hinge rotation axis and draw the circle describing the hinge rotation axis
				Vec3f hingeRotationAxis = bone.getJoint().getHingeRotationAxis();				
				float radius = boneLength * RADIUS_FACTOR;
				mCircle.draw(lineStart, hingeRotationAxis, radius, GLOBAL_HINGE_COLOUR, lineWidth, mvpMatrix);
				
				// Note: While ACW rotation is negative and CW rotation about an axis is positive, we store both 
				// of these as positive values between the range 0 to 180 degrees, as such we'll negate the
				// clockwise rotation value for it to turn in the correct direction.
				float anticlockwiseConstraintDegs =  bone.getHingeJointAnticlockwiseConstraintDegs();
				float clockwiseConstraintDegs     = -bone.getHingeJointClockwiseConstraintDegs(); 
				
				// If both the anticlockwise (positive) and clockwise (negative) constraint angles are not 180 degrees (i.e. we
				// are constraining the hinge about a reference direction which lies in the plane of the hinge rotation axis)...
				if ( !Utils.approximatelyEquals(anticlockwiseConstraintDegs, 180.0f, 0.01f) &&
				     !Utils.approximatelyEquals(    clockwiseConstraintDegs, 180.0f,  0.01f) )
				{	
					Vec3f hingeReferenceAxis = bone.getJoint().getHingeReferenceAxis();
					
					// ...then draw the hinge reference axis and ACW/CW constraints about it.
					mLine.draw(lineStart, lineStart.plus( hingeReferenceAxis.times(boneLength * RADIUS_FACTOR) ), REFERENCE_AXIS_COLOUR, lineWidth, mvpMatrix);
					
					Vec3f anticlockwiseDirection = Vec3f.rotateAboutAxisDegs(hingeReferenceAxis, anticlockwiseConstraintDegs, hingeRotationAxis);
					Vec3f clockwiseDirection     = Vec3f.rotateAboutAxisDegs(hingeReferenceAxis, clockwiseConstraintDegs, hingeRotationAxis);   
					Vec3f anticlockwisePoint = lineStart.plus( anticlockwiseDirection.times(radius) );
					Vec3f clockwisePoint     = lineStart.plus( clockwiseDirection.times(radius)     );
					mLine.draw(lineStart, anticlockwisePoint, ANTICLOCKWISE_CONSTRAINT_COLOUR, lineWidth, mvpMatrix);
					mLine.draw(lineStart,     clockwisePoint,     CLOCKWISE_CONSTRAINT_COLOUR, lineWidth, mvpMatrix);
				}
				break;
			}

			case LOCAL_HINGE: {				
				// Construct a rotation matrix based on the reference direction (i.e. the previous bone's direction)...
				Mat3f m = Mat3f.createRotationMatrix(referenceDirection);
				
				// ...and transform the hinge rotation axis into the previous bone's frame of reference
				Vec3f relativeHingeRotationAxis  = m.times( bone.getJoint().getHingeRotationAxis() ).normalise();
				
				// Draw the circle describing the hinge rotation axis
				float radius = boneLength * RADIUS_FACTOR;
				mCircle.draw(lineStart, relativeHingeRotationAxis, radius, LOCAL_HINGE_COLOUR, lineWidth, mvpMatrix);
															
				// Draw the hinge reference and clockwise/anticlockwise constraints if necessary
				float anticlockwiseConstraintDegs =  bone.getHingeJointAnticlockwiseConstraintDegs();
				float clockwiseConstraintDegs     = -bone.getHingeJointClockwiseConstraintDegs(); 
				if ( !Utils.approximatelyEquals(anticlockwiseConstraintDegs, 180.0f, 0.01f) &&
				     !Utils.approximatelyEquals(    clockwiseConstraintDegs, 180.0f, 0.01f) )
				{	
					// Get the relative hinge rotation axis and draw it...
					bone.getJoint().getHingeReferenceAxis().projectOntoPlane(relativeHingeRotationAxis);
					
					Vec3f relativeHingeReferenceAxis = m.times(bone.getJoint().getHingeReferenceAxis()).normalise();
					
					mLine.draw(lineStart, lineStart.plus( relativeHingeReferenceAxis.times(radius) ), REFERENCE_AXIS_COLOUR, lineWidth, mvpMatrix);
					
					// ...as well as the clockwise and anticlockwise constraints.
					Vec3f anticlockwiseDirection = Vec3f.rotateAboutAxisDegs(relativeHingeReferenceAxis, anticlockwiseConstraintDegs, relativeHingeRotationAxis);
					Vec3f clockwiseDirection     = Vec3f.rotateAboutAxisDegs(relativeHingeReferenceAxis, clockwiseConstraintDegs, relativeHingeRotationAxis);   
					Vec3f anticlockwisePoint = lineStart.plus( anticlockwiseDirection.times(radius) );
					Vec3f clockwisePoint     = lineStart.plus( clockwiseDirection.times(radius) );
					mLine.draw(lineStart, anticlockwisePoint, ANTICLOCKWISE_CONSTRAINT_COLOUR, lineWidth, mvpMatrix);
					mLine.draw(lineStart,     clockwisePoint,     CLOCKWISE_CONSTRAINT_COLOUR, lineWidth, mvpMatrix);
				}
				break;
			}
			
		} // End of switch statement
	}
	
	/**
	 * Draw the constraints on all bones in a FabrikChain3D object.
	 * 
	 * Line widths may commonly be between 1.0f and 32.0f, values outside of this range may result in unspecified behaviour.
	 * 
	 * @param chain			The chain to use.
	 * @param lineWidth		The width of the lines to draw.
	 * @param mvpMatrix		The ModelViewProjection matrix to use.
	 */
	public void draw(FabrikChain3D chain, float lineWidth, Mat4f mvpMatrix)
	{
		int numBones = chain.getNumBones();
		if (numBones > 0)
		{
			// Draw the base bone, using the constraint UV as the relative direction
			switch ( chain.getBaseboneConstraintType() )
			{
				case NONE:
					break;
			
				case GLOBAL_ROTOR: 
				case GLOBAL_HINGE:
					draw( chain.getBone(0), chain.getBaseboneConstraintUV(), lineWidth, mvpMatrix );
					break;
				
				case LOCAL_ROTOR:
				case LOCAL_HINGE:					
					// If the structure hasn't been solved yet then we won't have a relative basebone constraint which we require
					// to draw the constraint itself - so our best option is to simply not draw the constraint until we can. 
					if (chain.getBaseboneRelativeConstraintUV().length() > 0.0f) 
					{
						draw( chain.getBone(0), chain.getBaseboneRelativeConstraintUV(), lineWidth, mvpMatrix );
					}
					break;
			
				// No need for a default - constraint types are enums and we've covered them all.
			}
				
			// Draw all the bones AFTER the base bone, using the previous bone direction as the relative direction
			for (int loop = 1; loop < numBones; ++loop)
			{
				draw( chain.getBone(loop), chain.getBone(loop-1).getDirectionUV(), lineWidth, mvpMatrix );
			}
		}
	}	

	/**
	 * Draw the constraints on all bones in a FabrikChain3D object using the default line width.
	 * 
	 * If the chain does not contain any bones then an IllegalArgumentException is thrown.
	 * 
	 * @param chain			The chain to use.
	 * @param mvpMatrix		The ModelViewProjection matrix to use.
	 */
	public void draw(FabrikChain3D chain, Mat4f mvpMatrix) { draw(chain, 1.0f, mvpMatrix); }
	
	/**
	 * Draw the constraints on all chains and all bones in each chain of a FabrikStructure3D object.
	 * 
	 * If any chain in the structure does not contain any bones then an IllegalArgumentException is thrown.
	 * Line widths may commonly be between 1.0f and 32.0f, values outside of this range may result in unspecified behaviour.
	 * 
	 * @param structure		The structure to use.
	 * @param lineWidth		The width of the lines to draw.
	 * @param mvpMatrix		The ModelViewProjection matrix to use.
	 */
	public void draw(FabrikStructure3D structure, float lineWidth, Mat4f mvpMatrix)
	{
		int numChains = structure.getNumChains();
		for (int loop = 0; loop < numChains; ++loop)
		{
			draw( structure.getChain(loop), lineWidth, mvpMatrix );
		}
	}
	
	/**
	 * Draw the constraints on all chains and all bones in each chain of a FabrikStructure3D object using the default line width.
	 * 
	 * If any chain in the structure does not contain any bones then an IllegalArgumentException is thrown.
	 * 
	 * @param structure		The structure to use.
	 * @param mvpMatrix		The ModelViewProjection matrix to use.
	 */
	public void draw(FabrikStructure3D structure, Mat4f mvpMatrix) { draw(structure, 1.0f, mvpMatrix); }
	
} // End of FabrikConstraint3D class public class FabrikLine3D
{	
	// A static Line3D object used to draw lines.
	private static Line3D line = new Line3D();
	
	private FabrikLine3D() {}
	
	// ---------- Bone Drawing Methods ----------
	
	/**
	 * Draw this bone as a line.
	 * <p>
	 * The line will be drawn in the colour and line width as taken from the bone.
	 * 
	 * @param	bone		The bone to draw.
	 * @param	mvpMatrix	The ModelViewProjection matrix with which to draw the bone.
	 * @see		FabrikBone3D#setColour(Colour4f)
	 * @see		FabrikBone3D#setLineWidth(float)
	 * @see		Mat4f#createPerspectiveProjectionMatrix(float, float, float, float)
	 * @see		Mat4f#createPerspectiveProjectionMatrix(float, float, float, float, float, float)
	 */
	public static void draw(FabrikBone3D bone, Mat4f mvpMatrix)
	{
		line.draw(bone.getStartLocation(), bone.getEndLocation(), bone.getColour(), bone.getLineWidth(), mvpMatrix);
	}	
	
	/**
	 * Draw this bone as a line using a specific line width.
	 * <p>
	 * The line will be drawn in the colour as taken from the bone.
	 * 
	 * @param	bone		The bone to draw. 
	 * @param	lineWidth	The width of the line use to draw the bone.
	 * @param	mvpMatrix	The ModelViewProjection matrix with which to draw the bone.
	 */
	public static void draw(FabrikBone3D bone, float lineWidth, Mat4f mvpMatrix)
	{
		FabrikLine3D.line.draw(bone.getStartLocation(), bone.getEndLocation(), bone.getColour(), lineWidth, mvpMatrix);
	}

	/**
	 * Draw this bone as a line using a specific colour and line width.
	 * <p>
	 * The line will be drawn in the colour and line width as taken from the provided arguments.
	 * 
	 * @param	bone		The bone to draw.
	 * @param	colour		The colour to draw the bone.
	 * @param	lineWidth	The width of the line use to draw the bone.
	 * @param	mvpMatrix	The ModelViewProjection matrix with which to draw the bone.
	 * @see		Mat4f#createOrthographicProjectionMatrix(float, float, float, float, float, float)
	 */
	public static void draw(FabrikBone3D bone, Colour4f colour, float lineWidth, Mat4f mvpMatrix)
	{
		FabrikLine3D.line.draw(bone.getStartLocation(), bone.getEndLocation(), colour, lineWidth, mvpMatrix);
	}
	
	// ---------- Chain Drawing Methods ----------
	
	/**
	 * Draw the provided IK chain as a series of lines using the colour and line width properties of each bone.
	 * 
	 * @param	chain		The chain to draw.
	 * @param	mvpMatrix	The ModelViewProjection matrix with which to draw the chain.
	 * @see		Mat4f#createOrthographicProjectionMatrix(float, float, float, float, float, float)
	 */
	public static void draw(FabrikChain3D chain, Mat4f mvpMatrix)
	{
		int numBones = chain.getNumBones();
		for (int loop = 0; loop < numBones; ++loop)
		{
			draw(chain.getBone(loop), mvpMatrix);
		}
	}

	/**
	 * Draw a FabrikChain3D as a series of lines.
	 * <p>
	 * Each line is drawn using the specified line width, with the colour taken from the bone's
	 * {@link FabrikBone3D#mColour} property.
	 * 
	 * @param	chain		The chain to draw. 
	 * @param	lineWidth	The width of the line use to draw the bone..
	 * @param	mvpMatrix	The ModelViewProjection matrix with which to draw the chain.
	 */
	public static void draw(FabrikChain3D chain, float lineWidth, Mat4f mvpMatrix)
	{
		int numBones = chain.getNumBones();
		for (int loop = 0; loop < numBones; ++loop)
		{
			FabrikLine3D.draw(chain.getBone(loop), lineWidth, mvpMatrix);
		}
	}	

	// ---------- Structure Drawing Methods ----------
	
	/**
	 * Draw the provided FabrikStructure3D by drawing each chain in this structure.
	 * <p>
	 * All bones in all chains are drawn with line widths and colours as per their member properties.
	 * 
	 * @param	structure	The structure to draw.
	 * @param	mvpMatrix	The ModelViewProjection matrix with which to draw the structure.
	 * @see		Mat4f#createOrthographicProjectionMatrix(float, float, float, float, float, float)
	 */
	public static void draw(FabrikStructure3D structure, Mat4f mvpMatrix)
	{
		int numChains = structure.getNumChains();
		for (int loop = 0; loop < numChains; ++loop)
		{
			draw(structure.getChain(loop), mvpMatrix);
		}
	}
	
	/**
	 * Draw the provided FabrikStructure3D by drawing each chain in this structure.
	 * <p>
	 * All bones in all chains are draw with colours as per their member properties, but with the provided line width.
	 * 
	 * @param	structure	The structure to draw.
	 * @param	lineWidth	The width of the line use to draw the structure.
	 * @param	mvpMatrix	The ModelViewProjection matrix with which to draw the structure.
	 * @see		Mat4f#createOrthographicProjectionMatrix(float, float, float, float, float, float)
	 */
	public static void draw(FabrikStructure3D structure, float lineWidth, Mat4f mvpMatrix)
	{
		int numChains = structure.getNumChains();
		for (int loop = 0; loop < numChains; ++loop)
		{
			draw(structure.getChain(loop), lineWidth, mvpMatrix);
		}
	}
	
} // End of FabrikLine3D class public class FabrikModel3D
{	
	// Each vertex has three positional components - the x, y and z values. 
	private static final int VERTEX_COMPONENTS = 3;
	
	// A single static ShaderProgram is used to draw all axes
	private static ShaderProgram shaderProgram;
	
	// Vertex shader source
	private static final String VERTEX_SHADER_SOURCE =
			"#version 330"                                                                    + Utils.NEW_LINE +
			"in vec3 vertexLocation;   // Incoming vertex attribute"                          + Utils.NEW_LINE +
			"uniform mat4 mvpMatrix;   // Combined Model/View/Projection matrix  "            + Utils.NEW_LINE +
			"void main(void) {"                                                               + Utils.NEW_LINE +
			"	gl_Position = mvpMatrix * vec4(vertexLocation, 1.0); // Project our geometry" + Utils.NEW_LINE +
			"}";

	// Fragment shader source
	private static final String FRAGMENT_SHADER_SOURCE =
			"#version 330"              + Utils.NEW_LINE +
			"out vec4 outputColour;"    + Utils.NEW_LINE +
			"uniform vec4 colour;"      + Utils.NEW_LINE +
			"void main() {"             + Utils.NEW_LINE +
			"	outputColour = colour;" + Utils.NEW_LINE +
			"}";

    // Hold id values for the Vertex Array Object (VAO) and Vertex Buffer Object (VBO)
	private static int vaoId;
	private static int vboId;
	
	// Float buffers for the ModelViewProjection matrix and model colour
	private static FloatBuffer mvpMatrixFB;
	private static FloatBuffer colourFB;
	
	// We'll keep track of and restore the current OpenGL line width, which we'll store in this FloatBuffer.
	// Note: Although we only need a single float for this, LWJGL insists upon a minimum size of 16 floats.
	private static FloatBuffer currentLineWidthFB;	
	
	// ----- Non-Static Properties -----
	
	// The FloatBuffer which will contain our vertex data - as we may load multiple different models this cannot be static
	private FloatBuffer vertexFB;
		
	/** The actual Model associated with this FabrikModel3D. */
	private Model model;
	
	/** The float array storing the axis vertex (including colour) data. */
	private float[] modelData;	

	/**
	 * The line width with which to draw the model in pixels.
	 * 
	 * @default	1.0f
	 */
	private float mLineWidth = 1.0f;
	
	static {
		mvpMatrixFB        = Utils.createFloatBuffer(16);
		colourFB           = Utils.createFloatBuffer(16);
		currentLineWidthFB = Utils.createFloatBuffer(16);

		// ----- Grid shader program setup -----

		shaderProgram = new ShaderProgram();
		shaderProgram.initFromStrings(VERTEX_SHADER_SOURCE, FRAGMENT_SHADER_SOURCE);

		// ----- Grid shader attributes and uniforms -----

		// Add the shader attributes and uniforms
		shaderProgram.addAttribute("vertexLocation");			
		shaderProgram.addUniform("mvpMatrix");
		shaderProgram.addUniform("colour");

		// ----- Set up our Vertex Array Object (VAO) to hold the shader attributes -----

		// Create a VAO and bind to it
		vaoId = glGenVertexArrays();
		glBindVertexArray(vaoId);

		// ----- Vertex Buffer Object (VBO) -----

		// Create a VBO and bind to it
		vboId = glGenBuffers();
		glBindBuffer(GL_ARRAY_BUFFER, vboId);

		// Note: We do NOT copy the data into the buffer at this time - we do that on draw!
		
		// Vertex attribute configuration
		glVertexAttribPointer(shaderProgram.attribute("vertexLocation"), // Vertex location attribute index
				                                      VERTEX_COMPONENTS, // Number of components per vertex
				                                               GL_FLOAT, // Data type
				                                                  false, // Normalised?
                                            VERTEX_COMPONENTS * Float.BYTES, // Stride
				                                                    0); // Offset
		
		// Unbind VBO
		glBindBuffer(GL_ARRAY_BUFFER, 0);

		// Enable the vertex attributes
		glEnableVertexAttribArray(shaderProgram.attribute("vertexLocation"));

		// Unbind VAO - all the buffer and attribute settings above will now be associated with our VAO
		glBindVertexArray(0);			
	}
	
	/**
	 * Default constructor.
	 * 
	 * @param	modelFilename	The filename of the model to load.
	 * @param	lineWidth		The width of the lines used to draw the model in pixels.
	 */
	// Note: width is along +/- x-axis, depth is along +/- z-axis, height is the location on
	// the y-axis, numDivisions is how many lines to draw across each axis
	public FabrikModel3D(String modelFilename, float lineWidth)
	{	
		// Load the model, get the vertex data and put it into our vertex FloatBuffer
		model = new Model(modelFilename);
		modelData = model.getVertexFloatArray();
		vertexFB = Utils.createFloatBuffer(model.getNumVertices() * VERTEX_COMPONENTS);
		
		mLineWidth = lineWidth;
		
	} // End of constructor

	/** Private method to actually draw the model. */
	private void drawModel(float lineWidth, Colour4f colour, Mat4f mvpMatrix)
	{
		// Enable our shader program and bind to our VAO
		shaderProgram.use();
		glBindVertexArray(vaoId);

		// Bind to our VBO so we can update the axis data for this particular axis object
		glBindBuffer(GL_ARRAY_BUFFER, vboId);

		// Copy the data for this particular model into the vertex float buffer
		// Note: The model is scaled to each individual bone length, hence the GL_DYNAMIC_DRAW performance hint.
		vertexFB.put(modelData);
		vertexFB.flip();
		glBufferData(GL_ARRAY_BUFFER, vertexFB, GL_DYNAMIC_DRAW);

		// Provide the mvp matrix uniform data
		mvpMatrixFB.put( mvpMatrix.toArray() );
		mvpMatrixFB.flip();
		glUniformMatrix4fv(shaderProgram.uniform("mvpMatrix"), false, mvpMatrixFB);
		
		// Provide the model vertex colour data
		colourFB.put( colour.toArray() );
		colourFB.flip();
		glUniform4fv(shaderProgram.uniform("colour"), colourFB);

		// Store the current GL_LINE_WIDTH
		// IMPORTANT: We MUST allocate a minimum of 16 floats in our FloatBuffer in LWJGL, we CANNOT just get a FloatBuffer with 1 float!
		// ALSO: glPushAttrib(GL_LINE_BIT); /* do stuff */ glPopAttrib(); should work instead of this in theory - but LWJGL fails with 'function not supported'.
		glGetFloatv(GL_LINE_WIDTH, currentLineWidthFB);

		/// Set the GL_LINE_WIDTH to be the width requested, as passed to the constructor
		glLineWidth(lineWidth);

		// 	Draw the model as lines
		glDrawArrays( GL_LINES, 0, model.getNumVertices() );

		// Reset the line width to the previous value
		glLineWidth( currentLineWidthFB.get(0) );

		// Unbind from our VBO
		glBindBuffer(GL_ARRAY_BUFFER, 0);

		// Unbind from our VAO
		glBindVertexArray(0);

		// Disable our shader program
		shaderProgram.disable();
	}
	
	/**
	 * Draw a bone using the model loaded on this FabrikModel3D.
	 * 
	 * @param	bone				The bone to draw.
	 * @param	viewMatrix			The view matrix, typically retrieved from the camera.
	 * @param	projectionMatrix	The projection matrix of our scene.
	 * @param	colour				The colour of the lines used to draw the bone.
	 */
	public void drawBone(FabrikBone3D bone, Mat4f viewMatrix, Mat4f projectionMatrix, Colour4f colour)	
	{	
		// Clone the model and scale the clone to be twice as wide and deep, and scaled along the z-axis to match the bone length
		Model modelCopy = Model.clone(model);
		modelCopy.scale( 2.0f, 2.0f, bone.length() );
		
		// Get our scaled model data
		modelData = modelCopy.getVertexFloatArray();
		
		// Construct a model matrix for this bone
		Mat4f modelMatrix = new Mat4f( Mat3f.createRotationMatrix( bone.getDirectionUV().normalised() ), bone.getStartLocation() );
		
		// Construct a ModelViewProjection and draw the model for this bone
		Mat4f mvpMatrix = projectionMatrix.times(viewMatrix).times(modelMatrix);				
		this.drawModel(mLineWidth, colour, mvpMatrix);
	}
	
	/**
	 * Draw a bone using the model loaded on this FabrikModel3D using a default colour of white at full opacity.
	 * 
	 * @param	bone				The bone to draw.
	 * @param	viewMatrix			The view matrix, typically retrieved from the camera.
	 * @param	projectionMatrix	The projection matrix of our scene.
	 */
	public void drawBone(FabrikBone3D bone, Mat4f viewMatrix, Mat4f projectionMatrix)	
	{	
		this.drawBone(bone, viewMatrix, projectionMatrix, Utils.WHITE);
	}
	
	/**
	 * Draw a chain using the model loaded on this FabrikModel3D.
	 * 
	 * @param	chain				The FabrikChain3D to draw the model as bones on.
	 * @param	viewMatrix			The view matrix, typically retrieved from the camera.
	 * @param	projectionMatrix	The projection matrix of our scene.
	 * @param	colour				The colour of the lines used to draw the model.
	 */
	public void drawChain(FabrikChain3D chain, Mat4f viewMatrix, Mat4f projectionMatrix, Colour4f colour)	
	{	
		int numBones = chain.getNumBones();
		for (int loop = 0; loop < numBones; ++loop)
		{	
			this.drawBone( chain.getBone(loop), viewMatrix, projectionMatrix, colour );
		}	
	}
	
	/**
	 * Draw a chain using the model loaded on this FabrikModel3D using a default colour of white at full opacity.
	 * 
	 * @param	chain				The FabrikChain3D to draw the model as bones on.
	 * @param	viewMatrix			The view matrix, typically retrieved from the camera.
	 * @param	projectionMatrix	The projection matrix of our scene.
	 */
	public void drawChain(FabrikChain3D chain, Mat4f viewMatrix, Mat4f projectionMatrix)	
	{	
		int numBones = chain.getNumBones();
		for (int loop = 0; loop < numBones; ++loop)
		{	
			this.drawBone( chain.getBone(loop), viewMatrix, projectionMatrix, Utils.WHITE);
		}	
	}
	
	/**
	 * Draw a structure using the model loaded on this FabrikModel3D.
	 * 
	 * @param	structure			The FabrikStructure3D to draw the model as bones on.
	 * @param	viewMatrix			The view matrix, typically retrieved from the camera.
	 * @param	projectionMatrix	The projection matrix of our scene.
	 * @param	colour				The colour of the lines used to draw the model.
	 */
	public void drawStructure(FabrikStructure3D structure, Mat4f viewMatrix, Mat4f projectionMatrix, Colour4f colour)	
	{	
		int numChains = structure.getNumChains();
		for (int loop = 0; loop < numChains; ++loop)
		{	
			this.drawChain( structure.getChain(loop), viewMatrix, projectionMatrix, colour );
		}	
	}
	
	/**
	 * Draw a structure using the model loaded on this FabrikModel3D using a default colour of white at full opacity.
	 * 
	 * @param	structure	The FabrikStructure3D to draw the model as bones on.
	 * @param	viewMatrix			The view matrix, typically retrieved from the camera.
	 * @param	projectionMatrix	The projection matrix of our scene.
	 */
	public void drawStructure(FabrikStructure3D structure, Mat4f viewMatrix, Mat4f projectionMatrix)	
	{	
		int numChains = structure.getNumChains();
		for (int loop = 0; loop < numChains; ++loop)
		{	
			this.drawChain( structure.getChain(loop), viewMatrix, projectionMatrix, Utils.WHITE);
		}	
	}
	
	/**
	 * Line width property setter.
	 * <p>
	 * Valid line widths are between 1.0f an 32.0f - values outside of this range will result
	 * in an IllegalArgumentException being thrown.
	 * 
	 * @param	lineWidth	The width of the line used to draw this FabrikModel3D in pixels.
	 */
	void setLineWidth(float lineWidth)
	{
		Utils.validateLineWidth(lineWidth);
		mLineWidth = lineWidth;
	}
	
} // End of FabrikModel3D class public class Grid
{
	// ----- Static Properties -----

    /** Each vertex has an X, Y and Z component. */
	private static final int VERTEX_COMPONENTS = 3;
	
	/** The shader program used to draw all grids. */
	private static ShaderProgram gridShaderProgram;

	/** The ModelViewProjection matrix float buffer used to draw any given grid which is updated via the draw() method. */
	private static FloatBuffer mvpMatrixFB;
	
	/** The FloatBuffer used to get and restore the current line width. */
	private static FloatBuffer currentLineWidthFB = Utils.createFloatBuffer(16);

	//Define our vertex shader source code
	//Note: The R" notation is for raw strings and preserves all spaces, indentation,
	//newlines etc. in utf-8|16|32 wchar_t format, but requires C++0x or C++11.
	private static final String VERTEX_SHADER_SOURCE =
			"#version 330"                                                                    + Utils.NEW_LINE +
			"in vec3 vertexLocation; // Incoming vertex attribute"                            + Utils.NEW_LINE +
			"uniform mat4 mvpMatrix; // Combined Model/View/Projection matrix"                + Utils.NEW_LINE +
			"void main(void) {"                                                               + Utils.NEW_LINE +
			"	gl_Position = mvpMatrix * vec4(vertexLocation, 1.0); // Project our geometry" + Utils.NEW_LINE +
			"}";

	//Define our fragment shader source code
	private static final String FRAGMENT_SHADER_SOURCE =
			"#version 330"                                                               + Utils.NEW_LINE +
			"out vec4 vOutputColour; // Outgoing colour value"                           + Utils.NEW_LINE +
			"void main() {"                                                              + Utils.NEW_LINE +
			"	vOutputColour = vec4(1.0); // Draw our pixel in white with full opacity" + Utils.NEW_LINE +
			"}";

	// ----- Non-Static Properties -----

	private int         gridVaoId;          // The Vertex Array Object ID which holds our shader attributes
	private int         gridVertexBufferId; // The id of the vertex buffer containing the grid vertex data

	private int         numVerts;           // How many vertices in this grid?
	private float[]     gridArray;          // Array of floats used to draw the grid
	private FloatBuffer vertexFloatBuffer;  // Vertex buffer to hold the gridArray vertex data
	
	static {
		// Create our MVP matrix float buffer
		mvpMatrixFB = Utils.createFloatBuffer(16);

		// Set up out shader
		gridShaderProgram = new ShaderProgram();
		gridShaderProgram.initFromStrings(VERTEX_SHADER_SOURCE, FRAGMENT_SHADER_SOURCE);
		gridShaderProgram.addAttribute("vertexLocation");
		gridShaderProgram.addUniform("mvpMatrix");
	}

	/**
	 * Constructor.
	 * <p>
	 * Width is along +/- X-axis, depth is along +/- Z-axis, height is the location of
	 * the grid on the Y-axis, numDivisions is how many lines to draw across each axis.
	 * 
	 * @param	width			The width of the grid in world-space units.
	 * @param	depth			The depth of the grid in world-space units.
	 * @param	height			The location of the grid on the Y-axis.
	 * @param	numDivisions	The number of divisions in the grid.
	 */
	public Grid( float width,  float depth,  float height,  int numDivisions)
	{
		// Calculate how many vertices our grid will consist of.
		// Multiplied by 2 because 2 verts per line, and times 2 again because our
		// grid is composed of -z to +z lines, as well as -x to +x lines. Add +4 to
		// the total for the final two lines to 'close off' the grid.
		numVerts = (numDivisions * 2 * 2) + 4;

		// So for this many vertices, we're going to be using this many floats...
		int gridFloatCount = numVerts * VERTEX_COMPONENTS;

		// ...which we'll store in this float array!
		gridArray = new float[gridFloatCount];

		// For a grid of width and depth, the extent goes from -halfWidth to +halfWidth,
		// and -halfDepth to +halfDepth.
		float halfWidth = width / 2.0f;
		float halfDepth = depth / 2.0f;

		// How far we move our vertex locations each time through the loop
		float xStep = width / numDivisions;
		float zStep = depth / numDivisions;

		// Starting locations
		float xLoc = -halfWidth;
		float zLoc = -halfDepth;

		// Split the vertices into half for -z to +z lines, and half for -x to +x
		int halfNumVerts = numVerts / 2;

		// Our counter will keep track of the index of the float value we're working on
		int counter = 0;

		// Near to far lines
		// Note: Step by 2 because we're setting two vertices each time through the loop
		for (int loop = 0; loop < halfNumVerts; loop += 2)
		{
			// Far vertex
			gridArray[counter++] = xLoc;       // x
			gridArray[counter++] = height;     // y
			gridArray[counter++] = -halfDepth; // z

			gridArray[counter++]  = xLoc;      // x
			gridArray[counter++]  = height;    // y
			gridArray[counter++]  = halfDepth; // z

			// Move across on the x-axis
			xLoc += xStep;
		}

		// Left to right lines
		// Note: Step by 2 because we're setting two vertices each time through the loop
		for (int loop = halfNumVerts; loop < numVerts; loop += 2)
		{
			// Left vertex
			gridArray[counter++] = -halfWidth; // x
			gridArray[counter++] = height;     // y
			gridArray[counter++] = zLoc;       // z

			// Right vertex
			gridArray[counter++] = halfWidth; // x
			gridArray[counter++] = height;    // y
			gridArray[counter++] = zLoc;      // z

			// Move across on the z-axis
			zLoc += zStep;
		}

		// Transfer the data into the vertex float buffer
		vertexFloatBuffer = Utils.createFloatBuffer(counter);
		vertexFloatBuffer.put( gridArray );
		vertexFloatBuffer.flip();

		// Mark our grid array as null so the GC can free the memory
		gridArray = null;

		// ----- Set up our Vertex Array Object (VAO) to hold the shader attributes -----
		// Note: The grid VAO cannot be static because we may have multiple grids of various sizes.


		// Get an Id for the Vertex Array Object (VAO) and bind to it
		gridVaoId = glGenVertexArrays();
		glBindVertexArray(gridVaoId);

		// ----- Location Vertex Buffer Object (VBO) -----

		// Generate an id for the locationBuffer and bind to it
		gridVertexBufferId = glGenBuffers();
		glBindBuffer(GL_ARRAY_BUFFER, gridVertexBufferId);

		// Place the location data into the VBO...
		glBufferData(GL_ARRAY_BUFFER, vertexFloatBuffer, GL_STATIC_DRAW);

		// ...and specify the data format.
		glVertexAttribPointer(gridShaderProgram.attribute("vertexLocation"),  // 0, Vertex attribute index
				3,  // Number of normal components per vertex
				GL_FLOAT,  // Data type
				false,  // Normalised?
				0,  // Stride
				0); // Offset
		// Unbind VBO
		glBindBuffer(GL_ARRAY_BUFFER, 0);

		// Enable the vertex attribute at this location
		glEnableVertexAttribArray(gridShaderProgram.attribute("vertexLocation"));

		// Unbind our Vertex Array object - all the buffer and attribute settings above will be associated with our VAO!
		glBindVertexArray(0);
	}

	/**
	 * Draw the grid.
	 * 
	 * @param	mvpMatrix	The ModelViewProjection matrix used to draw the grid.
	 */
	public void draw(Mat4f mvpMatrix)
	{
		// Copy the ModelViewProjection matrix into the floatbuffer
		mvpMatrixFB.put( mvpMatrix.toArray() );
		mvpMatrixFB.flip();

		// Specify we're using our shader program
		gridShaderProgram.use();

		// Bind to our vertex buffer object
		glBindVertexArray(gridVaoId);

			// Provide the projection matrix uniform
			// Params: uniform name, normalised?, data source
			glUniformMatrix4fv(gridShaderProgram.uniform("mvpMatrix"), false, mvpMatrixFB);
	
			// Store the current GL_LINE_WIDTH
			// IMPORTANT: We MUST allocate a minimum of 16 floats in our FloatBuffer in LWJGL, we CANNOT just get a FloatBuffer with 1 float!
			// ALSO: glPushAttrib(GL_LINE_BIT); /* do stuff */ glPopAttrib(); should work instead of this in theory - but LWJGL fails with 'function not supported'.			
			glGetFloatv(GL_LINE_WIDTH, currentLineWidthFB);
	
			/// Set the GL_LINE_WIDTH to be the width requested, as passed to the constructor
			glLineWidth(1.0f);
	
			// Draw the grid as lines
			glDrawArrays(GL_LINES, 0, numVerts);
	
			// Reset the line width to the previous value
			glLineWidth( currentLineWidthFB.get(0) );

		// Unbind from our vertex array objext and disable our shader program
		glBindVertexArray(0);

		// Disable our shader program
		gridShaderProgram.disable();
	}
} public class MovingTarget3D
{
	/** Current location of the target. */
	private Vec3f mCurrentLocation;
	
	/** Location of the waypoint this target is moving towards. */
	private Vec3f mWaypointLocation;

	/** How far to 'step' each frame to reach our destination. */
	private Vec3f mStepValue;

	/** The center of the X/Y/Z extent about which waypoints may be chosen, */
	private Vec3f mCenter;
	
	/** The Point3D used to draw the current location of this MovingTarget3D. */
	private Point3D mPoint;
	
	/** A Line3D used so we can draw a vertical line through the current target position to assist in depth perception. */
	private Line3D  mDepthLine;
	
	/** The extent of the vertical line drawn through the current target location in world-space units. */
	private float mDepthLineHeight;

	/**
	 *  How far +/- the mCenter can our next waypoint be i.e. (50, 100, 150) would mean
	 *  a range of 100 on the x (-50 to +50), a range of 200 on the y (-100 to + 100)
	 *  and a range of 300 on the z (-150 to + 150) - all these values are mRanges around
	 *  our mCenter point.
	 */  
	private Vec3f mRanges;

	/** How many steps it should take to move from one waypoint to the next waypoint. */
	private int mNumSteps;

	/** Whether the movement between waypoints is paused or not.
	 * 
	 * @default	false
	 */
	private boolean mPaused = false;
	
	private static Vec3f lineTop    = new Vec3f();
	private static Vec3f lineBottom = new Vec3f();
	
	/**
	 * Constructur.
	 * 
	 * @param	center			The center point about which new waypoints are generated.
	 * @param	ranges			The X/Y/Z range about which new waypoints are generated.
	 * @param	numSteps		The number of steps we should use to traverse from one waypoint to the next.
	 * @param	depthLineHeight	The height (on the Y-axis) of the vertical line passing through the target.
	 */
	public MovingTarget3D(Vec3f center, Vec3f ranges, int numSteps, float depthLineHeight)
	{
		// Set properties from arguments
		mCenter          = center;
		mRanges          = ranges;
		mNumSteps        = numSteps;
		mDepthLineHeight = depthLineHeight;

		// Generate a new random location to start at
		mCurrentLocation   = new Vec3f();
		mCurrentLocation.x = Utils.randRange(mCenter.x - mRanges.x, mCenter.x + mRanges.x);
		mCurrentLocation.y = Utils.randRange(mCenter.y - mRanges.y, mCenter.y + mRanges.y);
		mCurrentLocation.z = Utils.randRange(mCenter.z - mRanges.z, mCenter.z + mRanges.z);

		// Generate a new random waypoint that we'll move towards
		mWaypointLocation   = new Vec3f();
		mWaypointLocation.x = Utils.randRange(mCenter.x - mRanges.x, mCenter.x + mRanges.x);
		mWaypointLocation.y = Utils.randRange(mCenter.y - mRanges.y, mCenter.y + mRanges.y);
		mWaypointLocation.z = Utils.randRange(mCenter.z - mRanges.z, mCenter.z + mRanges.z);

		// Calculate the step value to get to our waypoint in 'numSteps' steps
		mStepValue   = new Vec3f();
		mStepValue.x = (mWaypointLocation.x - mCurrentLocation.x) / numSteps;
		mStepValue.y = (mWaypointLocation.y - mCurrentLocation.y) / numSteps;
		mStepValue.z = (mWaypointLocation.z - mCurrentLocation.z) / numSteps;
		
		// Instantiate the point and line we'll draw to represent this target
		mPoint     = new Point3D();
		mDepthLine = new Line3D();
	}

	/**
	 * Step from our current location to our waypoint location.
	 * <p>
	 * Does nothing if the mPaused property is true.
	 * 
	 * @see #pause()
	 * @see #resume()
	 */
	public void step()
	{
		if (!mPaused)
		{
			//  Add step to location
			mCurrentLocation = mCurrentLocation.plus(mStepValue);

			// Generate a new waypoint when we're within one unit of the current waypoint
			float arrivalDistance = 1.0f;
			if ( (Math.abs(mCurrentLocation.x  - mWaypointLocation.x) < arrivalDistance) &&
                 (Math.abs(mCurrentLocation.y  - mWaypointLocation.y) < arrivalDistance) &&
                 (Math.abs(mCurrentLocation.z  - mWaypointLocation.z) < arrivalDistance) )
			{
				mWaypointLocation.x = Utils.randRange(mCenter.x - mRanges.x, mCenter.x + mRanges.x);
				mWaypointLocation.y = Utils.randRange(mCenter.y - mRanges.y, mCenter.y + mRanges.y);
				mWaypointLocation.z = Utils.randRange(mCenter.z - mRanges.z, mCenter.z + mRanges.z);

				// Recalculate our step value for the new waypoint location
				mStepValue.x = (mWaypointLocation.x - mCurrentLocation.x) / mNumSteps;
				mStepValue.y = (mWaypointLocation.y - mCurrentLocation.y) / mNumSteps;
				mStepValue.z = (mWaypointLocation.z - mCurrentLocation.z) / mNumSteps;
			}
		}
	}

	/**
	 * Return the current location of this target.
	 * 
	 * @return	the current location of this target.
	 */
	public Vec3f getCurrentLocation() { return mCurrentLocation; }
	
	/**
	 * Draw this target.
	 * 
	 * @param	colour		The colour to draw this target.
	 * @param	pointSize	The size of the point used to draw this target in pixels.
	 * @param	mvpMatrix	The ModelViewProjection matrix used to draw this target.
	 */
	public void draw(Colour4f colour, float pointSize, Mat4f mvpMatrix)
	{
		// Draw the point...
		mPoint.draw(mCurrentLocation, colour, pointSize, mvpMatrix);
		
		// ...and the line that assists in recognising the depth of the target location.
		lineTop.set(mCurrentLocation.x,  mDepthLineHeight, mCurrentLocation.z);
		lineBottom.set(mCurrentLocation.x, -mDepthLineHeight, mCurrentLocation.z);		
		mDepthLine.draw(lineTop, lineBottom, 1.0f, mvpMatrix);
	}
		
	/**
	 * Pause movement of the target.
	 * 
	 * @see #resume()
	 */
	public void pause() { mPaused = true; }
	
	/**
	 * Resume movement of the target after it has been paused.
	 * 
	 * @see #pause()
	 */
	public void resume() { mPaused = false; }
	
	/** Toggle the mPaused flag between false and true or vice versa. */
	public void togglePause() { mPaused = !mPaused; }
} public class Mat4f
{
	/** Get the line separator for this system. */
	private static final String newLine = System.lineSeparator();
	
	// Constants to convert angles between degrees and radians
	private static final float DEGS_TO_RADS = (float)Math.PI / 180.0f;
	//private static final float RADS_TO_DEGS = 180.0f / (float)Math.PI;
	
	/** Constant used to determine if a float is approximately equal to another float value. */
	private static final float FLOAT_EQUALITY_TOLERANCE = 0.0001f;
	
	// ---------- Public Properties ---------

	// These properties are deliberately declared as public for fast access without the need
	// to use getters and setters.

	public float m00, m01, m02, m03; // First  column - x-axis
	public float m10, m11, m12, m13; // Second column - y-axis
	public float m20, m21, m22, m23; // Third  column - z-axis
	public float m30, m31, m32, m33; // Fourth column - origin

	// ---------- Constructors ----------
	
	/**
	 * Default constructor.
	 * <p>
	 * All member properties are implicitly set to zero by Java as that is the default value of a float primitive.
	 */
	public Mat4f() { }
	
	/**
	 * Copy constructor.
	 * <p>
	 * Copies all properties from the source matrix into the newly constructed matrix.
	 * 
	 * @param	source	The matrix to copy the values from.
	 */
	public Mat4f(Mat4f source)
	{
		m00 = source.m00;
		m01 = source.m01;
		m02 = source.m02;
		m03 = source.m03;

		m10 = source.m10;
		m11 = source.m11;
		m12 = source.m12;
		m13 = source.m13;

		m20 = source.m20;
		m21 = source.m21;
		m22 = source.m22;
		m23 = source.m23;

		m30 = source.m30;
		m31 = source.m31;
		m32 = source.m32;
		m33 = source.m33;
	}
	
	/** 
	 * Constructor to create a Mat4f from a Mat3f and an origin.
	 * 
	 * @param	rotationMatrix	The Mat3f to use for the X/Y/Z axes.
	 * @param	origin			The Vec3f to use as the origin.
	 */
	public Mat4f(Mat3f rotationMatrix, Vec3f origin)
	{
		m00 = rotationMatrix.m00;
		m01 = rotationMatrix.m01;
		m02 = rotationMatrix.m02;
		m03 = 0.0f;

		m10 = rotationMatrix.m10;
		m11 = rotationMatrix.m11;
		m12 = rotationMatrix.m12;
		m13 = 0.0f;

		m20 = rotationMatrix.m20;
		m21 = rotationMatrix.m21;
		m22 = rotationMatrix.m22;
		m23 = 0.0f;

		m30 = origin.x;
		m31 = origin.y;
		m32 = origin.z;
		m33 = 1.0f;
	}
	
	/**
	 * Float array constructor.
	 * <p>
	 * The matrix is set a column at a time, so the first four floats from the source array are set on m0
	 * to m03, the next four on m10 to m13 and so on.
	 * <p>
	 * If the source array is not an array of precisely 16 floats then a IllegalArgumentException is thrown.
	 * 
	 * @param	source	The array of 16 floats used as the source values to set on this Mat4f.
	 */	
	public Mat4f(float[] source)
	{
		// If we have an array of precisely 16 floats then proceed with initialisation
		if (source.length == 16)
		{		
			// First column (x-axis)
			m00 = source[0 ];
			m01 = source[1 ];
			m02 = source[2 ];
			m03 = source[3 ];
	
			// Second column (y-axis)
			m10 = source[4 ];
			m11 = source[5 ];
			m12 = source[6 ];
			m13 = source[7 ];
	
			// Third column (z-axis)
			m20 = source[8 ];
			m21 = source[9 ];
			m22 = source[10];
			m23 = source[11];
	
			// Fourth column (origin)
			m30 = source[12];
			m31 = source[13];
			m32 = source[14];
			m33 = source[15];
		}
		else // Bad array size? Throw an exception.
		{
			throw new IllegalArgumentException("Source array must contain precisely 16 floats.");
		}
	}

	/**
	 * One parameter constructor.
	 * <p>
	 * Sets the provided value diagonally across the matrix. For example, to create an identity matrix with
	 * 1.0f across the diagonal then you can call:
	 * <p>
	 * {@code Mat4f m = new Mat4f(1.0f)}
	 * <p>
	 * Which will result in a matrix with the following properties:
	 * <p>
	 * 1.0f  0.0f  0.0f  0.0f
	 * <p>
	 * 0.0f  1.0f  0.0f  0.0f
	 * <p>
	 * 0.0f  0.0f  1.0f  0.0f
	 * <p>
	 * 0.0f  0.0f  0.0f  1.0f
	 * 
	 * @param	value	The value to set across the diagonal of the matrix.
	 */
	public Mat4f(float value)
	{
		// Set diagonal
		m00 = m11 = m22 = m33 = value;

		// All other values are implicitly set to zero by Java as that is the default value of a float primitive.
	}
	
	// ---------- Methods ----------

	/**
	 * Zero all properties of a matrix.
	 */
	public void zero()
	{
		m00 = m01 = m02 = m03 = m10 = m11 = m12 = m13 = m20 = m21 = m22 = m23 = m30 = m31 = m32 = m33 = 0.0f;
	}

	/**
	 * Reset a matrix to the identity matrix.
	 * <p>
	 * The matrix is set as follows:
	 * <p>
	 * 1.0f  0.0f  0.0f  0.0f
	 * <p>
	 * 0.0f  1.0f  0.0f  0.0f
	 * <p>
	 * 0.0f  0.0f  1.0f  0.0f
	 * <p>
	 * 0.0f  0.0f  0.0f  1.0f
	 */
	public void setIdentity()
	{
		// Set diagonal
		m00 = m11 = m22 = m33 = 1.0f;

		// Zero the rest of the matrix
		m01 = m02 = m03 = m10 = m12 = m13 = m20 = m21 = m23 = m30 = m31 = m32 = 0.0f;
	}

	/**
	 * Set the details of this Mat4f from an array of 16 floats.
	 * <p>
	 * The matrix is set a column at a time, so the first four floats from the source array are set on m0
	 * to m03, the next four on m10 to m13 and so on.
	 * <p>
	 * If the source array is not an array of precisely 16 floats then a IllegalArgumentException is thrown.
	 * 
	 * @param	source	The array of 16 floats used as the source values to set on this Mat4f.
	 */ 
	public void setFromArray(float[] source)
	{
		/// If we have an array of precisely 16 floats then proceed with setting
		if (source.length == 16)
		{		
			// First column (x-axis)
			m00 = source[0 ];
			m01 = source[1 ];
			m02 = source[2 ];
			m03 = source[3 ];
	
			// Second column (y-axis)
			m10 = source[4 ];
			m11 = source[5 ];
			m12 = source[6 ];
			m13 = source[7 ];
	
			// Third column (z-axis)
			m20 = source[8 ];
			m21 = source[9 ];
			m22 = source[10];
			m23 = source[11];
	
			// Fourth column (origin)
			m30 = source[12];
			m31 = source[13];
			m32 = source[14];
			m33 = source[15];
		}
		else // Bad array size? Throw an exception.
		{
			throw new IllegalArgumentException("Source array must contain precisely 16 floats.");
		}
	}
	
	/**
	 * Return the x-basis of this Mat4f as a Vec3f.
	 * <p>
	 * The x-basis is the orientation of the x-axis, as held by the m00, m01 and m02 properties.
	 * 
	 * @return		A Vec3f containing the x-basis of this Mat4f.
	 */
	public Vec3f getXBasis()
	{
		return new Vec3f(m00, m01, m02);
	}
	
	/**
	 * Return the x-basis of this Mat4f as an array of three floats.
	 * <p>
	 * The x-basis is the orientation of the x-axis, as held by the m00, m01 and m02 properties.
	 * <p>
	 * This method is provided to allow for interoperability for users who do not want to use the {@link Vec3f} class.
	 * 
	 * @return		An array of three floats containing the x-basis of this Mat4f.
	 */
	public float[] getXBasisArray()
	{
		return new float[] { m00, m01, m02 };
	}

	/**
	 * Set the x-basis of this Mat4f from a provided Vec3f.
	 * <p>
	 * The x-basis is the orientation of the x-axis, as held by the m00, m01 and m02 properties.
	 * <p>
	 * To ensure the legality of the x-basis, the provided Vec3f is normalised if required before being set.
	 * <p>
	 * If you wish to use this class for storing matrices which do not represent a rotation matrix
	 * then you should avoid the setXBasis/setYBasis/setZBasis methods and instead set the matrix
	 * properties via other methods which accept a float array and do not attempt to enforce
	 * rotation matrix legality such as {@link #setFromArray(float[])} and {@link #Mat4f(float[])}. 
	 * 
	 * @param	v	The Vec3f holding the x-basis to set.
	 * @see #setFromArray(float[])
	 * @see #Mat4f(float[])
	 */
	public void setXBasis(Vec3f v)
	{
		// If the provided Vec3f is not normalised then normalise it
		if ( !v.lengthIsApproximately(1.0f, FLOAT_EQUALITY_TOLERANCE) )
		{
			v.normalise();
		}

		// Set the x-basis
		m00 = v.x;
		m01 = v.y;
		m02 = v.z;
	}
	
	/**
	 * Set the x-basis of this Mat4f from a provided array of three floats.
	 * <p>
	 * The x-basis is the orientation of the x-axis, as held by the m00, m01 and m02 properties.
	 * <p>
	 * To ensure the legality of the x-basis, the provided array is converted into a Vec3f and
	 * normalised (if required) before being set.
	 * <p>
	 * If you wish to use this class for storing matrices which do not represent a rotation matrix
	 * then you should avoid the setXBasis/setYBasis/setZBasis methods and instead set the matrix
	 * properties via other methods which do not enforce normalisation such as the
	 * {@link #setFromArray(float[])} and {@link #Mat4f(float[])} methods. 
	 * 
	 * @param	f	The array of three floats to set as the x-basis of this Mat4f.
	 * @see		#setFromArray(float[])
	 * @see		#Mat4f(float[])
	 */
	public void setXBasis(float[] f)
	{
		// Array is correct size? If not then return without setting any values
		if (f.length != 3)
		{
			return;
		}
		
		// Construct a Vec3f from the array and normalise it if required
		Vec3f v = new Vec3f(f[0], f[1], f[2]);
		if ( !v.lengthIsApproximately(1.0f, FLOAT_EQUALITY_TOLERANCE) )
		{
			v.normalise();
		}

		// Set the x-basis
		m00 = v.x;
		m01 = v.y;
		m02 = v.z;
	}

	/**
	 * Return the y-basis of this Mat4f as a Vec3f.
	 * <p>
	 * The y-basis is the orientation of theyx-axis, as held by the m10, m11 and m12 properties.
	 * 
	 * @return		A Vec3f containing the y-basis of this Mat4f.
	 */
	public Vec3f getYBasis()
	{
		return new Vec3f(m10, m11, m12);
	}
	

	/**
	 * Return the y-basis of this Mat4f as an array of three floats.
	 * <p>
	 * The y-basis is the orientation of the y-axis, as held by the m10, m11 and m12 properties.
	 * <p>
	 * This method is provided to allow for interoperability for users who do not want to use the {@link Vec3f} class.
	 * 
	 * @return		An array of three floats containing the y-basis of this Mat4f.
	 */
	public float[] getYBasisArray()
	{
		return new float[] { m10, m11, m12 };
	}

	/**
	 * Set the y-basis of this Mat4f from a provided Vec3f.
	 * <p>
	 * The y-basis is the orientation of the y-axis, as held by the m10, m11 and m12 properties.
	 * <p>
	 * To ensure the legality of the y-basis, the provided Vec3f is normalised if required before being set.
	 * <p>
	 * If you wish to use this class for storing matrices which do not represent a rotation matrix
	 * then you should avoid the setXBasis/setYBasis/setZBasis methods and instead set the matrix
	 * properties via other methods which do not enforce normalisation such as the
	 * {@link #setFromArray(float[])} and {@link #Mat4f(float[])} methods. 
	 * 
	 * @param	v	The Vec3f holding the y-basis to set.
	 * @see		#setFromArray(float[])
	 * @see		#Mat4f(float[])
	 */
	public void setYBasis(Vec3f v)
	{
		// If the provided Vec3f is not normalised then normalise it
		if ( !v.lengthIsApproximately(1.0f, FLOAT_EQUALITY_TOLERANCE) )
		{
			v.normalise();
		}

		// Set the y-basis
		m10 = v.x;
		m11 = v.y;
		m12 = v.z;
	}
	
	/**
	 * Set the y-basis of this Mat4f from a provided array of three floats.
	 * <p>
	 * The y-basis is the orientation of the y-axis, as held by the m10, m11 and m12 properties.
	 * <p>
	 * To ensure the legality of the y-basis, the provided array is converted into a Vec3f and
	 * normalised (if required) before being set.
	 * <p>
	 * If you wish to use this class for storing matrices which do not represent a rotation matrix
	 * then you should avoid the setXBasis/setYBasis/setZBasis methods and instead set the matrix
	 * properties via other methods which do not enforce normalisation such as the
	 * {@link #setFromArray(float[])} and {@link #Mat4f(float[])} methods. 
	 * 
	 * @param	f	The array of three floats to set as the y-basis of this Mat4f.
	 * @see		#setFromArray(float[])
	 * @see		#Mat4f(float[])
	 */
	public void setYBasis(float[] f)
	{
		// Array is correct size? If not then return without setting any values
		if (f.length != 3)
		{
			return;
		}
		
		// Construct a Vec3f from the array and normalise it if required
		Vec3f v = new Vec3f(f[0], f[1], f[2]);
		if ( !v.lengthIsApproximately(1.0f, FLOAT_EQUALITY_TOLERANCE) )
		{
			v.normalise();
		}

		// Set the y-basis
		m10 = v.x;
		m11 = v.y;
		m12 = v.z;
	}

	/**
	 * Return the z-basis of this Mat4f as a Vec3f.
	 * <p>
	 * The z-basis is the orientation of the x-axis, as held by the m20, m21 and m22 properties.
	 * 
	 * @return		A Vec3f containing the z-basis of this Mat4f.
	 */
	public Vec3f getZBasis()
	{
		return new Vec3f(m20, m21, m22);
	}
	
	/**
	 * Return the z-basis of this Mat4f as an array of three floats.
	 * <p>
	 * The z-basis is the orientation of the z-axis, as held by the m20, m21 and m22 properties.
	 * <p>
	 * This method is provided to allow for interoperability for users who do not want to use the {@link Vec3f} class.
	 * 
	 * @return		An array of three floats containing the y-basis of this Mat4f.
	 */
	public float[] getZBasisArray()
	{
		return new float[] { m20, m21, m22 };
	}

	/**
	 * Set the z-basis of this Mat4f from a provided Vec3f.
	 * <p>
	 * The z-basis is the orientation of the z-axis, as held by the m20, m21 and m22 properties.
	 * <p>
	 * To ensure the legality of the z-basis, the provided Vec3f is normalised if required before being set.
	 * If you wish to use this class for storing matrices which do not represent a rotation matrix
	 * then you should avoid the setXBasis/setYBasis/setZBasis methods and instead set the matrix
	 * properties via other methods which do not enforce normalisation such as the
	 * {@link #setFromArray(float[])} and {@link #Mat4f(float[])} methods.
	 * 
	 * @param	v	The Vec3f holding the z-basis to set.
	 * @see #setFromArray(float[])
	 * @see #Mat4f(float[])
	 */
	public void setZBasis(Vec3f v)
	{
		// If the provided Vec3f is not normalised then normalise it
		if ( !v.lengthIsApproximately(1.0f, FLOAT_EQUALITY_TOLERANCE) )
		{
			v.normalise();
		}

		// Set the y-basis
		m20 = v.x;
		m21 = v.y;
		m22 = v.z;
	}
	
	/**
	 * Set the z-basis of this Mat4f from a provided array of three floats.
	 * <p>
	 * The z-basis is the orientation of the z-axis, as held by the m20, m21 and m22 properties.
	 * <p>
	 * To ensure the legality of the z-basis, the provided array is converted into a Vec3f and
	 * normalised (if required) before being set.
	 * <p>
	 * If you wish to use this class for storing matrices which do not represent a rotation matrix
	 * then you should avoid the setXBasis/setYBasis/setZBasis methods and instead set the matrix
	 * properties via other methods which do not enforce normalisation such as the
	 * {@link #setFromArray(float[])} and {@link #Mat4f(float[])} methods.
	 * 
	 * @param	f	The array of three floats to set as the z-basis of this Mat4f.
	 * @see 	#setFromArray(float[])
	 * @see 	#Mat4f(float[])
	 */
	public void setZBasis(float[] f)
	{
		// Array is correct size? If not then return without setting any values
		if (f.length != 3)
		{
			return;
		}
		
		// Construct a Vec3f from the array and normalise it if required
		Vec3f v = new Vec3f(f[0], f[1], f[2]);
		if ( !v.lengthIsApproximately(1.0f, FLOAT_EQUALITY_TOLERANCE) )
		{
			v.normalise();
		}

		// Set the y-basis
		m20 = v.x;
		m21 = v.y;
		m22 = v.z;
	}

	/**
	 * Return the origin of this Mat4f.
	 * 
	 * @return		A Vec3f of the origin location of this Mat4f, as stored in the m30, m31 and m32 properties.
	 */
	public Vec3f getOrigin()
	{
		return new Vec3f(m30, m31, m32);
	}

	/**
	 * Set the origin of this Mat4f.
	 * <p>
	 * The origin is stored in the matrix properties m30 (x location), m31 (y location) and m32 (z location).
	 * <p>
	 * As the origin may be at any location, as such it is <em>not</em> normalised before being set.
	 * 
	 * @param	v	The origin of this Mat4f as a Vec3f.
	 */
	public void setOrigin(Vec3f v)
	{
		// Set the origin
		m30 = v.x;
		m31 = v.y;
		m32 = v.z;
		m33 = 1.0f;
	}

	/**
	 * Return whether or not all three axes of this Mat4f are orthogonal (i.e. at 90 degrees to each other).
	 * <p>
	 * Any two axes, such as x/y, x/z or y/z will be orthogonal if their dot product is zero. However, to
	 * account for floating point precision errors, this method accepts that two axes are orthogonal if
	 * their dot product is less than or equal to 0.01f.
	 * <p>
	 * If you want to find out if any two specific axes are orthogonal, then you can use code similar to the
	 * following:
	 * <p>
	 * {@code boolean xDotYOrthogonal = Math.abs( xAxis.dot(yAxis) ) <= 0.01f;}
	 * 
	 * @return		A boolean indicating whether this Mat4f is orthogonal or not. 
	 */
	public boolean isOrthogonal()
	{
		// Get the x, y and z axes of the matrix as Vec3f objects
		Vec3f xAxis = new Vec3f(m00, m01, m02);
		Vec3f yAxis = new Vec3f(m10, m11, m12);
		Vec3f zAxis = new Vec3f(m20, m21, m22);
		
		// As exact floating point comparisons are a bad idea, we'll accept that a float is
		// approximately equal to zero if it is +/- this epsilon.
		float epsilon = 0.01f;

		// Check whether the x/y, x/z and y/z axes are orthogonal.
		// If two axes are orthogonal then their dot product will be zero (or approximately zero in this case).
		// Note: We could have picked y/x, z/x, z/y but it's the same thing - they're either orthogonal or they're not.		
		boolean xDotYOrthogonal = Math.abs( Vec3f.dotProduct(xAxis, yAxis) ) <= epsilon;
		boolean xDotZOrthogonal = Math.abs( Vec3f.dotProduct(xAxis, zAxis) ) <= epsilon;
		boolean yDotZOrthogonal = Math.abs( Vec3f.dotProduct(yAxis, zAxis) ) <= epsilon;

		// All three axes are orthogonal? Return true
		return (xDotYOrthogonal && xDotZOrthogonal && yDotZOrthogonal);
	}

	/**
	 * Transpose this Mat4f.
	 * <p>
	 * <strong>This</strong> Mat4f is transposed, not a copy.
	 * <p>
	 * If you want to return a transposed version of a Mat4f <em>without modifying</em> the source
	 * then use the {@link #transposed()} method instead.
	 * 
	 * @return	Return this Mat4f for chaining.
	 * @see		#transposed()
	 */
	public Mat4f transpose()
	{
		// Take a copy of this matrix as a series of floats.
		// Note: We do not need the m00, m11, m22, or m33 values as they do not change places.
		
		//float m00copy = m00;
		float m01copy = m01;
		float m02copy = m02;
		float m03copy = m03;

		float m10copy = m10;
		//float m11copy = m11;
		float m12copy = m12;
		float m13copy = m13;

		float m20copy = m20;
		float m21copy = m21;
		//float m22copy = m22;
		float m23copy = m23;

		float m30copy = m30;
		float m31copy = m31;
		float m32copy = m32;
		//float m33copy = m33;
		
		// Assign the values back to this matrix in transposed order
		
		//this.m00 = m00copy; // No-op!
		this.m01 = m10copy;
		this.m02 = m20copy;
		this.m03 = m30copy;

		this.m10 = m01copy;
		//this.m11 = m11copy; // No-op!
		this.m12 = m21copy;
		this.m13 = m31copy;

		this.m20 = m02copy;
		this.m21 = m12copy;
		//this.m22 = m22copy; // No-op!
		this.m23 = m32copy;

		this.m30 = m03copy;
		this.m31 = m13copy;
		this.m32 = m23copy;
		//this.m33 = m33copy; // No-op!

		// Return this for chaining
		return this;
	}
	
	/**
	 * Return a transposed version of this Mat4f.
	 * <p>
	 * This Mat4f is <strong>not</strong> modified during the transposing process.
	 * <p>
	 * If you want to transpose 'this' Mat4f rather than return a transposed copy then use the
	 * {@link #transpose()} method instead.
	 * 
	 * @return		A transposed version of this Mat4f.
	 */
	public Mat4f transposed()
	{
		// Create a new Mat4f to hold the transposed version of this matrix
		Mat4f result = new Mat4f();
		
		// Assign the values of this matrix to the result matrix in transposed order.
		// Note: We don't really need to specify 'this.mXX' (the 'this' is implied) but
		// there's no harm in doing so, and it helps to clarify what's going on.
		result.m00 = this.m00;
		result.m01 = this.m10;
		result.m02 = this.m20;
		result.m03 = this.m30;

		result.m10 = this.m01;
		result.m11 = this.m11;
		result.m12 = this.m21;
		result.m13 = this.m31;

		result.m20 = this.m02;
		result.m21 = this.m12;
		result.m22 = this.m22;
		result.m23 = this.m32;

		result.m30 = this.m03;
		result.m31 = this.m13;
		result.m32 = this.m23;
		result.m33 = this.m33;

		// Return the transposed matrix
		return result;
	}

	/**
	 * Method to multiply this Mat4f by another Mat4f and return the result.
	 * <p>
	 * Neither this matrix or the provided matrix argument are modified by the multiplication process,
	 * instead a new Mat4f containing the resulting matrix is returned.
	 * <p>
	 * Matrix multiplication is <strong>not commutative</strong> (i.e. {@code AB != BA} - that is, the
	 * result of multiplying matrix A by matrix B is <em>not</em> the same as multiplying matrix B by
	 * matrix A). As such, to construct a ModelView or ModelViewProjection matrix you must specify them
	 * in that precise order <strong>reversed</strong> (because of the order in which the calculations
	 * take place), for example:
	 * <p>
	 * {@code Mat4f modelViewMatrix = viewMatrix.times(modelMatrix); }
	 * <p>
	 * {@code Mat4f modelViewProjectionMatrix = projectionMatrix.times(viewMatrix).times(modelMatrix); } 
	 * <p>
	 * Although matrix multiplication is <strong>not</strong> commutative, it <strong>is</strong>
	 * associative (i.e. {@code A(BC) == (AB)C}), so you can quite happily combine matrices like this:
	 * <p>
	 * {@code Mat4f modelViewMatrix = viewMatrix.times(modelMatrix); }
	 * <p>
	 * {@code Mat4f modelViewProjectionMatrix = projectionMatrix.times(modelViewMatrix); }
	 * 
	 * @param	m	The matrix to multiply this matrix by. 
	 * @return		A Mat4f which is the result of multiplying this matrix by the provided matrix.
	 */
	public Mat4f times(Mat4f m)
	{
		// Create a new Mat4f to hold the resulting matrix
		Mat4f result = new Mat4f();
		
		// Multiply this matrix by the provided matrix
		// Note: This multiplication cannot be performed 'inline' on the same object, hence the need to
		// calculate the and place them into a new matrix which we return.
		result.m00 = m00 * m.m00 + m10 * m.m01 + m20 * m.m02 + m30 * m.m03;
		result.m01 = m01 * m.m00 + m11 * m.m01 + m21 * m.m02 + m31 * m.m03;
		result.m02 = m02 * m.m00 + m12 * m.m01 + m22 * m.m02 + m32 * m.m03;
		result.m03 = m03 * m.m00 + m13 * m.m01 + m23 * m.m02 + m33 * m.m03;
		
		result.m10 = m00 * m.m10 + m10 * m.m11 + m20 * m.m12 + m30 * m.m13;
		result.m11 = m01 * m.m10 + m11 * m.m11 + m21 * m.m12 + m31 * m.m13;
		result.m12 = m02 * m.m10 + m12 * m.m11 + m22 * m.m12 + m32 * m.m13;
		result.m13 = m03 * m.m10 + m13 * m.m11 + m23 * m.m12 + m33 * m.m13;
		
		result.m20 = m00 * m.m20 + m10 * m.m21 + m20 * m.m22 + m30 * m.m23;
		result.m21 = m01 * m.m20 + m11 * m.m21 + m21 * m.m22 + m31 * m.m23;
		result.m22 = m02 * m.m20 + m12 * m.m21 + m22 * m.m22 + m32 * m.m23;
		result.m23 = m03 * m.m20 + m13 * m.m21 + m23 * m.m22 + m33 * m.m23;
		
		result.m30 = m00 * m.m30 + m10 * m.m31 + m20 * m.m32 + m30 * m.m33;
		result.m31 = m01 * m.m30 + m11 * m.m31 + m21 * m.m32 + m31 * m.m33;
		result.m32 = m02 * m.m30 + m12 * m.m31 + m22 * m.m32 + m32 * m.m33;
		result.m33 = m03 * m.m30 + m13 * m.m31 + m23 * m.m32 + m33 * m.m33;		

		return result;
	}

	/**
	 * Transform a point in 3D space.
	 * <p>
	 * This method multiplies the provided Vec3f by this matrix. This is commonly used to transform vertices
	 * between coordinate spaces, for example you might multiply a vertex location by a model matrix to
	 * transform the vertex from model space into world space.
	 * <p>
	 * The difference between this method and the {@link #transformDirection} method is that this method
	 * takes the origin of this Mat4f into account when performing the transformation, whilst
	 * transformDirection does not.
	 * 
	 * @param	v	The Vec3f location to transform.
	 * @return		The transformed Vec3f location.
	 */  
	public Vec3f transformPoint(Vec3f v)
	{
		// Create a new Vec3f with all properties set to zero
		Vec3f result = new Vec3f();
		
		// Apply the transformation of the given point using this matrix
		result.x = this.m00 * v.x + this.m10 * v.y + this.m20 * v.z + this.m30; // * 1.0f; - no need as w is 1.0 for a location
		result.y = this.m01 * v.x + this.m11 * v.y + this.m21 * v.z + this.m31; // * 1.0f; - no need as w is 1.0 for a location
		result.z = this.m02 * v.x + this.m12 * v.y + this.m22 * v.z + this.m32; // * 1.0f; - no need as w is 1.0 for a location

		return result;
	}

	/**
	 * Transform a direction in 3D space taking into account the orientation of this Mat4fs x/y/z axes.
	 * <p>
	 * The difference between this method and the {@link #transformPoint} method is that this method
	 * does not take the origin of this Mat4f into account when performing the transformation, whilst
	 * transformPoint does.
	 * 
	 * @param	v	The Vec3f to transform.
	 * @return		The transformed Vec3f.
	 */  
	public Vec3f transformDirection(Vec3f v)
	{
		Vec3f result = new Vec3f(0.0f);		
		
		// NO!
		result.x = this.m00 * v.x + this.m10 * v.y + this.m20 * v.z; // + this.m30 * 0.0; - no need as w is 0.0 for a direction
		result.y = this.m01 * v.x + this.m11 * v.y + this.m21 * v.z; // + this.m31 * 0.0; - no need as w is 0.0 for a direction
		result.z = this.m02 * v.x + this.m12 * v.y + this.m22 * v.z; // + this.m32 * 0.0; - no need as w is 0.0 for a direction

		return result;
	}

	/**
	 * Calculate and return the inverse of a Mat4f.
	 * <p>
	 * Only matrices which do not have a {@link #determinant()} of zero can be inverted. If
	 * the determinant of the provided matrix is zero then an IllegalArgumentException is thrown.
	 * 
	 * @param	m	The matrix to invert.
	 * @return		The inverted matrix.
	 */
	public static Mat4f inverse(Mat4f m)
	{
		Mat4f temp = new Mat4f();

		temp.m00 = m.m12*m.m23*m.m31 - m.m13*m.m22*m.m31 + m.m13*m.m21*m.m32 - m.m11*m.m23*m.m32 - m.m12*m.m21*m.m33 + m.m11*m.m22*m.m33;
		temp.m01 = m.m03*m.m22*m.m31 - m.m02*m.m23*m.m31 - m.m03*m.m21*m.m32 + m.m01*m.m23*m.m32 + m.m02*m.m21*m.m33 - m.m01*m.m22*m.m33;
		temp.m02 = m.m02*m.m13*m.m31 - m.m03*m.m12*m.m31 + m.m03*m.m11*m.m32 - m.m01*m.m13*m.m32 - m.m02*m.m11*m.m33 + m.m01*m.m12*m.m33;
		temp.m03 = m.m03*m.m12*m.m21 - m.m02*m.m13*m.m21 - m.m03*m.m11*m.m22 + m.m01*m.m13*m.m22 + m.m02*m.m11*m.m23 - m.m01*m.m12*m.m23;
		temp.m10 = m.m13*m.m22*m.m30 - m.m12*m.m23*m.m30 - m.m13*m.m20*m.m32 + m.m10*m.m23*m.m32 + m.m12*m.m20*m.m33 - m.m10*m.m22*m.m33;
		temp.m11 = m.m02*m.m23*m.m30 - m.m03*m.m22*m.m30 + m.m03*m.m20*m.m32 - m.m00*m.m23*m.m32 - m.m02*m.m20*m.m33 + m.m00*m.m22*m.m33;
		temp.m12 = m.m03*m.m12*m.m30 - m.m02*m.m13*m.m30 - m.m03*m.m10*m.m32 + m.m00*m.m13*m.m32 + m.m02*m.m10*m.m33 - m.m00*m.m12*m.m33;
		temp.m13 = m.m02*m.m13*m.m20 - m.m03*m.m12*m.m20 + m.m03*m.m10*m.m22 - m.m00*m.m13*m.m22 - m.m02*m.m10*m.m23 + m.m00*m.m12*m.m23;
		temp.m20 = m.m11*m.m23*m.m30 - m.m13*m.m21*m.m30 + m.m13*m.m20*m.m31 - m.m10*m.m23*m.m31 - m.m11*m.m20*m.m33 + m.m10*m.m21*m.m33;
		temp.m21 = m.m03*m.m21*m.m30 - m.m01*m.m23*m.m30 - m.m03*m.m20*m.m31 + m.m00*m.m23*m.m31 + m.m01*m.m20*m.m33 - m.m00*m.m21*m.m33;
		temp.m22 = m.m01*m.m13*m.m30 - m.m03*m.m11*m.m30 + m.m03*m.m10*m.m31 - m.m00*m.m13*m.m31 - m.m01*m.m10*m.m33 + m.m00*m.m11*m.m33;
		temp.m23 = m.m03*m.m11*m.m20 - m.m01*m.m13*m.m20 - m.m03*m.m10*m.m21 + m.m00*m.m13*m.m21 + m.m01*m.m10*m.m23 - m.m00*m.m11*m.m23;
		temp.m30 = m.m12*m.m21*m.m30 - m.m11*m.m22*m.m30 - m.m12*m.m20*m.m31 + m.m10*m.m22*m.m31 + m.m11*m.m20*m.m32 - m.m10*m.m21*m.m32;
		temp.m31 = m.m01*m.m22*m.m30 - m.m02*m.m21*m.m30 + m.m02*m.m20*m.m31 - m.m00*m.m22*m.m31 - m.m01*m.m20*m.m32 + m.m00*m.m21*m.m32;
		temp.m32 = m.m02*m.m11*m.m30 - m.m01*m.m12*m.m30 - m.m02*m.m10*m.m31 + m.m00*m.m12*m.m31 + m.m01*m.m10*m.m32 - m.m00*m.m11*m.m32;
		temp.m33 = m.m01*m.m12*m.m20 - m.m02*m.m11*m.m20 + m.m02*m.m10*m.m21 - m.m00*m.m12*m.m21 - m.m01*m.m10*m.m22 + m.m00*m.m11*m.m22;

		// Get the determinant of this matrix
		float determinant = temp.determinant();

		// Each property of the inverse matrix is multiplied by 1.0f divided by the determinant.
		// As we cannot divide by zero, we will throw an IllegalArgumentException if the determinant is zero.
		if (Float.compare(determinant,0.0f)==0)
		{
			throw new IllegalArgumentException("Cannot invert a matrix with a determinant of zero.");
		}

		// Otherwise, calculate the value of one over the determinant and scale the matrix by that value
		float oneOverDeterminant = 1.0f / temp.determinant();

		temp.m00 *= oneOverDeterminant;
		temp.m01 *= oneOverDeterminant;
		temp.m02 *= oneOverDeterminant;
		temp.m03 *= oneOverDeterminant;
		temp.m10 *= oneOverDeterminant;
		temp.m11 *= oneOverDeterminant;
		temp.m12 *= oneOverDeterminant;
		temp.m13 *= oneOverDeterminant;
		temp.m20 *= oneOverDeterminant;
		temp.m21 *= oneOverDeterminant;
		temp.m22 *= oneOverDeterminant;
		temp.m23 *= oneOverDeterminant;
		temp.m30 *= oneOverDeterminant;
		temp.m31 *= oneOverDeterminant;
		temp.m32 *= oneOverDeterminant;
		temp.m33 *= oneOverDeterminant;

		// Finally, return the inverted matrix
		return temp;
	}

	/**
	 * Calculate the determinant of this matrix.
	 * 
	 * @return	The determinant of this matrix.
	 */
	public float determinant()
	{
		return  m03*m12*m21*m30 - m02*m13*m21*m30 - m03*m11*m22*m30 + m01*m13*m22*m30 +
			m02*m11*m23*m30 - m01*m12*m23*m30 - m03*m12*m20*m31 + m02*m13*m20*m31 +
			m03*m10*m22*m31 - m00*m13*m22*m31 - m02*m10*m23*m31 + m00*m12*m23*m31 +
			m03*m11*m20*m32 - m01*m13*m20*m32 - m03*m10*m21*m32 + m00*m13*m21*m32 +
			m01*m10*m23*m32 - m00*m11*m23*m32 - m02*m11*m20*m33 + m01*m12*m20*m33 +
			m02*m10*m21*m33 - m00*m12*m21*m33 - m01*m10*m22*m33 + m00*m11*m22*m33;
	}
	
	/**
	 * Translate this matrix by a provided Vec3f.
	 * <p>
	 * The changes made are to <strong>this</strong> Mat4f, in the coordinate space of this matrix.
	 * 
	 * @param	v	The vector to translate this matrix by.
	 * @return		This Mat4f for chaining. 
	 */ 
	public Mat4f translate(Vec3f v)
	{
		// Unlike in rotation, the translation procedure can be applied 'inline'
		// so we are able to/ work directly on this Mat4f, rather than a copy.
		this.m30 += this.m00 * v.x + this.m10 * v.y + this.m20 * v.z;
		this.m31 += this.m01 * v.x + this.m11 * v.y + this.m21 * v.z;
		this.m32 += this.m02 * v.x + this.m12 * v.y + this.m22 * v.z;
		this.m33 += this.m03 * v.x + this.m13 * v.y + this.m23 * v.z;
		
		// Return this for chaining
		return this;
	}

	/**
	 * Translate this matrix by separate x, y and z amounts specified as floats.
	 * <p>
	 * The changes made are to <strong>this</strong> Mat4f.
	 * 
	 * @param	x	The amount to translate on the X-axis.
	 * @param	y	The amount to translate on the Y-axis.
	 * @param	z	The amount to translate on the Z-axis.
	 * @return		This translated Mat4f for chaining.
	 */
	public Mat4f translate(float x, float y, float z)
	{
		return this.translate( new Vec3f(x, y, z) );
	}

	/*public static Mat4f rotateAroundArbitraryAxisRads(float angleRads, Vec3f axis)
	{
		Mat4f result = new Mat4f();
		
		float sinTheta = (float)Math.sin(angleRads);
		float cosTheta = (float)Math.cos(angleRads);
		float oneMinusCosTheta = 1.0f - cosTheta;
		
		result.m00 = m00 * m00 * oneMinusCosTheta + cosTheta;
		result.m01 = 
	}*/
	
	/**
	 * Rotate this matrix about a local axis by an angle specified in radians.
	 * <p>
	 * By a 'local' axis we mean that for example, if you rotated this matrix about the positive
	 * X-axis (1,0,0), then rotation would occur around the positive X-axis of
	 * <strong>this matrix</strong>, not the <em>global / world-space</em> X-axis.
	 * 
	 * @param	angleRads	The angle to rotate the matrix in radians.
	 * @param	localAxis	The local axis around which to rotate the matrix.
	 * @return				This rotated matrix
	 */
	// Method to rotate a matrix around an arbitrary axis
	public Mat4f rotateAboutLocalAxisRads(float angleRads, Vec3f localAxis)
	{
		Mat4f dest = new Mat4f();

		float cos = (float) Math.cos(angleRads);
		float sin = (float) Math.sin(angleRads);
		float nivCos = 1.0f - cos;

		float xy = localAxis.x * localAxis.y;
		float yz = localAxis.y * localAxis.z;
		float xz = localAxis.x * localAxis.z;
		float xs = localAxis.x * sin;
		float ys = localAxis.y * sin;
		float zs = localAxis.z * sin;

		// Rotate the axis
		float f00 = localAxis.x * localAxis.x * nivCos + cos;
		float f01 = xy * nivCos + zs;
		float f02 = xz * nivCos - ys;

		float f10 = xy * nivCos - zs;
		float f11 = localAxis.y * localAxis.y * nivCos + cos;
		float f12 = yz * nivCos + xs;

		float f20 = xz * nivCos + ys;
		float f21 = yz * nivCos - xs;
		float f22 = localAxis.z * localAxis.z * nivCos + cos;

		float t00 = m00 * f00 + m10 * f01 + m20 * f02;
		float t01 = m01 * f00 + m11 * f01 + m21 * f02;
		float t02 = m02 * f00 + m12 * f01 + m22 * f02;
		float t03 = m03 * f00 + m13 * f01 + m23 * f02;
		float t10 = m00 * f10 + m10 * f11 + m20 * f12;
		float t11 = m01 * f10 + m11 * f11 + m21 * f12;
		float t12 = m02 * f10 + m12 * f11 + m22 * f12;
		float t13 = m03 * f10 + m13 * f11 + m23 * f12;

		// Construct rotate matrix
		dest.m20 = m00 * f20 + m10 * f21 + m20 * f22;
		dest.m21 = m01 * f20 + m11 * f21 + m21 * f22;
		dest.m22 = m02 * f20 + m12 * f21 + m22 * f22;
		dest.m23 = m03 * f20 + m13 * f21 + m23 * f22;
		dest.m00 = t00;
		dest.m01 = t01;
		dest.m02 = t02;
		dest.m03 = t03;
		dest.m10 = t10;
		dest.m11 = t11;
		dest.m12 = t12;
		dest.m13 = t13;

		dest.m30 = m30;
		dest.m31 = m31;
		dest.m32 = m32;
		dest.m33 = m33;

		// Update this matrix to be the rotated matrix
		this.m00 = dest.m00;
		this.m01 = dest.m01;
		this.m02 = dest.m02;
		this.m03 = dest.m03;

		this.m10 = dest.m10;
		this.m11 = dest.m11;
		this.m12 = dest.m12;
		this.m13 = dest.m13;

		this.m20 = dest.m20;
		this.m21 = dest.m21;
		this.m22 = dest.m22;
		this.m23 = dest.m23;

		this.m30 = dest.m30;
		this.m31 = dest.m31;
		this.m32 = dest.m32;
		this.m33 = dest.m33;

		return this;
	}

	/**
	 * Rotate this matrix about a local axis by an angle specified in degrees.
	 * <p>
	 * By a 'local' axis we mean that for example, if you rotated this matrix about the positive
	 * X-axis (1,0,0), then rotation would occur around the positive X-axis of
	 * <strong>this matrix</strong>, not the <em>global / world-space</em> X-axis.
	 * 
	 * @param	angleDegs	The angle to rotate the matrix in degrees.
	 * @param	localAxis	The local axis around which to rotate the matrix.
	 * @return				This rotated matrix
	 */
	public Mat4f rotateAboutLocalAxisDegs(float angleDegs, Vec3f localAxis) { return rotateAboutLocalAxisRads(angleDegs * DEGS_TO_RADS, localAxis); }
	
	/**
	 * Rotate this matrix about a world-space axis by an angle specified in radians.
	 * <p>
	 * The cardinal 'world-space' axes are defined so that the +X axis runs to the right,
	 * the +Y axis runs upwards, and the +Z axis runs directly outwards from the screen.
	 *
	 * @param	angleRads	The angle to rotate the matrix in radians.
	 * @param	worldAxis	The world-space axis around which to rotate the matrix.
	 * @return			This rotated matrix.
	 */
	public Mat4f rotateAboutWorldAxisRads(float angleRads, Vec3f worldAxis)
	{
		// Extract the x/y/z axes from this matrix
		Vec3f xAxis = new Vec3f(m00, m01, m02);
		Vec3f yAxis = new Vec3f(m10, m11, m12);
		Vec3f zAxis = new Vec3f(m20, m21, m22);
		
		// Rotate them around the global axis
		Vec3f rotatedXAxis = Vec3f.rotateAboutAxisRads(xAxis, angleRads, worldAxis);
		Vec3f rotatedYAxis = Vec3f.rotateAboutAxisRads(yAxis, angleRads, worldAxis);
		Vec3f rotatedZAxis = Vec3f.rotateAboutAxisRads(zAxis, angleRads, worldAxis);
		
		
		// Assign the rotated axes back to the this matrix
		
		// Set rotated X-axis
		this.m00 = rotatedXAxis.x;
		this.m01 = rotatedXAxis.y;
		this.m02 = rotatedXAxis.z;
		//this.m03 = this.m03; // No change to w
		
		// Set rotated Y-axis
		this.m10 = rotatedYAxis.x;
		this.m11 = rotatedYAxis.y;
		this.m12 = rotatedYAxis.z;
		//this.m13 = matrix.m13; // No change to w
		
		// Set rotated Z-axis
		this.m20 = rotatedZAxis.x;
		this.m21 = rotatedZAxis.y;
		this.m22 = rotatedZAxis.z;
		// this.m23 = matrix.m23; No change to w
		
		// The origin does not change
		//this.m30 = matrix.m30;
		//this.m31 = matrix.m31;
		//this.m32 = matrix.m32;
		//this.m33 = matrix.m33;
		
		// Return the rotated matrix
		return this;
	}

	/**
	 * Rotate a matrix about a given global axis by an angle in radians.
	 * <p>
	 * The matrix provided is NOT modified - a rotated version is returned.
	 * <p>
	 * The axis specified to rotate the matrix about is <strong>not</strong>
	 * specified in the coordinate space of the matrix being rotated - it is
	 * specified in global coordinates, such as used in OpenGL world space. In
	 * this coordinate system:
	 * <ul>
	 * <li>The positive X-axis runs to the right (1,0,0),
	 * <li>The positive Y-axis runs vertically upwards (0,1,0), and</li>
	 * <li>The positive Z-axis runs outwards from the screen (0,0,1).</li>
	 * </ul>
	 * 
	 * @param	matrix		The matrix to rotate.
	 * @param	angleRads	The angle to rotate the matrix in radians.
	 * @param	worldAxis	The world-space axis to rotate around.
	 * @return			The rotated matrix.
	 */	
	public static Mat4f rotateAboutWorldAxisRads(Mat4f matrix, float angleRads, Vec3f worldAxis)
	{
		// Extract the x/y/z axes from the matrix
		Vec3f xAxis = new Vec3f(matrix.m00, matrix.m01, matrix.m02);
		Vec3f yAxis = new Vec3f(matrix.m10, matrix.m11, matrix.m12);
		Vec3f zAxis = new Vec3f(matrix.m20, matrix.m21, matrix.m22);
		
		//System.out.println("In rotMat, xAxis is: " + xAxis);
		
		// Rotate them around the global axis
		Vec3f rotatedXAxis = Vec3f.rotateAboutAxisRads(xAxis, angleRads, worldAxis);
		Vec3f rotatedYAxis = Vec3f.rotateAboutAxisRads(yAxis, angleRads, worldAxis);
		Vec3f rotatedZAxis = Vec3f.rotateAboutAxisRads(zAxis, angleRads, worldAxis);
		
		//System.out.println("In rotMat, rotated xAxis is: " + rotatedXAxis);
		
		// Assign them back to the result matrix
		Mat4f result = new Mat4f();
		
		// Set rotated X-axis
		result.m00 = rotatedXAxis.x;
		result.m01 = rotatedXAxis.y;
		result.m02 = rotatedXAxis.z;
		result.m03 = matrix.m03;
		
		// Set rotated Y-axis
		result.m10 = rotatedYAxis.x;
		result.m11 = rotatedYAxis.y;
		result.m12 = rotatedYAxis.z;
		result.m13 = matrix.m13;
		
		// Set rotated Z-axis
		result.m20 = rotatedZAxis.x;
		result.m21 = rotatedZAxis.y;
		result.m22 = rotatedZAxis.z;
		result.m23 = matrix.m23;
		
		// The origin does not change
		result.m30 = matrix.m30;
		result.m31 = matrix.m31;
		result.m32 = matrix.m32;
		result.m33 = matrix.m33;
		
		// Return the rotated matrix
		return result;
	}
	
	/**
	 * Rotate a matrix about a given global axis by an angle in radians.
	 * <p>
	 * The matrix provided is NOT modified - a rotated version is returned.
	 * <p>
	 * The axis specified to rotate the matrix about is <strong>not</strong>
	 * specified in the coordinate space of the matrix being rotated - it is
	 * specified in global coordinates, such as used in OpenGL world space. In
	 * this coordinate system:
	 * <ul>
	 * <li>The positive X-axis runs to the right (1,0,0),
	 * <li>The positive Y-axis runs vertically upwards (0,1,0), and</li>
	 * <li>The positive Z-axis runs outwards from the screen (0,0,1).</li>
	 * </ul>
	 * 
	 * @param	matrix		The matrix to rotate.
	 * @param	angleDegs	The angle to rotate the matrix in degrees.
	 * @param	worldAxis	The world-space axis to rotate around.
	 * @return			A rotated version of the provided matrix.
	 */	
	public static Mat4f rotateAboutWorldAxisDegs(Mat4f matrix, float angleDegs, Vec3f worldAxis)
	{
		return rotateAboutWorldAxisRads(matrix, angleDegs * DEGS_TO_RADS, worldAxis);
	}
	
	/**
	 * Rotate matrix about a relative axis.
	 * <p>
	 * The matrix is rotated about the provided axis in the coordinate system of the provided matrix.
	 * 
	 * @param	matrix		The matrix to rotate.
	 * @param	angleRads	The angle to rotate the matrix about in radians.
	 * @param	localAxis	The relative axis to rotate about.
	 * @return			A rotated matrix.
	 */
	public static Mat4f rotateMatrixAboutLocalAxisRads(Mat4f matrix, float angleRads, Vec3f localAxis)
	{		
		// Transform the local rotation axis into world space
		Vec3f worldSpaceAxis = matrix.transformDirection(localAxis);
		
		// With that done, we can now use the world-space axis method to perform the matrix rotation
		return Mat4f.rotateAboutWorldAxisRads(matrix, angleRads, worldSpaceAxis);
	}
	
	/**
	 * Rotate matrix about a relative axis.
	 * <p>
	 * The matrix is rotated about the provided axis in the coordinate system of the provided matrix.
	 * 
	 * @param	matrix		The matrix to rotate.
	 * @param	angleDegs	The angle to rotate the matrix about in degrees.
	 * @param	localAxis	The relative axis to rotate about.
	 * @return			A rotated matrix.
	 */
	public static Mat4f rotateMatrixAboutLocalAxisDegs(Mat4f matrix, float angleDegs, Vec3f localAxis)
	{
		return Mat4f.rotateMatrixAboutLocalAxisDegs(matrix, angleDegs * DEGS_TO_RADS, localAxis);
	}

	/**
	 * Return a Mat3f version of this Mat4f.
	 * <p>
	 * The x, y and z axes are returned in the Mat3f, while the w component of each axis
	 * and the origin plus it's w component are discarded.
	 * <p>
	 * In effect, we are extracting the orientation of this Mat4f into a Mat3f.
	 * 
	 * @return	A Mat3f version of this Mat4f.
	 */
	public Mat3f toMat3f()
	{
		Mat3f rotationMatrix = new Mat3f();

		rotationMatrix.m00 = this.m00;
		rotationMatrix.m01 = this.m01;
		rotationMatrix.m02 = this.m02;

		rotationMatrix.m10 = this.m10;
		rotationMatrix.m11 = this.m11;
		rotationMatrix.m12 = this.m12;

		rotationMatrix.m20 = this.m20;
		rotationMatrix.m21 = this.m21;
		rotationMatrix.m22 = this.m22;

		return rotationMatrix;
	}

	/**
	 * Return this Mat4f as an array of 16 floats.
	 * 
	 * @return	This Mat4f as an array of 16 floats.
	 */
	public float[] toArray() { return new float[] { m00, m01, m02, m03, m10, m11, m12, m13, m20, m21, m22, m23, m30, m31, m32, m33 }; }
	
	/**
	 * Overridden toString method.
	 * <p>
	 * The matrix values are formatted to three decimal places. If you require the exact unformatted
	 * m<em>XY</em> values then they may be accessed directly.
	 * 
	 * @return	A concise, human-readable description of a Mat4f.
	 */
	@Override
	public String toString()
	{
		// Note: '0' means put a 0 there if it's zero, '#' means omit if zero
		DecimalFormat df = new DecimalFormat("0.000");

		StringBuilder sb = new StringBuilder();
		
		sb.append("X-axis: (" + df.format(m00) + ", " + df.format(m01) + ", " + df.format(m02) + ", " + df.format(m03) + ")" + newLine);
		sb.append("Y-axis: (" + df.format(m10) + ", " + df.format(m11) + ", " + df.format(m12) + ", " + df.format(m13) + ")" + newLine);
		sb.append("Z-axis: (" + df.format(m20) + ", " + df.format(m21) + ", " + df.format(m22) + ", " + df.format(m23) + ")" + newLine);
		sb.append("Origin: (" + df.format(m30) + ", " + df.format(m31) + ", " + df.format(m32) + ", " + df.format(m33) + ")" + newLine);
		
		return sb.toString();
	}

	// ---------- Static methods ----------

	/**
	 * Construct an orthographic projection matrix.
	 * <p>
	 * Orthographic projections are commonly used when working in 2D or CAD scenarios. As orthographic projection
	 * does not perform foreshortening on any projected geometry, objects are drawn the same size regardless of
	 * their distance from the camera.
	 * <p>
	 * By specifying the bottom clipping plane to be 0.0f and the top clipping plane to be the height of the window,
	 * the origin of the coordinate space is at the bottom-left of the window and the positive y-axis runs upwards.
	 * To place the origin at the top left of the window and have the y-axis run downwards, simply swap the top and
	 * bottom values.
	 * <p>
	 * Once you have an orthographic projection matrix, if you are not using any separate Model or View matrices
	 * then you may simply use the orthographic matrix as a ModelViewProjection matrix.
	 * <p>
	 * If values are passed so that (right - left), (top - bottom) or (far - near) are zero then an
	 * IllegalArgumentException is thrown.
	 * 
	 * @param	left	The left   clipping plane, typically 0.0f.	
	 * @param	right	The right  clipping plane, typically the width of the window.
	 * @param	top		The top    clipping plane, typically the height of the window.
	 * @param	bottom  The bottom clipping plane, typically 0.0f.
	 * @param	near	The near   clipping plane, typically -1.0f.
	 * @param	far     The far    clipping plane, typically  1.0f.
	 * @return			The constructed orthographic matrix
	 * @see				<a href="http://www.songho.ca/opengl/gl_projectionmatrix.html#ortho">http://www.songho.ca/opengl/gl_projectionmatrix.html#ortho</a>
	 */
	public static Mat4f createOrthographicProjectionMatrix(float left, float right, float top, float bottom, float near, float far)
	{
		// Perform sanity checking to avoid divide by zero errors
		if (Float.compare(right - left,0.0f)==0) { throw new IllegalArgumentException("(right - left) cannot be zero."); }
		if (Float.compare(top - bottom,0.0f)==0) {	throw new IllegalArgumentException("(top - bottom) cannot be zero."); }		
		if (Float.compare(far - near,0.0f)==0) { throw new IllegalArgumentException("(far - near) cannot be zero.");   }
		
		// Got legal arguments? Construct the orthographic matrix		
		Mat4f m = new Mat4f();
		
		m.m00 = 2.0f / (right - left);
		m.m01 = 0.0f;
		m.m02 = 0.0f;
		m.m03 = 0.0f;

		m.m10 = 0.0f;
		m.m11 = 2.0f / (top - bottom);
		m.m12 = 0.0f;
		m.m13 = 0.0f;

		m.m20 = 0.0f;
		m.m21 = 0.0f;
		m.m22 = -2.0f / (far - near);
		m.m23 = 0.0f;

		m.m30 = -(right + left  ) / (right - left  );
		m.m31 = -(top   + bottom) / (top   - bottom);
		m.m32 = -(far   + near  ) / (far   - near  );
		m.m33 = 1.0f;

		return m;
	}

	/**
	 * Construct a perspective projection matrix.
	 * <p>
	 * The parameters provided are the locations of the left/right/top/bottom/near/far clipping planes.
	 * <p>
	 * There is rarely any need to specify the bounds of a projection matrix in this manner, and you
	 * are likely to be better served by using the
	 * {@link #createPerspectiveProjectionMatrix(float, float, float, float)} method instead.
	 * <p>
	 * Once you have a Projection matrix, then it can be combined with a ModelView or separate Model and
	 * View matrices in the following manner (be careful: multiplication order is important) to create a
	 * ModelViewProjection matrix:
	 * <p>
	 * {@code Mat4f mvpMatrix = projectionMatrix.times(modelViewMatrix);}
	 * <p>
	 * or
	 * <p>
	 * {@code Mat4f mvpMatrix = projectionMatrix.times(viewMatrix).times(modelMatrix);}
	 * 
	 * @param	left	The left   clipping plane, typically 0.0f.	
	 * @param	right	The right  clipping plane, typically the width of the window.
	 * @param	top		The top    clipping plane, typically the height of the window.
	 * @param	bottom  The bottom clipping plane, typically 0.0f.
	 * @param	near	The near   clipping plane, typically -1.0f.
	 * @param	far     The far    clipping plane, typically  1.0f.
	 * @return			The constructed orthographic matrix
	 * @see				<a href="http://www.songho.ca/opengl/gl_projectionmatrix.html#ortho">http://www.songho.ca/opengl/gl_projectionmatrix.html#ortho</a>
	 */
	public static Mat4f createPerspectiveProjectionMatrix(float left, float right, float top, float bottom, float near, float far)
	{
		// Instantiate a new matrix, initialised to identity
		Mat4f p = new Mat4f(1.0f);

		// Set matrix values
		p.m00 =  (2.0f * near) / (right - left);

		p.m11 =  (2.0f * near) / (top - bottom);

		p.m20 =  (right + left)   / (right - left);
		p.m21 =  (top   + bottom) / (top   - bottom);
		p.m22 = -(far   + near)   / (far   - near);
		p.m23 = -1.0f;

		p.m32 = (-2.0f * far * near) / (far - near);
		p.m33 = 0.0f;

		return p;
	}

	/***
	 * Construct a perspective projection matrix.
	 * <p>
	 * The vertical and horizontal field of view (FoV) values are related in such a way that if you know one
	 * then you can calculate the other. This method takes the vertical FoV and allows the horizontal FoV to
	 * adapt to it based on the aspect ratio of the screen in a technique called 'Hor+' (horizontal plus) scaling.
	 * <p>
	 * If required, the horizontal and vertical FoVs can be calculated via the following process (note: all angles
	 * are specified in <strong>radians</strong>):
	 * <p>
	 * {@code float horizFoVRads = 2.0f * (float)Math.atan( Math.tan(vertFoVRads  / 2.0f) * aspectRatio);}
	 * <p>
	 * {@code float vertFoVRads  = 2.0f * (float)Math.atan( Math.tan(horizFoVRads / 2.0f) * (1.0f / aspectRatio) ); }
	 * <p>
	 * The aspect ratio can be calculated as: {@code (float)windowWidth / (float)windowHeight} - if the size of the
	 * window changes then a new projection matrix should be created with the new aspect ratio of the window.
	 * <p>
	 * {@code zNear} and {@code zFar} represent the near and far clipping distances outside of which any geometry
	 * will be clipped (i.e. not rendered). An acceptable value for zNear is <strong>1.0f</strong> (0.0f should be
	 * avoided), however, specifying a zNear value of <strong>2.0f</strong> will essentially double the precision
	 * of your depth buffer due to the way in which floating point values distribute bits between the significand
	 * (i.e. the value before the decimal point) and the exponent (the value to raise or lower the significand to).
	 * Choosing good values for your near and far clipping planes can often eliminate any z-fighting in your scenes.
	 * <p>
	 * An {@link IllegalArgumentException} is thrown is any of the parameters are specified with illegal values.
	 *  
	 * @param	vertFoVDegs	The vertical Field of View angle - must be a positive value between 1.0f and 179.0f. For good
	 * choices, see <a href="http://en.wikipedia.org/wiki/Field_of_view_in_video_games#Choice_of_field_of_view">choice of
	 * field of view</a>.
	 * @param	aspectRatio	The aspect ratio of the window in which drawing will occur - must be a positive value.
	 * @param	zNear		The near clipping distance - must be a positive value which is less than zFar.
	 * @param	zFar		The far  clipping distance - must be a positive value which is greater than zNear.
	 * @return				A projection matrix as a Mat4f.
	 * @see		<a href="http://en.wikipedia.org/wiki/Field_of_view_in_video_games">http://en.wikipedia.org/wiki/Field_of_view_in_video_games</a>
	 * @see		<a href="http://www.songho.ca/opengl/gl_projectionmatrix.html">http://www.songho.ca/opengl/gl_projectionmatrix.html</a>
	 * @see		<a href="https://www.opengl.org/archives/resources/faq/technical/depthbuffer.htm">https://www.opengl.org/archives/resources/faq/technical/depthbuffer.htm</a>
	 * @see		<a href="http://en.wikipedia.org/wiki/Floating_point">http://en.wikipedia.org/wiki/Floating_point</a>
	 * @see		<a href="http://en.wikipedia.org/wiki/Z-fighting">http://en.wikipedia.org/wiki/Z-fighting</a>
	 */ 
	public static Mat4f createPerspectiveProjectionMatrix(float vertFoVDegs, float aspectRatio, float zNear, float zFar)
	{
		// Sanity checking
		if (aspectRatio < 0.0f                        ) { throw new IllegalArgumentException("Aspect ratio cannot be negative.");                        }
		if (zNear <= 0.0f || zFar <= 0.0f             ) { throw new IllegalArgumentException("The values of zNear and zFar must be positive.");          }
		if (zNear >= zFar                             ) { throw new IllegalArgumentException("zNear must be less than than zFar.");                      }
		if (vertFoVDegs < 1.0f || vertFoVDegs > 179.0f) {throw new IllegalArgumentException("Vertical FoV must be within 1 and 179 degrees inclusive."); }
		
		float frustumLength = zFar - zNear;
		
		// Calculate half the vertical field of view in radians
		float halfVertFoVRads = (vertFoVDegs / 2.0f) * DEGS_TO_RADS;

		// There is no built in Math.cot() in Java, but co-tangent is simply 1 over tangent
		float cotangent = 1.0f / (float)Math.tan(halfVertFoVRads);
		
		// Instantiate a new matrix, initialised to identity
		Mat4f p = new Mat4f(1.0f);

		// Set matrix values and return the constructed projection matrix
		p.m00 = cotangent / aspectRatio;

		p.m11 = cotangent;

		p.m22 = -(zFar + zNear) / frustumLength;
		p.m23 = -1.0f;

		p.m32 = (-2.0f * zNear * zFar) / frustumLength;
		p.m33 = 0.0f;

		return p;
	}

} // End of Mat4f class public final class Utils
{
	// Constants to translate values from degrees to radians and vice versa
	private static final float DEGS_TO_RADS = (float)Math.PI / 180.0f;
	private static final float RADS_TO_DEGS = 180.0f / (float)Math.PI;

	// Define a private static DecimalFormat to be used by our toString() method.
	// Note: '0' means put a 0 there if it's zero, '#' means omit if zero.
	public static final DecimalFormat df = new DecimalFormat("0.000");
	
	/** Newline character for this system. */
	public static final String NEW_LINE = System.lineSeparator();
	
	/**
	 * Some colours with which to draw things.
	 * <p>
	 * These colours are neither final nor 'constant', so they can be user modified at runtime if desired.
	 */
	public static final Colour4f RED       = new Colour4f(1.0f, 0.0f, 0.0f, 1.0f);
	public static final Colour4f GREEN     = new Colour4f(0.0f, 1.0f, 0.0f, 1.0f);
	public static final Colour4f BLUE      = new Colour4f(0.0f, 0.0f, 1.0f, 1.0f);
	public static final Colour4f MID_RED   = new Colour4f(0.6f, 0.0f, 0.0f, 1.0f);
	public static final Colour4f MID_GREEN = new Colour4f(0.0f, 0.6f, 0.0f, 1.0f);
	public static final Colour4f MID_BLUE  = new Colour4f(0.0f, 0.0f, 0.6f, 1.0f);
	public static final Colour4f BLACK     = new Colour4f(0.0f, 0.0f, 0.0f, 1.0f);
	public static final Colour4f GREY      = new Colour4f(0.5f, 0.5f, 0.5f, 1.0f);
	public static final Colour4f WHITE     = new Colour4f(1.0f, 1.0f, 1.0f, 1.0f);
	public static final Colour4f YELLOW    = new Colour4f(1.0f, 1.0f, 0.0f, 1.0f);
	public static final Colour4f CYAN      = new Colour4f(0.0f, 1.0f, 1.0f, 1.0f);
	public static final Colour4f MAGENTA   = new Colour4f(1.0f, 0.0f, 1.0f, 1.0f);
	
	/**
	 * A Random object used to generate random numbers in the randRange methods.
	 * <p>
	 * If you want a reproducable sequence of events, then set a seed value using Utils.setRandomSeed(someValue).
	 * 
	 * @see #setRandomSeed(int)
	 * @see #randRange(float, float)
	 * @see #randRange(int, int)
	 */ 
	public static Random random = new Random();
	
	/** The maximum length in characters of any names which may be used for bones, chains or structures. */
	public static final int MAX_NAME_LENGTH = 100;
	
	private Utils() {}

	/**
	 * Set a fixed seed value - call this with any value before starting the inverse kinematics runs to get a repeatable sequence of events.
	 * 
	 * @param	seedValue	The seed value to set.
	 */
	public static void setRandomSeed(int seedValue)
	{
		random = new Random(seedValue);
	}
	
	/**
	 * Return a random floating point value between the half-open range [min..max).
	 * <p>
	 * This means that, for example, a call to {@code randRange(-5.0f, 5.0f)} may return a value between -5.0f up to a maximum of 4.999999f.
	 * 
	 * @param	min	The minimum value
	 * @param	max The maximum value
	 * @return		A random float within the specified half-open range.
	 */
	public static float randRange(float min, float max) { return random.nextFloat() * (max - min) + min; }
	
	/**
	 * Return a random integer value between the half-open range (min..max]
	 * <p>
	 * This means that, for example, a call to {@code randRange(-5, 5)} will return a value between -5 up to a maximum of 4.
	 * 
	 * @param	min	The minimum value
	 * @param	max The maximum value
	 * @return		A random int within the specified half-open range.
	 */
	public static int randRange(int min, int max) {	return random.nextInt(max - min) + min; }

	/**
	 * Return the co-tangent of an angle specified in radians.
	 * 
	 * @param	angleRads	The angle specified in radians to return the co-tangent of.
	 * @return				The co-tangent of the specified angle.
	 */  
	public static float cot(float angleRads) { return (float)( 1.0f / Math.tan(angleRads) ); }
		
	/**
	 * Convert an radians to degrees.
	 * 
	 * @param	angleRads	The angle in radians.
	 * @return				The converted angle in degrees.
	 */
	public static float radiansToDegrees(float angleRads) { return angleRads * RADS_TO_DEGS; }

	/**
	 * Convert an angle from degrees to radians.
	 * 
	 * @param	angleDegs	The angle in degrees.
	 * @return				The converted angle in radians.
	 */
	public static float degreesToRadians(float angleDegs) { return angleDegs * DEGS_TO_RADS; }	

	/**
	 * Return a FloatBuffer which can hold the specified number of floats.
	 *
	 * @param	numFloats	The number of floats this FloatBuffer should hold.
	 * @return				A float buffer which can hold the specified number of floats.
	 */
	public static FloatBuffer createFloatBuffer(int numFloats)
	{	
		return ByteBuffer.allocateDirect(numFloats * Float.BYTES).order(ByteOrder.nativeOrder()).asFloatBuffer();
	}
	
	/**
	 * Determine the sign of a float value.
	 * 
	 * @param	value	The value to return the sign of.
	 * @return			1.0f if the provided float value is positive, -1.0f otherwise.
	 */ 
	public static float sign(float value)
	{
		if (value >= 0.0f) { 
		  return 1.0f; 
		}		
		return -1.0f;
	}
	
	/**
	 * Method to set a provided seed value to be used for random number generation.
	 * <p>
	 * This allows you to have a repoducable sequence of pseudo-random numbers which are used by
	 * the visualisation MovingTarget class.
	 * 
	 * @param	seed	The seed value
	 */
	public static void setSeed(int seed) { random = new Random(seed); }
	
	/**
	 * Validate a direction unit vector (Vec2f) to ensure that it does not have a magnitude of zero.
	 * <p>
	 * If the direction unit vector has a magnitude of zero then an IllegalArgumentException is thrown.
	 * @param	directionUV	The direction unit vector to validate
	 */
	public static void validateDirectionUV(Vec2f directionUV)
	{
		// Ensure that the magnitude of this direction unit vector is greater than zero
		if ( directionUV.length() <= 0.0f )
		{
			throw new IllegalArgumentException("Vec2f direction unit vector cannot be zero.");
		}
	}
	
	/**
	 * Validate a direction unit vector (Vec3f) to ensure that it does not have a magnitude of zero.
	 * <p>
	 * If the direction unit vector has a magnitude of zero then an IllegalArgumentException is thrown.
	 * @param	directionUV	The direction unit vector to validate
	 */
	public static void validateDirectionUV(Vec3f directionUV)
	{
		// Ensure that the magnitude of this direction unit vector is greater than zero
		if ( directionUV.length() <= 0.0f )
		{
			throw new IllegalArgumentException("Vec3f direction unit vector cannot be zero.");
		}
	}
	
	/**
	 * Validate the length of a bone to ensure that it's a positive value.
	 * <p>
	 * If the provided bone length is not greater than zero then an IllegalArgumentException is thrown.
	 * @param	length	The length value to validate.
	 */
	public static void validateLength(float length)
	{
		// Ensure that the magnitude of this direction unit vector is not zero
		if (length < 0.0f)
		{
			throw new IllegalArgumentException("Length must be a greater than or equal to zero.");
		}
	}
	
	/**
	 * Convert a value in one range into a value in another range.
	 * <p>
	 * If the original range is approximately zero then the returned value is the
	 * average value of the new range, that is: (newMin + newMax) / 2.0f
	 * 
	 * @param	origValue	The original value in the original range.
	 * @param	origMin		The minimum value in the original range.
	 * @param	origMax		The maximum value in the original range.
	 * @param	newMin		The new range's minimum value.
	 * @param	newMax		The new range's maximum value.
	 * @return				The original value converted into the new range.
	 */
	public static float convertRange(float origValue, float origMin, float origMax, float newMin, float newMax)
	{	
		float origRange = origMax - origMin;
		float newRange  = newMax  - newMin;
		
		float newValue;
		if (origRange > -0.000001f && origRange < 0.000001f)
		{
		    newValue = (newMin + newMax) / 2.0f;
		}
		else
		{
			newValue = (((origValue - origMin) * newRange) / origRange) + newMin;				
		}
		
		return newValue;
	}
	
	/**
	 * Return a boolean indicating whether a float approximately equals another to within a given tolerance.
	 * 
	 * @param	a		The first value
	 * @param	b		The second value
	 * @param	tolerance	The difference within the <strong>a</strong> and <strong>b</strong> values must be within to be considered approximately equal.
	 * @return			Whether the a and b values are approximately equal or not.
	 */
	public static boolean approximatelyEquals(float a, float b, float tolerance)
	{
		return (Math.abs(a - b) <= tolerance) ? true : false; 
	}
	
	/** Ensure we have a legal line width with which to draw.
	 * <p>
	 * Valid line widths are between 1.0f and 32.0f pixels inclusive.
	 * <p>
	 * Line widths outside this range will cause an IllegalArgumentException to be thrown.
	 * 
	 * @param	lineWidth	The width of the line we are validating. 
	 */
	public static void validateLineWidth(float lineWidth)
	{
		if (lineWidth < 1.0f || lineWidth > 32.0f)
		{
			throw new IllegalArgumentException("Line widths must be within the range 1.0f to 32.0f - but only 1.0f is guaranteed to be supported.");
		}
	}
	
	/**
	 * Return the given name capped at 100 characters, if necessary.
	 * 
	 * @param	name	The name to validate.
	 * @return			The given name capped at 100 characters, if necessary.
	 */
	public static String getValidatedName(String name)
	{
		if (name.length() >= Utils.MAX_NAME_LENGTH)
		{
			return name.substring(0, Utils.MAX_NAME_LENGTH);
		} 
		else
		{	
			return name;
		}
	}

} // End of Utils clas public class Vec3f implements Vectorf<Vec3f>, Serializable
{
	private static final long serialVersionUID = 1L;
	
	// ----- Static Properties -----
	
	// Define a DecimalFormat to be used by our toString() method.
	// Note: '0' means put a 0 there if it's zero, '#' means omit if zero.
	private static DecimalFormat df = new DecimalFormat("0.000");
	
	// Conversion constants to/from degrees and radians
	private static final float DEGS_TO_RADS = (float)Math.PI / 180.0f;
	private static final float RADS_TO_DEGS = 180.0f / (float)Math.PI;

	// Cardinal axes
	private static Vec3f X_AXIS = new Vec3f(1.0f, 0.0f, 0.0f);
	private static Vec3f Y_AXIS = new Vec3f(0.0f, 1.0f, 0.0f);
	private static Vec3f Z_AXIS = new Vec3f(0.0f, 0.0f, 1.0f);
	
	// ----- Properties -----

	// A Vec3 simply has three properties called x, y and z - these are public so we can access them directly for speed
	public float x, y, z;

	// ----- Methods -----
	
	/** Default constructor - x, y, and z are initialised to zero. */
	public Vec3f() { /* x, y and z are initialised to zero by default as that's how primitives are initialised in Java*/ }

	/**
	 * Single parameter constructor sets the same value across all three components.
	 *
	 * @param	value	The value to set on the x, y and z components of this vector.
	 */
	public Vec3f(float value) { x = y = z = value; }

	/**
	 * Three parameter constructor.
	 * 
	 * @param	x	The x component value to set.
	 * @param	y	The y component value to set.
	 * @param	z	The z component value to set. 
	 */
	public Vec3f(float x, float y, float z)	{ this.x = x; this.y = y; this.z = z; }

	/**
	 * Copy constructor.
	 * 
	 * @param	source	The vector used to set component values on this newly created vector.
	 */
	public Vec3f(Vec3f source) { this.x = source.x; this.y = source.y; this.z = source.z; }

	/**
	 * Return an identical copy of the provided Vec3f.
	 *
	 * @param	source	The vector to clone. 
	 * @return		An identical copy of the provided Vec3f.
	 */
	public static Vec3f clone(Vec3f source) { return new Vec3f(source.x, source.y, source.z); }

	/** 
	 * Float setter for convenience. Note: x/y/z properties are public.
	 *
	 * @param	x	The x value to set.
	 * @param	y	The y value to set.
	 * @param	z	The z value to set.
	 */
	public void set(float x, float y, float z) { this.x = x; this.y = y; this.z = z; }

	/**
	 * {@inheritDoc}
	 */
	@Override
	public void set(Vec3f source) {	x = source.x; y = source.y; z = source.z; }

	/**
	 * {@inheritDoc}
	 */
	@Override
	public boolean approximatelyEquals(Vec3f v, float tolerance)
	{	
		if (tolerance < 0.0f)
		{
			throw new IllegalArgumentException("Equality threshold must be greater than or equal to 0.0f");
		}
		
		// Get the absolute differences between the components
		float xDiff = Math.abs(this.x - v.x);
		float yDiff = Math.abs(this.y - v.y);
		float zDiff = Math.abs(this.z - v.z);
		
		// Return true or false
		return (xDiff < tolerance && yDiff < tolerance && zDiff < tolerance);
	}
	
	/**
	 * Return whether the two provided vectors are perpendicular (to a dot-product tolerance of 0.01f).
	 *
	 * @param	a	The first vector.
	 * @param	b	The second vector.
	 * @return		Whether the two provided vectors are perpendicular (true) or not (false).
	 */
	public static boolean perpendicular(Vec3f a, Vec3f b)
	{
		return Utils.approximatelyEquals( Vec3f.dotProduct(a, b), 0.0f, 0.01f ) ? true : false;
	}
	
	/**
	 * Return whether the length of this Vec3f is approximately equal to a given value to within a given tolerance.
	 * 
	 * @param	value		The value to compare the length of this vector to.
	 * @param	tolerance	The tolerance within which the values must be to return true.
	 * @return				A boolean indicating whether the length of this vector is approximately the same as that of the provided value.
	 */
	public boolean lengthIsApproximately(float value, float tolerance)
	{
		// Check for a valid tolerance
		if (tolerance < 0.0f)
		{
			throw new IllegalArgumentException("Comparison tolerance cannot be less than zero.");
		}
		
		if ( Math.abs(this.length() - value) < tolerance)
		{
			return true;
		}
		
		return false;
	}
	
	/** Set all components of this vector to 0.0f */
	public void zero() { x = y = z = 0.0f; }

	/**
	 * Negate and return this vector.
	 * <p>
	 * Note: It is actually <em>this</em> vector which is negated and returned, not a copy / clone.
	 * 
	 * @return	This vector negated.
	 */
	public Vec3f negate()
	{
		x = -x;
		y = -y;
		z = -z;

		// Return this for chaining
		return this;
	}

	/**
	 * {@inheritDoc}
	 */
	@Override
	public Vec3f negated() { return new Vec3f(-x, -y, -z); }
	
	/**
	 * Return whether two vectors are approximately equal to within a given tolerance.
	 *
	 * @param	a		The first vector.
	 * @param	b		The second vector.
	 * @param	tolerance	The value which each component of each vector must be within to be considered approximately equal.
	 * @return			Whether the two provided vector arguments are approximately equal (true) or not (false).
	 */
	public static boolean approximatelyEqual(Vec3f a, Vec3f b, float tolerance)
	{
		if ( (Math.abs(a.x - b.x) < tolerance) &&
		     (Math.abs(a.y - b.y) < tolerance) &&
		     (Math.abs(a.z - b.z) < tolerance) )
		{
			return true;
		}
		
		return false;
	}
	
	/**
	 * {@inheritDoc}
	 */
	@Override
	public Vec3f normalise()
	{
		// Calculate the magnitude of our vector
		float magnitude = (float)Math.sqrt(x * x + y * y + z * z);

		// As long as the magnitude is greater then zero, divide each element by the
		// magnitude to get the normalised value between -1 and +1.
		// Note: If the vector has a magnitude of zero we simply return it - we
		// could instead throw a RuntimeException here... but it's better to continue.
		if (magnitude > 0.0f)
		{
			x /= magnitude;
			y /= magnitude;
			z /= magnitude;
		}
		
		// Return this for chaining
		return this;
	}
	
	/**
	 * Return a normalised version of this vector without modifying 'this' vector.
	 *
	 * @return	A normalised version of this vector.
	 */
	public Vec3f normalised() { return new Vec3f(this).normalise(); }
	
	/**
	 * Return the scalar product of two vectors.
	 * <p>
	 * If the provided vectors are normalised then this will be the same as the dot product.
	 * 
	 * @param	v1	The first vector.
	 * @param	v2	The second vector.
	 * @return		The scalar product of the two vectors.
	 */
	public static float scalarProduct(Vec3f v1, Vec3f v2) {	return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;	}

	/**
	 * Return the scalar product of two vectors.
	 * <p>
	 * Normalised versions of the provided vectors are used in the dot product operation.
	 * 
	 * @param	v1	The first vector.
	 * @param	v2	The second vector.
	 * @return		The dot product of the two vectors.
	 */
	public static float dotProduct(Vec3f v1, Vec3f v2)
	{
		Vec3f v1Norm = v1.normalised();
		Vec3f v2Norm = v2.normalised();
		
		return v1Norm.x * v2Norm.x + v1Norm.y * v2Norm.y + v1Norm.z * v2Norm.z;
	}

	/**
	 * Calculate and return a vector which is the cross product of the two provided vectors.
	 * <p>
	 * The returned vector is not normalised.
	 * 
	 * @param	v1	The first vector.
	 * @param	v2	The second vector.
	 * @return		The non-normalised cross-product of the two vectors v1-cross-v2.
	 */
	public static Vec3f crossProduct(Vec3f v1, Vec3f v2) { return new Vec3f(v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z, v1.x * v2.y - v1.y * v2.x); }

	/**
	 * Calculate and return a vector which is the cross product of this vector and another vector.
	 * <p>
	 * The returned vector is not normalised.
	 * 
	 * @param	v	The Vec3f with which we will cross product this Vec3f.
	 * @return		The non-normalised cross product if the two vectors this-cross-v.
	 */
	public Vec3f cross(Vec3f v) { return new Vec3f(y * v.z - z * v.y,	z * v.x - x * v.z, x * v.y - y * v.x); }

	/**
	 * Calculate and return the distance between two points in 3D space.
	 * 
	 * @param	v1	The first point.
	 * @param	v2	The second point.
	 * @return		The distance between the two points.
	 */
	public static float distanceBetween(Vec3f v1, Vec3f v2)
	{
		float dx = v2.x - v1.x;
		float dy = v2.y - v1.y;
		float dz = v2.z - v1.z;
		return (float)Math.sqrt(dx * dx + dy * dy + dz * dz);
	}
	
	/**
	 * Calculate and return the Manhattan distance between two Vec3f objects.
	 * <p>
	 * The Manhattan distance is an approximate distance between two points, but
	 * can be calculated faster than the exact distance.
	 * <p>
	 * Further reading:
	 *     http://en.wikipedia.org/wiki/floataxicab_geometry
	 *     http://stackoverflow.com/questions/3693514/very-fast-3d-distance-check
	 *  
	 * @param	v1	The first point.
	 * @param	v2	The second point.
	 * @return		The Manhattan distance between the two points.
	 */
	public static float manhattanDistanceBetween(Vec3f v1, Vec3f v2) { return Math.abs(v2.x - v1.x) + Math.abs(v2.x - v1.x) + Math.abs(v2.x - v1.x); }
	
	/**
	 * Return whether two locations are within a given manhattan distance of each other.
	 * <p>
	 * The manhattan distance is an approximate distance between two points, but
	 * can be calculated faster than the exact distance.
	 * <p>
	 * Further reading:
	 *     http://en.wikipedia.org/wiki/floataxicab_geometry
	 *     http://stackoverflow.com/questions/3693514/very-fast-3d-distance-check
	 *  
	 * @param	v1	The first location vector
	 * @param	v2	The second location vector
	 * @return	boolean
	 */
	boolean withinManhattanDistance(Vec3f v1, Vec3f v2, float distance)
	{	
		if (Math.abs(v2.x - v1.x) > distance) return false; // Too far in x direction
		if (Math.abs(v2.y - v1.y) > distance) return false; // Too far in y direction
		if (Math.abs(v2.z - v1.z) > distance) return false; // Too far in z direction	
		return true;
	}
	
	/**
	 * {@inheritDoc}
	 */
	@Override
	public float length() { return (float)Math.sqrt(x * x + y * y + z * z); }
	
	/**
	 * Return a component-wise absolute version (i.e. all components are positive) of this vector.
	 * <p>
	 * Note: This vector itself is not modified - a new vector is created, each component is made positive, and the new vector is returned.
	 *
	 * @param	source	The vector to make absolute.
	 * @return		A component-wise absolute version of this vector.
	 */
	public static Vec3f abs(Vec3f source)
	{
		Vec3f absVector = new Vec3f();

		if (source.x < 0.0f) { absVector.x = -source.x; } else { absVector.x = source.x; }
		if (source.y < 0.0f) { absVector.y = -source.y; } else { absVector.y = source.y; }
		if (source.z < 0.0f) { absVector.z = -source.z; } else { absVector.z = source.z; }

		return absVector;
	}

	/**
	 * Return a normalised Vec3f which is perpendicular to the vector provided.
	 * <p>
	 * This is a very fast method of generating a perpendicular vector that works for any vector
	 * which is 5 degrees or more from vertical 'up'.
	 * <p>
	 * The code in this method is adapted from: http://blog.selfshadow.com/2011/10/17/perp-vectors/
	 *
	 * @param	u	The vector to use as the basis for generating the perpendicular vector.
	 * @return		A normalised vector which is perpendicular to the provided vector argument.
	 */
	public static Vec3f genPerpendicularVectorQuick(Vec3f u)
	{
		Vec3f perp;
		
		if (Math.abs(u.y) < 0.99f)
		{
			perp = new Vec3f(-u.z, 0.0f, u.x); // cross(u, UP)
		}
		else
		{
			perp = new Vec3f(0.0f, u.z, -u.y); // cross(u, RIGHT)
		}
		
		return perp.normalise();
	}
	
	/**
	 * Method to generate a vector perpendicular to another one using the Hughes-Muller method.
	 * <p>
	 * The returned vector is normalised.
	 * <p>
	 * The code in this method is adapted from: http://blog.selfshadow.com/2011/10/17/perp-vectors/
	 * <p>
	 * Further reading: Hughes, J. F., Muller, T., "Building an Orthonormal Basis from a Unit Vector", Journal of Graphics Tools 4:4 (1999), 33-35.
	 * 
	 * @param	u	The vector with regard to which we will generate a perpendicular unit vector.
	 * @return		A normalised vector which is perpendicular to the provided vector argument.
	 */
	public static Vec3f genPerpendicularVectorHM(Vec3f u)
	{
		// Get the absolute source vector
		Vec3f a = Vec3f.abs(u);

		if (a.x <= a.y && a.x <= a.z)
		{
			return new Vec3f(0.0f, -u.z, u.y).normalise();
		}
		else if (a.y <= a.x && a.y <= a.z)
		{
			return new Vec3f(-u.z, 0.0f, u.x).normalise();
		}
		else
		{
			return new Vec3f(-u.y, u.x, 0.0f).normalise();
		}
	}
	
	// TODO: Fix up and document properly
	// Further reading: Stark, M. M., "Efficient Construction of Perpendicular Vectors without Branching", Journal of Graphics Tools 14:1 (2009), 55-61.
//	Vec3f genPerpendicularVectorStark(Vec3f u)
//	{
//		// Get the absolute source vector
//	    Vec3f a = Vec3f.abs(u);
//
//	    unsigned int = SIGNBIT(a.x - a.y);
//	    uint uzx = SIGNBIT(a.x - a.z);
//	    uint uzy = SIGNBIT(a.y - a.z);
//
//	    uint xm = uyx & uzx;
//	    uint ym = (1^xm) & uzy;
//	    uint zm = 1^(xm & ym);
//
//	    float3 v = cross(u, float3(xm, ym, zm));
//	    return v;
//	}
	

	//TODO: Test if better than Quick version and document.
	/**
	 * Method to generate a vector perpendicular to another one using the Frisvad method.
	 * <p>
	 * The returned vector is normalised.
	 * 
	 * @param	u	The vector with regard to which we will generate a perpendicular unit vector.
	 * @return		A normalised vector which is perpendicular to the provided vector argument.
	 */
	public static Vec3f genPerpendicularVectorFrisvad(Vec3f u)
	{
	    if (u.z < -0.9999999f) // Handle the singularity
	    {
	      return new Vec3f(0.0f, -1.0f, 0.0f);
	      //b2 = Vec3f(-1.0f,  0.0f, 0.0f);
	      //return;
	    }
	    
	    float a = 1.0f/(1.0f + u.z);
	    //float b = -n.x*n.y*a;
	    return new Vec3f(1.0f - u.x * u.x * a, -u.x * u.y * a, -u.x).normalised();
	    //b2 = Vec3f(b, 1.0f - n.y*n.y*a, -n.y);
	}

	/**
	 * Return the unit vector between two provided vectors.
	 *
	 * @param	v1	The first vector.
	 * @param	v2	The second vector.
	 * @return		The unit vector between the two provided vector arguments.
	 */
	public static Vec3f getUvBetween(Vec3f v1, Vec3f v2) { return new Vec3f( v2.minus(v1) ).normalise(); }

	/**
	 * Calculate and return the angle between two vectors in radians.
	 * <p>
	 * The result will always be a positive value between zero and pi (3.14159f) radians.
	 * <p>
	 * This method does not modify the provided vectors, but does use normalised versions of them in the calculations.
	 * 
	 * @param	v1	The first vector.
	 * @param	v2	The second vector.
	 * @return		The angle between the vector in radians.
	 */
	public static float getAngleBetweenRads(Vec3f v1, Vec3f v2)
	{
		// Note: a and b are normalised within the dotProduct method.
		return (float)Math.acos( Vec3f.dotProduct(v1,  v2) );
	}

	/**
	 * Calculate and return the angle between two vectors in degrees.
	 * <p>
	 * The result will always be a positive value between [0..180) degrees.
	 * <p>
	 * This method does not modify the provided vectors, but does use normalised versions of them in the calculations.
	 * 
	 * @param	v1	The first vector.
	 * @param	v2	The second vector.
	 * @return		The angle between the vector in degrees.
	 */
	public static float getAngleBetweenDegs(Vec3f v1, Vec3f v2) { return Vec3f.getAngleBetweenRads(v1, v2) * RADS_TO_DEGS; }
	
	/**
	 * Return a signed angle between two vectors within the range -179.9f..180.0f degrees.
	 *
	 * @param	referenceVector	The baseline vector which we consider to be at zero degrees.
	 * @param	otherVector		The vector we will use to calculate the signed angle with respect to the reference vector.
	 * @param	normalVector	The normal vector (i.e. vector perpendicular to) both the reference and 'other' vectors.
	 * @return					The signed angle from the reference vector to the other vector in degrees.
	 **/
	public static float getSignedAngleBetweenDegs(Vec3f referenceVector, Vec3f otherVector, Vec3f normalVector)
	{
		float unsignedAngle = Vec3f.getAngleBetweenDegs(referenceVector, otherVector);
		float sign          = Utils.sign( Vec3f.dotProduct(Vec3f.crossProduct(referenceVector, otherVector), normalVector));		
		return unsignedAngle * sign;
	}
  
	/**
	 * Return an angle limited vector with regard to another vector.
	 * <p>
	 * @param	vecToLimit		The vector which we will limit to a given angle with regard to the the baseline vector.
	 * @param	vecBaseline		The vector which will be used as the baseline / frame-of-reference when rotating the vecToLimit.
	 * @param	angleLimitDegs	The maximum angle which the vecToLimit may be rotated away from the vecBaseline, in degrees.
	 * @return					The rotated vecToLimit, which is constraint to a maximum of the angleLimitDegs argument.
	 */
    public static Vec3f getAngleLimitedUnitVectorDegs(Vec3f vecToLimit, Vec3f vecBaseline, float angleLimitDegs)
    {
    	// Get the angle between the two vectors
    	// Note: This will ALWAYS be a positive value between 0 and 180 degrees.
        float angleBetweenVectorsDegs = Vec3f.getAngleBetweenDegs(vecBaseline, vecToLimit);
        
        if (angleBetweenVectorsDegs > angleLimitDegs)
        {        	
        	// The axis which we need to rotate around is the one perpendicular to the two vectors - so we're
            // rotating around the vector which is the cross-product of our two vectors.
        	// Note: We do not have to worry about both vectors being the same or pointing in opposite directions
        	// because if they bones are the same direction they will not have an angle greater than the angle limit,
        	// and if they point opposite directions we will approach but not quite reach the precise max angle
        	// limit of 180.0f (I believe).
            Vec3f correctionAxis = Vec3f.crossProduct(vecBaseline.normalised(), vecToLimit.normalised() ).normalise();
            
            // Our new vector is the baseline vector rotated by the max allowable angle about the correction axis
            return Vec3f.rotateAboutAxisDegs(vecBaseline, angleLimitDegs, correctionAxis).normalised();
        }
        else // Angle not greater than limit? Just return a normalised version of the vecToLimit
        {
        	// This may already BE normalised, but we have no way of knowing without calcing the length, so best be safe and normalise.
        	// TODO: If performance is an issue, then I could get the length, and if it's not approx. 1.0f THEN normalise otherwise just return as is.
            return vecToLimit.normalised();
        }
    }

	/**
	 * Return the global pitch of this vector about the global X-Axis. The returned value is within the range -179.9f..180.0f 
degrees.
	 *
	 * @return	The pitch of the vector in degrees.
	 **/
	public float getGlobalPitchDegs()
	{
		Vec3f xProjected = this.projectOntoPlane(X_AXIS);
		float pitch = Vec3f.getAngleBetweenDegs( Z_AXIS.negated(), xProjected);
		return xProjected.y < 0.0f ? -pitch : pitch;
	}

	/**
	 * Return the global yaw of this vector about the global Y-Axis. The returned value is within the range -179.9f..180.0f 
degrees.
	 *
	 * @return	The yaw of the vector in degrees.
	 **/
	public float getGlobalYawDegs()
	{
		Vec3f yProjected = this.projectOntoPlane(Y_AXIS);
		float yaw = Vec3f.getAngleBetweenDegs( Z_AXIS.negated(), yProjected);
		return yProjected.x < 0.0f ? -yaw : yaw;
	}



    /**
     * Rotate a Vec3f about the world-space X-axis by a given angle specified in radians.
     * 
     * @param	source		The vector to rotate.
     * @param	angleRads	The angle to rotate the vector in radians.
     * @return				A rotated version of the vector.
     */ 
 	public static Vec3f rotateXRads(Vec3f source, float angleRads)
 	{
 		// Rotation about the x-axis:
 		// x' = x
 		// y' = y*cos q - z*sin q
 		// z' = y*sin q + z*cos q

 		float cosTheta = (float)Math.cos(angleRads);
 		float sinTheta = (float)Math.sin(angleRads);
 		
 		return new Vec3f(source.x, source.y * cosTheta - source.z * sinTheta, source.y * sinTheta + source.z * cosTheta);
 	}
 	
 	/**
     * Rotate a Vec3f about the world-space X-axis by a given angle specified in degrees.
     * 
     * @param	source		The vector to rotate.
     * @param	angleDegs	The angle to rotate the vector in degrees.
     * @return				A rotated version of the vector.
     */
 	public static Vec3f rotateXDegs(Vec3f source, float angleDegs) { return Vec3f.rotateXRads(source, angleDegs * DEGS_TO_RADS); }
	
 	/**
     * Rotate a Vec3f about the world-space Y-axis by a given angle specified in radians.
     * 
     * @param	source		The vector to rotate.
     * @param	angleRads	The angle to rotate the vector in radians.
     * @return				A rotated version of the vector.
     */
	public static Vec3f rotateYRads(Vec3f source, float angleRads)
	{
		// Rotation about the y axis:
		// x' = z*sin q + x*cos q
		// y' = y
		// z' = z*cos q - x*sin q

		float cosTheta = (float)Math.cos(angleRads);
		float sinTheta = (float)Math.sin(angleRads);
		
		return new Vec3f(source.z * sinTheta + source.x * cosTheta, source.y, source.z * cosTheta - source.x * sinTheta); 
	}

	/**
     * Rotate a Vec3f about the world-space Y-axis by a given angle specified in degrees.
     * 
     * @param	source		The vector to rotate.
     * @param	angleDegs	The angle to rotate the vector in degrees.
     * @return				A rotated version of the vector.
     */
	public static Vec3f rotateYDegs(Vec3f source, float angleDegs) { return Vec3f.rotateYRads(source, angleDegs * DEGS_TO_RADS); }
	
	/**
     * Rotate a Vec3f about the world-space Z-axis by a given angle specified in radians.
     * 
     * @param	source		The vector to rotate.
     * @param	angleRads	The angle to rotate the vector in radians.
     * @return				A rotated version of the vector.
     */
	public static Vec3f rotateZRads(Vec3f source, float angleRads)
	{
		// Rotation about the z-axis:
		// x' = x*cos q - y*sin q
		// y' = x*sin q + y*cos q
		// z' = z

		float cosTheta = (float)Math.cos(angleRads);
		float sinTheta = (float)Math.sin(angleRads);
		
		return new Vec3f(source.x * cosTheta - source.y * sinTheta, source.x * sinTheta + source.y * cosTheta, source.z); 
	}
		
	/**
     * Rotate a Vec3f about the world-space Z-axis by a given angle specified in degrees.
     * 
     * @param	source		The vector to rotate.
     * @param	angleDegs	The angle to rotate the vector in degrees.
     * @return				A rotated version of the vector.
     */
	public static Vec3f rotateZDegs(Vec3f source, float angleDegs) { return Vec3f.rotateZRads(source, angleDegs * DEGS_TO_RADS); }
		
	/**
	 * Rotate a source vector an amount in radians about an arbitrary axis.
	 * 
	 * @param source		The vector to rotate.
	 * @param angleRads		The amount of rotation to perform in radians.
	 * @param rotationAxis	The rotation axis.
	 * @return				The source vector rotated about the rotation axis.
	 */
	public static Vec3f rotateAboutAxisRads(Vec3f source, float angleRads, Vec3f rotationAxis)
	{
		Mat3f rotationMatrix = new Mat3f();

		float sinTheta         = (float)Math.sin(angleRads);
		float cosTheta         = (float)Math.cos(angleRads);
		float oneMinusCosTheta = 1.0f - cosTheta;
		
		// It's quicker to pre-calc these and reuse than calculate x * y, then y * x later (same thing).
		float xyOne = rotationAxis.x * rotationAxis.y * oneMinusCosTheta;
		float xzOne = rotationAxis.x * rotationAxis.z * oneMinusCosTheta;
		float yzOne = rotationAxis.y * rotationAxis.z * oneMinusCosTheta;
		
		// Calculate rotated x-axis
		rotationMatrix.m00 = rotationAxis.x * rotationAxis.x * oneMinusCosTheta + cosTheta;
		rotationMatrix.m01 = xyOne + rotationAxis.z * sinTheta;
		rotationMatrix.m02 = xzOne - rotationAxis.y * sinTheta;

		// Calculate rotated y-axis
		rotationMatrix.m10 = xyOne - rotationAxis.z * sinTheta;
		rotationMatrix.m11 = rotationAxis.y * rotationAxis.y * oneMinusCosTheta + cosTheta;
		rotationMatrix.m12 = yzOne + rotationAxis.x * sinTheta;

		// Calculate rotated z-axis
		rotationMatrix.m20 = xzOne + rotationAxis.y * sinTheta;
		rotationMatrix.m21 = yzOne - rotationAxis.x * sinTheta;
		rotationMatrix.m22 = rotationAxis.z * rotationAxis.z * oneMinusCosTheta + cosTheta;

		// Multiply the source by the rotation matrix we just created to perform the rotation
		return rotationMatrix.times(source);
	}

	/**
	 * Rotate a source vector an amount in degrees about an arbitrary axis.
	 * 
	 * @param source		The vector to rotate.
	 * @param angleDegs		The amount of rotation to perform in degrees.
	 * @param rotationAxis	The rotation axis.
	 * @return				The source vector rotated about the rotation axis.
	 */
	public static Vec3f rotateAboutAxisDegs(Vec3f source, float angleDegs, Vec3f rotationAxis)
	{
		return Vec3f.rotateAboutAxisRads(source, angleDegs * DEGS_TO_RADS, rotationAxis);
	}
	
	// Overloaded toString method
	@Override
	public String toString()
	{
		StringBuilder sb = new StringBuilder();
		sb.append("x: " + df.format(x) + ", y: " + df.format(y) + ", z: " + df.format(z) );
		return sb.toString();
	}


	/**
	 * {@inheritDoc}
	 */
	@Override
	public Vec3f plus(Vec3f v) { return new Vec3f(this.x + v.x, this.y + v.y, this.z + v.z); }

	/**
	 * {@inheritDoc}
	 */
	@Override
	public Vec3f minus(Vec3f v) { return new Vec3f(this.x - v.x, this.y - v.y, this.z - v.z); }
	
	/**
	 * Return a vector which is the result of multiplying this vector by another vector. This vector remains unchanged.
	 * 
	 * @param	v	The vector to multiply this vector by.
	 * @return		The result of multiplying this vector by the 'v' vector.
	 **/
	public Vec3f times(Vec3f v) { return new Vec3f(this.x * v.x, this.y * v.y, this.z * v.z); }

	/**
	 * {@inheritDoc}
	 */
	@Override
	public Vec3f times(float scale) { return new Vec3f(this.x * scale, this.y * scale, this.z * scale); }
	
	/**
	 * Multiply the value of a Vec3f in place by a given scaling factor.
	 * 
	 * @param	v		The vector to scale.
	 * @param	scale	The value used to scale each component of the 'v' vector.
	 **/
	public static void times(Vec3f v, float scale) { v.x *= scale; v.y *= scale; v.z *= scale; }
	
	/**
	 * Add a vector to a source vector - the source vector is modified.
	 * <p>
	 * This method does not perform any memory allocations - it merely adds 'other' to 'source'.
	 * 
	 *  @param	source	The vector to which we will add a vector.
	 *  @param	other	The vector we will add to the 'source' vector.
	 */
	public static void add(Vec3f source, Vec3f other) { source.x += other.x; source.y += other.y; source.z += other.z; }
	
	/**
	 * Subtract a vector from a source vector - the source vector is modified.
	 * <p>
	 * This method does not perform any memory allocations - it merely subtracts 'other' from 'source'.
	 * 
	 *  @param	source	The vector to which we will subtract a vector.
	 *  @param	other	The vector we will suctract from the 'source' vector.
	 */
	public static void subtract(Vec3f source, Vec3f other) { source.x -= other.x; source.y -= other.y; source.z -= other.z; }

	/**
	 * {@inheritDoc}
	 */
	@Override
	public Vec3f dividedBy(float value) { return new Vec3f(this.x / value, this.y / value, this.z / value);	}
	
	/**
	 * Return a vector which is the result of projecting this vector onto a plane described by the provided surface normal.
	 * <p>
	 * Neither the vector on which this method is called or the provided plane normal vector are modified.
	 * <p>
	 * If the plane surface normal has a magnitude of zero then an IllegalArgumentException is thrown.
	 *  
	 * @param	planeNormal	The normal that describes the plane onto which we will project this vector.
	 * @return				A projected version of this vector.
	 */
	public Vec3f projectOntoPlane(Vec3f planeNormal)
	{
		if ( !(planeNormal.length() > 0.0f) ) {	throw new IllegalArgumentException("Plane normal cannot be a zero vector."); }
		
		// Projection of vector b onto plane with normal n is defined as: b - ( b.n / ( |n| squared )) * n
		// Note: |n| is length or magnitude of the vector n, NOT its (component-wise) absolute value		
		Vec3f b = this.normalised();
		Vec3f n = planeNormal.normalised();		
		return b.minus( n.times( Vec3f.dotProduct(b, planeNormal) ) ).normalise();
		
		/** IMPORTANT: We have to be careful here - even code like the below (where dotProduct uses normalised
		 *             versions of 'this' and planeNormal is off by enough to make the IK solutions oscillate:
		 *
		 *             return this.minus( planeNormal.times( Vec3f.dotProduct(this, planeNormal) ) ).normalised();
		 *             
		 */
				
		// Note: For non-normalised plane vectors we can use:
		// float planeNormalLength = planeNormal.length();
		// return b.minus( n.times( Vec3f.dotProduct(b, n) / (planeNormalLength * planeNormalLength) ).normalised();
	}
	
	/**
	 * Calculate and return the direction unit vector from point a to point b.
	 * <p>
	 * If the opposite direction is required then the argument order can be swapped or the the result can simply be negated.
	 * 
	 * @param	v1	The first location.
	 * @param	v2	The second location.
	 * @return		The normalised direction unit vector between point v1 and point v2.
	 */
	public static Vec3f getDirectionUV(Vec3f v1, Vec3f v2) { return v2.minus(v1).normalise(); }	
	
	/**
	 * Randomise the components of this vector to be random values between the provided half-open range as described by the minimum and maximum value arguments.
	 *
	 * @param	min	The minimum value for any given component (inclusive).
	 * @param	max	The maximum value for any given component (exclusive, i.e. a max of 5.0f will be assigned values up to 4.9999f or such).
	 **/
	public void randomise(float min, float max)
	{
		this.x = Utils.randRange(min, max);
		this.y = Utils.randRange(min, max);
		this.z = Utils.randRange(min, max);
	}

  @Override
  public int hashCode() {
    final int prime = 31;
    int result = 1;
    result = prime * result + Float.floatToIntBits(x);
    result = prime * result + Float.floatToIntBits(y);
    result = prime * result + Float.floatToIntBits(z);
    return result;
  }

  @Override
  public boolean equals(Object obj) {
    if (this == obj) {
      return true;
    }
    if (obj == null) {
      return false;
    }
    if (getClass() != obj.getClass()) {
      return false;
    }
    Vec3f other = (Vec3f) obj;
    if (Float.floatToIntBits(x) != Float.floatToIntBits(other.x)) {
      return false;
    }
    if (Float.floatToIntBits(y) != Float.floatToIntBits(other.y)) {
      return false;
    }
    if (Float.floatToIntBits(z) != Float.floatToIntBits(other.z)) {
      return false;
    }
    return true;
  }	
	
} // End of Vec3f class
import au.edu.federation.caliko.FabrikStructure3D;
import au.edu.federation.caliko.demo3d.CalikoDemoStructure3D;
import au.edu.federation.caliko.demo3d.CalikoDemoStructure3DFactory;
import au.edu.federation.caliko.visualisation.Axis;
import au.edu.federation.caliko.visualisation.Camera;
import au.edu.federation.caliko.visualisation.FabrikConstraint3D;
import au.edu.federation.caliko.visualisation.FabrikLine3D;
import au.edu.federation.caliko.visualisation.FabrikModel3D;
import au.edu.federation.caliko.visualisation.Grid;
import au.edu.federation.caliko.visualisation.MovingTarget3D;
import au.edu.federation.utils.Mat4f;
import au.edu.federation.utils.Utils;
import au.edu.federation.utils.Vec3f;
package au.edu.federation.caliko.demo;


/**
 * Class to demonstrate some of the features of the Caliko library in 3D.
 * 
 * @author Al Lansley
 * @version 0.7.1 - 20/07/2016
 */
public class CalikoDemo3D implements CalikoDemo
{
	static float defaultBoneLength      = 10.0f;
	static float boneLineWidth          = 5.0f;
	static float constraintLineWidth    = 2.0f;	
	static float baseRotationAmountDegs = 0.3f;
	
	// Set yo a camera which we'll use to navigate. Params: location, orientation, width and height of window.
	static Camera camera = new Camera(new Vec3f(0.0f, 00.0f, 150.0f), new Vec3f(), Application.windowWidth, Application.windowHeight);
		
	// Setup some grids to aid orientation
	static float extent       = 1000.0f;
	static float gridLevel    = 100.0f;
	static int   subdivisions = 20;
	static Grid  lowerGrid    = new Grid(extent, extent, -gridLevel, subdivisions);