@XmlRootElement
public class ObjectLanguageInstance implements Serializable 
{
	private static final long serialVersionUID = 1L;

	public String uniqueID;
	public String name;
	public String type;

	//Objects can be added with the least amount of information possible
	//The ultimate goal is that a physical representation is present but initially
	//the object will simply have a name
	public ObjectPhysicalInstance physicalRepresentation = null;
	
	//The state of an object (property value)
	public String[] properties_name = new String[]{};
	public String[] properties_value = new String[]{};

	//An object's pose can consist of many separate poses e.g. head pose, arm pose, face expression etc.
	//Each pose can have name value properties
	
	//This refers to pose models (as pose instances
	public String[] poseModels = new String[]{};
	//which role within the pose e.g. sat on or sitting
	public int[] poseRole = new int[]{};
	
	//Relations between objects (typically only refers to objects near to this object (as could be deduced by sensing the object)
	//For example
	//Relative position in space
//	public String[] relations_with = new String[]{};
//	public String[] relations_with_type = new String[]{};
//	public String[][] relations_properties = new String[][]{};
	
	//Objects can have named sub parts (they are not necessarily exclusive e.g. front of person, torso, rib cage,
//	public String[] parts = new String[]{};
		
	//For example: 
	//Relative position in space
	//Supported by
	//Directed at
//	public String[] parts_relations_with = new String[]{};
//	public String[] parts_relations_with_type = new String[]{};
//	//e.g. feet resting on the top of a bed
//	public String[] parts_relations_part = new String[]{};
//	public String[][] parts_relations_properties = new String[][]{};
	

	//Special relations relating to how space is defined
	//important for prioritising what to show or reason about

	//The object we are within (usually a room)
	public String[] within = new String[]{};
	public String[] within_type = new String[]{};

	//for objects like clothes, furniture, vehicles and rooms which have objects within them 
	public String[] contains = new String[]{};
	public String[] contains_type = new String[]{};

	public String[] contains_poseModels = new String[]{};
	//indicates an instance or a model
	//public String[] contains_poses_type = new String[]{};
	//mapping contained instances to the instances defined by the pose
	public String[][] contains_poses_instances = new String[][]{};

	//objects, such as buildings, can have objects within them, such as rooms, that themselves
	//contain objects
	//public String[] portals_to_internal_objects = null;

	public static String uniqueIdToPath(String uid)
	{
		int ind = uid.indexOf("/");
		String concept = uid.substring(0,ind);
		String instance = uid.substring(ind+1);
		return "Concepts/"+concept+"/instances/"+instance;
	}
	
	public static ObjectLanguageInstance loadObject(FileStoreInterface fs,String path)
	{
		if(fs.exists(path))
		{
	      try {
		         FileInputStream fileIn = new FileInputStream(path);
		         ObjectInputStream in = new ObjectInputStream(fileIn);
		         ObjectLanguageInstance o = (ObjectLanguageInstance) in.readObject();
		         in.close();
		         fileIn.close();
		         
		         StateToXML.objectToXMLFile(o);
		         
		         return o;
		      }catch(IOException i) {
		         i.printStackTrace();
		         return null;
		      }catch(ClassNotFoundException c) {
		         System.out.println("Employee class not found");
		         c.printStackTrace();
		         return null;
		      }
		}
		else
		{
			return null;
		}
	}

	public static void saveObject(ObjectLanguageInstance o,FileStoreInterface fs,String path)
	{
		try {
			fs.rename(path, "~"+path);
//			 File fout = new File(path);
//			 if(fout.exists())
//				 fout.delete();
	         FileOutputStream fileOut =
	         new FileOutputStream(path);
	         ObjectOutputStream out = new ObjectOutputStream(fileOut);
	         out.writeObject(o);
	         out.close();
	         fileOut.close();
	         
	         StateToXML.objectToXMLFile(o);
	      }
		catch(IOException i) 
	    {
	         i.printStackTrace();
	    }
	}

	public void moveContainedTo(ObjectLanguageInstance o,
				GeneralMatrixDouble zrot)
	{
		int ind = GeneralMatrixString.find(o.within, uniqueID);
		
		for(int j=0;j<3;j++)
		{
			for(int i=0;i<3;i++)
			{
				o.physicalRepresentation.within_metric_transform[12*ind+3*j+i] = zrot.value[4*j+i];
			}
		}
		o.physicalRepresentation.within_metric_transform[12*ind+9+0] = zrot.value[4*3+0];
		o.physicalRepresentation.within_metric_transform[12*ind+9+1] = zrot.value[4*3+1];
		o.physicalRepresentation.within_metric_transform[12*ind+9+2] = zrot.value[4*3+2];

		int cind = GeneralMatrixString.find(contains, o.uniqueID);

		for(int j=0;j<3;j++)
		{
			for(int i=0;i<3;i++)
			{
				physicalRepresentation.contains_metric_transform[12*cind+3*j+i] = zrot.value[4*j+i];
			}
		}
		physicalRepresentation.contains_metric_transform[12*cind+9+0] = zrot.value[4*3+0];
		physicalRepresentation.contains_metric_transform[12*cind+9+1] = zrot.value[4*3+1];
		physicalRepresentation.contains_metric_transform[12*cind+9+2] = zrot.value[4*3+2];
	}

	public void add(ObjectLanguageInstance i)
	{
		i.within = ArrayUtils.add(i.within,uniqueID);
		i.within_type = ArrayUtils.add(i.within_type,type);
		contains = ArrayUtils.add(contains,i.uniqueID);
		contains_type = ArrayUtils.add(contains_type,i.type);
	}
	public void remove(ObjectLanguageInstance i)
	{		
		int ind = ArrayUtils.getIndex(i.within,uniqueID);
		int cind = ArrayUtils.getIndex(contains,i.uniqueID);

		physicalRepresentation.remove(i.physicalRepresentation, ind, cind);
		
		i.within = ArrayUtils.remove(i.within,ind,1);
		i.within_type = ArrayUtils.remove(i.within,ind,1);

		contains = ArrayUtils.remove(contains,cind,1);
		contains_type = ArrayUtils.remove(contains_type,cind,1);
	}

} public class FileStoreInterface 
{
	private static final int BUFFER = 8192;

	public String root = "/";
	
	//Locked files, with temp files that have been changed
	
	//This class is used to make accessing and changing large collections of files more reliable
	//The MVStore database used in the DatabaseInterface is fast and reliable 
	//however it is designed to load stored data entirely into memory
	//if large files, such as videos, are stored directly in the database 
	//then when they are accessed they will be entirely loaded into the computers memory
	//this will probably crash the program or at the very least make it run unreliably
	//for this reason larger files are typically stored directly on the hard disk or remote
	//server
	//
	public FileStoreInterface(String rootpath)
	{
		File rootf = new File(rootpath);
		try
		{
			root = rootf.getCanonicalPath();
		}
		catch(Exception e)
		{
			System.out.println(e.toString());
		}
	}
	
	public void createAllParentDirectories(String dir)
	{
		String[] split = dir.split("/");
		String path = "";
		for(int i=1;i<(split.length-1);i++)
		{
			path += "/"+split[i];
			if(!exists(path))
				createDirectory(path);
		}
	}

	public String decodeFilePath( String uri )
	{
		String path = WebSecurity.decodePercent(uri);

		try
		{
			File f = new File(root+"/"+path);
			String canonical = f.getCanonicalPath();
			if(canonical.startsWith(root))
				return canonical;
			else
				return null;
		}
		catch(Exception e)
		{
			return null;
		}
	}

	public long lockForRead(String file)
	{
		//is the file already locked?
		return 0;
	}
	
	public long lockForWrite(String file)
	{
		//is the file already locked?
		return 0;
	}
	
	public void unlockForRead(String file,long id)
	{
		
	}
	public void unlockForWrite(String file,long id)
	{
		
	}
	
	//A queue of file operations that have been blocked because of a lock on the file
	public void processQueue()
	{
		//Go through the file operations in the queue and if the file is not locked
	}
	
	public boolean exists(String dir)
	{
		File dirf = new File(dir);
		return dirf.exists()&&dirf.isFile();
	}


	public String newuniquefile(String in)
	{
		if(!exists(in))
			return in;
		int num = 0;
		while(true)
		{
			String tryf = in+num;
			if(!exists(tryf))
				return tryf;
			num++;
		}
	}

	public boolean copyFile(String in, String out) 
	{
		File inf = new File(in);
		File outf = new File(out);
		return copyFile(inf, outf);
	}
	public boolean swapFiles(String in, String out) 
	{
		File inf = new File(in);
		File outf = new File(out);
		File outft = new File(newuniquefile(out));
		inf.renameTo(outft);
		outf.renameTo(inf);
		outft.renameTo(outf);
		return true;
	}
	public boolean moveFile(File file,File intodir)
	{
		boolean success = file.renameTo(new File(intodir, file.getName()));
		return success;
	}
	
	public static void deepCopy(File dir,String toFolder,GeneralMatrixString replace)
	{
		(new File(toFolder)).mkdir();
		File[] cs = dir.listFiles();
		for(int i=0;i<cs.length;i++)
		{
			if(cs[i].isFile())
			{
				GeneralMatrixString lines = new GeneralMatrixString(1);
				TextImportExport.loadTextFile(cs[i].getAbsolutePath(), lines);
				for(int li=0;li<lines.height;li++)
				{
					for(int ri=0;ri<replace.height;ri++)
					{
						String in = replace.value[ri*2+0];
						String out = replace.value[ri*2+1];
						
						if(lines.value[li].contains(in))
						{
							System.out.println(in);
						}
						lines.value[li] = lines.value[li].replace(in, out);
					}
				}
				TextImportExport.saveTextFile(toFolder+"/"+cs[i].getName(), lines);;
			}
			else
			{
				deepCopy(cs[i],toFolder+"/"+cs[i].getName(),replace);
			}
		}
	}

	public void deepCopy(File dir,String toFolder)
	{
		(new File(toFolder)).mkdir();
		File[] cs = dir.listFiles();
		for(int i=0;i<cs.length;i++)
		{
			if(cs[i].isFile())
			{
				copyFile(cs[i], new File(toFolder+"/"+cs[i].getName()));
			}
			else
			{
				deepCopy(cs[i],toFolder+"/"+cs[i].getName());
			}
		}
	}

	private static void close(Closeable closable) {
	    if (closable != null) {
	        try {
	            closable.close();
	        } catch (IOException e) {
	            e.printStackTrace();
	        }
	    }
	}
	
	public void createDirectory(String dir)
	{
		boolean success = (new File(dir)).mkdir();
	    if (success) {
	      System.out.println("Directory: " + dir + " created");
	    }    		
	    else
	    {
	    	System.out.println("!Directory: " + dir + " not created");
	    }
	}

	public boolean copyFile(File source, File target) 
	{
		if(source.isDirectory())
		{
			createDirectory(target.getAbsolutePath());
			return true;
		}
		
        FileChannel in = null;
        FileChannel out = null;

        boolean success = true;
        try {
            in = new FileInputStream(source).getChannel();
            out = new FileOutputStream(target).getChannel();

            ByteBuffer buffer = ByteBuffer.allocateDirect(BUFFER);
            while (in.read(buffer) != -1) {
                buffer.flip();

                while(buffer.hasRemaining()){
                    out.write(buffer);
                }

                buffer.clear();
            }
        } catch (IOException e) {
        	success = false;
            e.printStackTrace();
        } finally {
            close(in);
            close(out);
        }

	    return success;
	  }
	  
	public void deleteFile(File in) 
	{
		in.delete();
	}
	public void deleteFile(String in) 
	{
		new File(in).delete();
	}
	
	public static void deleteDirectory(String dir)
	{
		File dirf = new File(dir);
		deleteDirectory(dirf);
	}
	public static void deleteDirectory(File dirf)
	{
		if( dirf.exists() ) {
		      File[] files = dirf.listFiles();
		      for(int i=0; i<files.length; i++) {
		         if(files[i].isDirectory()) 
		         {
		           deleteDirectory(files[i]);
		         }
		         else {
		           files[i].delete();
		         }
		      }
		      dirf.delete();
		}
	}

	public void rename(String from,String to) 
	{
		File f = new File(from);
		File fto = new File(to);
		if(fto.exists())
		{
			System.out.println(to+":exists");
		}
		else
			f.renameTo(fto);
	}
} public class GeneralMatrixDouble  implements Serializable
{
	private static final long serialVersionUID = 1L;

	public static final double EPSILON=5.9e-8f;
	public int width; //columns
	public int height; //rows
	public double[] value; //array of values

	public static void main(String[] list) throws Exception 
    {
		GeneralMatrixDouble pose = new GeneralMatrixDouble(3,3);
		GeneralMatrixDouble vpose = new GeneralMatrixDouble(3,3);
		GeneralMatrixDouble euler = new GeneralMatrixDouble(3,1);
		
		GeneralMatrixDouble tests = new GeneralMatrixDouble(3);

		double v = Math.PI;
		tests.push_back_row(0.0,0.0,0.0);
		tests.push_back_row(v,0.0,0.0);
		tests.push_back_row(0.0,v,0.0);
		tests.push_back_row(v,v,0.0);
		tests.push_back_row(0.0,0.0,v);
		tests.push_back_row(v,0.0,v);
		tests.push_back_row(0.0,v,v);
		tests.push_back_row(v,v,v);

		for(int i=0;i<tests.height;i++)
		{
			pose.set3DTransformRotationYXZ(tests.value[i*3+0], tests.value[i*3+1], tests.value[i*3+2]);
			GeneralMatrixDouble.getEulerYXZ(pose, euler);
			double errx = euler.value[0]-tests.value[i*3+0];
			double erry = euler.value[1]-tests.value[i*3+1];
			double errz = euler.value[2]-tests.value[i*3+2];
			System.out.println("err "+i+" x:"+errx+" y:"+erry+" z:"+errz);
			vpose.set3DTransformRotationYXZ(euler.value[0], euler.value[1], euler.value[2]);
			
		}
    }
	
	public void printTransform2()
	{
		System.out.println("Xx:"+value[0+0]+ "\tXy:"+value[1+0]);
		System.out.println("Yx:"+value[0+2]+ "\tYy:"+value[1+2]);
	}
	public void printTransform()
	{
		System.out.println("Xx:"+value[0+0]+ "\tXy:"+value[1+0]+ "\tXz:"+value[2+0]);
		System.out.println("Yx:"+value[0+4]+ "\tYy:"+value[1+4]+ "\tYz:"+value[2+4]);
		System.out.println("Zx:"+value[0+8]+ "\tZy:"+value[1+8]+ "\tZz:"+value[2+8]);
		System.out.println("Tx:"+value[0+12]+"\tTy:"+value[1+12]+"\tTz:"+value[2+12]);
	}
	public void printProjection()
	{
		System.out.println("Xx:"+value[0+0]+ "\tXy:"+value[1+0]+ "\tXz:"+value[2+0]);
		System.out.println("Yx:"+value[0+4]+ "\tYy:"+value[1+4]+ "\tYz:"+value[2+4]);
		System.out.println("Zx:"+value[0+8]+ "\tZy:"+value[1+8]+ "\tZz:"+value[2+8]);
		System.out.println("Tx:"+value[0+12]+"\tTy:"+value[1+12]+"\tTz:"+value[2+12]);
		System.out.println("Hx:"+value[3+0]+ "\tHy:"+value[3+4]+ "\tHz:"+value[3+8]+"\tHt:"+value[3+12]);
	}
	
	public static double calculateMachineEpsilonDouble() 
	{
        double machEps = 1.0f;
 
        do {
           machEps /= 2.0f;
        }
        while ((double)(1.0 + (machEps/2.0)) != 1.0);
 
        return machEps;
    }
	
	public GeneralMatrixDouble(double[] v)
	{
		this.width = 1;
		this.height = v.length;
		value = v;
	}
	public GeneralMatrixDouble(double a)
	{
		this.width = 1;
		this.height = 1;
		value = new double[1];
		value[0] = a;
	}
	public GeneralMatrixDouble(double a,double b)
	{
		this.width = 1;
		this.height = 2;
		value = new double[2];
		value[0] = a;
		value[1] = b;
	}
	public GeneralMatrixDouble(GeneralMatrixDouble a)
	{
		this.width = a.width;
		this.height = a.height;
		value = new double[a.width*a.height];
		set(a);
	}
	public GeneralMatrixDouble(GeneralMatrixFloat a)
	{
		this.width = a.width;
		this.height = a.height;
		value = new double[a.width*a.height];
		set(a);
	}
	public GeneralMatrixDouble(int width,int height)
	{
		this.width = width;
		this.height = height;
		value = new double[width*height];
	}
	public GeneralMatrixDouble(int width,int height,double[] v)
	{
		this.width = width;
		this.height = height;
		value = v;
	}
	public GeneralMatrixDouble()
	{
		width = 0;
		height = 0;
	}

	  public boolean isequal(GeneralMatrixDouble m)
	  {
		  if(width!=m.width)
			  return false;
		  if(height!=m.height)
			  return false;
		  for (int i = 0; i < width*height; i++) 
		  {
			  if(value[i]!=m.value[i])
				  return false;
		  }		  
		  return true;
	  }

	  
	public void clear(double v)
	{
		  for (int i = 0; i < width*height; i++)
		  {
			  value[i] = v;
		  }
	}

	public final double get(int i,int j)
	{
		return value[j*width+i];
	}

	public void reshape(int nw,int nh)
	{
		ensureCapacity(nw*nh);
		if(nw<width)
		{
			for(int j=0;j<nh;j++)
			{
				for(int i=0;i<nw;i++)
				{
					value[i+j*nw] = value[i+j*width];
				}
			}
		}
		else
		{
			for(int j=(nh-1);j>=0;j--)
			{
				for(int i=(nw-1);i>=0;i--)
				{
					value[i+j*nw] = value[i+j*width];
				}
			}
		}
		width = nw;
		height = nh;
	}
	
	public void reshapeleft(int nw,int nh)
	{
		ensureCapacity(nw*nh);
		if(nw<width)
		{
			for(int j=0;j<nh;j++)
			{
				for(int i=0;i<nw;i++)
				{
					value[i+j*nw] = value[i+(width-nw)+j*width];
				}
			}
		}
		else
		{
			for(int j=(nh-1);j>=0;j--)
			{
				for(int i=(nw-1);i>=0;i--)
				{
					value[i+(nw-width)+j*nw] = value[i+j*width];
				}
			}
		}
		width = nw;
		height = nh;
	}

	public double getMirror(int x, int y)
    {
        if (x >= width)
            x = width - (x - width + 2);

        if (y >= height)
            y = height - (y - height + 2);

        if (x < 0)
        {
            int tmp = 0;
            int dir = 1;

            while (x < 0)
            {
                tmp += dir;
                if (tmp == width - 1 || tmp == 0)
                    dir *= -1;
                x++;
            }
            x = tmp;
        }

        if (y < 0)
        {
            int tmp = 0;
            int dir = 1;

            while (y < 0)
            {
                tmp += dir;
                if (tmp == height - 1 || tmp == 0)
                    dir *= -1;
                y++;
            }
            y = tmp;
        }

       return value[x+y*width];
    }

	public void set(int i,int j,double v)
	{
		value[j*width+i] = v;
	}

	public void add(int i,int j,double v)
	{
		value[j*width+i] += v;
	}

	public void set(float[] a)
	{
		  for (int i = 0; i < width*height; i++)
		  {
			  value[i] = a[i];
		  }
	}
	public void set(GeneralMatrixFloat a)
	{
		ensureCapacityNoCopy(a.width*a.height);
		width = a.width;
		height = a.height;
		  for (int i = 0; i < width*height; i++)
		  {
			  value[i] = a.value[i];
		  }
	}
	public void set(GeneralMatrixDouble a)
	{
		ensureCapacityNoCopy(a.width*a.height);
		width = a.width;
		height = a.height;
		  for (int i = 0; i < width*height; i++)
		  {
			  value[i] = a.value[i];
		  }
	}
	public void set(double[] a)
	{
		  for (int i = 0; i < width*height; i++)
		  {
			  value[i] = (double)a[i];
		  }
	}
	public void set(double[][] a)
	{
		  for (int j = 0; j < height; j++)
		  {
			  for (int i = 0; i < width; i++)
			  {
				  value[i+j*width] = (double)a[j][i];
			  }
		  }
	}

	public void setDiagonal(double v)
	{
		  for (int i = 0; i < width; i++)
		  {
			  value[i+i*width] = v;
		  }
	}

	public static double determinant(GeneralMatrixDouble m)
	{
		int n = m.width; 
		if(n==1)
			return m.value[0];
		else
		if(n==2)
		{
			return m.value[0*2+0] * m.value[1*2+1] - m.value[1*2+0] * m.value[0*2+1];
		}
		else
		{
			float det = 0.0f;
			for (int j1=0;j1<n;j1++) 
			{
		         if(m.value[0*n+j1]==0.0f)
		        	 continue;

		         GeneralMatrixDouble subm = new GeneralMatrixDouble(n-1,n-1);

		         for (int i=1;i<n;i++) 
		         {
		            int j2 = 0;
		            for (int j=0;j<n;j++) 
		            {
		               if (j == j1)
		                  continue;
		               subm.value[(i-1)*(n-1)+j2] = m.value[i*n+j];
		               j2++;
		            }
		         }
		         int ind = 1+j1+1;
		         
		         if((ind%2)==0)
		        	 det += m.value[0*n+j1] * determinant(subm);
		         else
		        	 det -= m.value[0*n+j1] * determinant(subm);
			}
			return det;
		}
	}
	
	public void set3DTransformPosition(double x,double y,double z)
	{
		value[4*3+0] = x;
		value[4*3+1] = y;
		value[4*3+2] = z;
	}
	public void set3DTransformZRotation(double z)
	{
		double sin=(double)Math.sin(z);
		double cos=(double)Math.cos(z);
		value[width*0+0] = cos;
		value[width*0+1] = -sin;
		value[width*1+0] = sin;
		value[width*1+1] = cos;
		value[width*2+2] = 1.0f;
	}
	public void set3DTransformYRotation(double y)
	{
		double sin=(double)Math.sin(y);
		double cos=(double)Math.cos(y);
		value[width*0+0] = cos;
		value[width*0+2] = sin;
		value[width*2+0] = -sin;
		value[width*2+2] = cos;
		value[width*1+1] = 1.0f;
	}
	public void set3DTransformXRotation(double x)
	{
		double sin=(double)Math.sin(x);
		double cos=(double)Math.cos(x);
		value[width*1+1] = cos;
		value[width*1+2] = -sin;
		value[width*2+1] = sin;
		value[width*2+2] = cos;
		value[width*0+0] = 0.0f;
	}

	public void set3DTransformRotationYFirst(double x,double y,double z)
	{
		//Normal result
		double sx=(double)Math.sin(x);
		double cx=(double)Math.cos(x);
		double sy=(double)Math.sin(y);
		double cy=(double)Math.cos(y);
		double sz=(double)Math.sin(z);
		double cz=(double)Math.cos(z);
		
		//times this
		//0,  1, 0
		//-1, 0, 0
		//0,  0, 1

//		value[4*0+0] = cz*cy;
//		value[4*0+1] = (sy*sx*cz-cx*sz);
//		value[4*0+2] = (sy*cx*cz+sz*sx);
//
//		value[4*1+0] = sz*cy;
//		value[4*1+1] = (sy*sx*sz+cx*cz);
//		value[4*1+2] = (sy*cx*sz-cz*sx);
//
//		value[4*2+0] = -sy;
//		value[4*2+1] = cy*sx;
//		value[4*2+2] = cy*cx;

		
		//by this 
//		value[4*0+0] = sz*cy;
//		value[4*0+1] = (sy*sx*sz+cx*cz);
//		value[4*0+2] = (sy*cx*sz-cz*sx);
//
//		value[4*1+0] = -cz*cy;
//		value[4*1+1] = -(sy*sx*cz-cx*sz);
//		value[4*1+2] = -(sy*cx*cz+sz*sx);
//
//		value[4*2+0] = -sy;
//		value[4*2+1] = cy*sx;
//		value[4*2+2] = cy*cx;
		
		//times this
		//0,  -1, 0
		//1, 0, 0
		//0,  0, 1

		//First rotate by -90 z 		
		//x axis becomes y
		//y becomes -x
		
//		for (int k=0; k<3; k++) 
//		{
//		for (int j=0; j<3; j++) 
//		{
//			rbmat.value[k*3+j] = 
//				bmat.value[k*3+0] * procrustesTransform.value[0*4+j] +
//				bmat.value[k*3+1] * procrustesTransform.value[1*4+j] +
//				bmat.value[k*3+2] * procrustesTransform.value[2*4+j];
//		}
//		}
		
		if(width==4)
		{
			value[4*0+0] = (sy*sx*sz+cx*cz);
			value[4*0+1] = -sz*cy;
			value[4*0+2] = (sy*cx*sz-cz*sx);
	
			value[4*1+0] = -(sy*sx*cz-cx*sz);
			value[4*1+1] = cz*cy;
			value[4*1+2] = -(sy*cx*cz+sz*sx);
	
			value[4*2+0] = cy*sx;
			value[4*2+1] = sy;
			value[4*2+2] = cy*cx;
		}
		else
		{
			value[3*0+0] = (sy*sx*sz+cx*cz);
			value[3*0+1] = -sz*cy;
			value[3*0+2] = (sy*cx*sz-cz*sx);

			value[3*1+0] = -(sy*sx*cz-cx*sz);
			value[3*1+1] = cz*cy;
			value[3*1+2] = -(sy*cx*cz+sz*sx);

			value[3*2+0] = cy*sx;
			value[3*2+1] = sy;
			value[3*2+2] = cy*cx;
		}
		
		
		
		
		//then rotate back +90 z
		//x axis becomes -y
		//y becomes x
	}

	public void set3DTransformRotation(double x,double y,double z)
	{
		double sx=(double)Math.sin(x);
		double cx=(double)Math.cos(x);
		double sy=(double)Math.sin(y);
		double cy=(double)Math.cos(y);
		double sz=(double)Math.sin(z);
		double cz=(double)Math.cos(z);

	    if(width==4)
		{
			value[4*0+0] = cz*cy;
			value[4*0+1] = (sy*sx*cz-cx*sz);
			value[4*0+2] = (sy*cx*cz+sz*sx);
	
			value[4*1+0] = sz*cy;
			value[4*1+1] = (sy*sx*sz+cx*cz);
			value[4*1+2] = (sy*cx*sz-cz*sx);
	
			value[4*2+0] = -sy;
			value[4*2+1] = cy*sx;
			value[4*2+2] = cy*cx;
		}
		else
		{
			value[3*0+0] = cz*cy;
			value[3*0+1] = (sy*sx*cz-cx*sz);
			value[3*0+2] = (sy*cx*cz+sz*sx);
	
			value[3*1+0] = sz*cy;
			value[3*1+1] = (sy*sx*sz+cx*cz);
			value[3*1+2] = (sy*cx*sz-cz*sx);
	
			value[3*2+0] = -sy;
			value[3*2+1] = cy*sx;
			value[3*2+2] = cy*cx;
		}
	}
//	   [  (cy*cz)+(sy*sx*sz)   ,  (cy*(-(sz)))+(sy*sx*cz)   ,sy*cx]
//	   [                                                          ]
//	   [         cx*sz         ,           cx*cz            ,-(sx)]
//	   [                                                          ]
//	   [((-(sy))*cz)+(cy*sx*sz),((-(sy))*(-(sz)))+(cy*sx*cz),cy*cx]
	public void set3DTransformRotationYXZ(double x,double y,double z)
	{
		double sx=(double)Math.sin(x);
		double cx=(double)Math.cos(x);
		double sy=(double)Math.sin(y);
		double cy=(double)Math.cos(y);
		double sz=(double)Math.sin(z);
		double cz=(double)Math.cos(z);

		if(width==4)
		{
			value[4*0+0] = (cy*cz)+(sy*sx*sz);
			value[4*0+1] = (cy*(-(sz)))+(sy*sx*cz);
			value[4*0+2] = sy*cx;
	
			value[4*1+0] = cx*sz;
			value[4*1+1] = cx*cz;
			value[4*1+2] = -(sx);
	
			value[4*2+0] = ((-(sy))*cz)+(cy*sx*sz);
			value[4*2+1] = ((-(sy))*(-(sz)))+(cy*sx*cz);
			value[4*2+2] = cy*cx;
		}
		else
		{
			value[3*0+0] = (cy*cz)+(sy*sx*sz);
			value[3*0+1] = (cy*(-(sz)))+(sy*sx*cz);
			value[3*0+2] = sy*cx;
	
			value[3*1+0] = cx*sz;
			value[3*1+1] = cx*cz;
			value[3*1+2] = -(sx);
	
			value[3*2+0] = ((-(sy))*cz)+(cy*sx*sz);
			value[3*2+1] = ((-(sy))*(-(sz)))+(cy*sx*cz);
			value[3*2+2] = cy*cx;
		}
	}

	public static final void getEulerYXZ(GeneralMatrixFloat  m, GeneralMatrixFloat euler) 
	{
		final int i=0;
		final int j=1;
		final int k=2;

		final int w=m.width;
		
		double cx = Math.sqrt(m.value[j*w+i]*m.value[j*w+i] + m.value[j*w+j]*m.value[j*w+j]);
		if (cx > 16*EPSILON) {
		    euler.value[0] = (float)Math.atan2(-m.value[j*w+k], cx);
		    euler.value[1] = (float)Math.atan2(m.value[i*w+k], m.value[k*w+k]);
		    euler.value[2] = (float)Math.atan2(m.value[j*w+i], m.value[j*w+j]);
		} else {
			euler.value[0] = (float)Math.atan2(-m.value[j*w+k], cx);
		    euler.value[1] = 0;
		    euler.value[2] = (float)Math.atan2(m.value[i*w+i], m.value[i*w+j]);
		}
	}
	public static final void getEulerYXZ(GeneralMatrixDouble  m, GeneralMatrixDouble euler) 
	{
		final int i=0;
		final int j=1;
		final int k=2;

		final int w=m.width;
		
		double cx = Math.sqrt(m.value[j*w+i]*m.value[j*w+i] + m.value[j*w+j]*m.value[j*w+j]);
		if (cx > 16*EPSILON) {
		    euler.value[0] = (double)Math.atan2(-m.value[j*w+k], cx);
		    euler.value[1] = (double)Math.atan2(m.value[i*w+k], m.value[k*w+k]);
		    euler.value[2] = (double)Math.atan2(m.value[j*w+i], m.value[j*w+j]);
		} else {
			euler.value[0] = (double)Math.atan2(-m.value[j*w+k], cx);
		    euler.value[1] = 0;
		    euler.value[2] = (double)Math.atan2(m.value[i*w+i], m.value[i*w+j]);
		}
	}

	public static final void getEuler(GeneralMatrixFloat  m, GeneralMatrixDouble euler) 
	{
		final int i=0;
		final int j=1;
		final int k=2;

		final int w=m.width;

		double cy = Math.sqrt(m.value[i*w+i]*m.value[i*w+i] + m.value[j*w+i]*m.value[j*w+i]);
		if (cy > 16*EPSILON) {
		    euler.value[0] = (double)Math.atan2(m.value[k*w+j], m.value[k*w+k]);
		    euler.value[1] = (double)Math.atan2(-m.value[k*w+i], cy);
		    euler.value[2] = (double)Math.atan2(m.value[j*w+i], m.value[i*w+i]);
		} else {
		    euler.value[0] = (double)Math.atan2(-m.value[i*w+k], m.value[j*w+j]);
		    euler.value[1] = (double)Math.atan2(-m.value[k*w+i], cy);
		    euler.value[2] = 0;
		}
	}
	
	public static final void getEuler(GeneralMatrixDouble  m, GeneralMatrixDouble euler) 
	{
		final int i=0;
		final int j=1;
		final int k=2;

		final int w=m.width;

		double cy = Math.sqrt(m.value[i*w+i]*m.value[i*w+i] + m.value[j*w+i]*m.value[j*w+i]);
		if (cy > 16*EPSILON) {
		    euler.value[0] = (double)Math.atan2(m.value[k*w+j], m.value[k*w+k]);
		    euler.value[1] = (double)Math.atan2(-m.value[k*w+i], cy);
		    euler.value[2] = (double)Math.atan2(m.value[j*w+i], m.value[i*w+i]);
		} else {
		    euler.value[0] = (double)Math.atan2(-m.value[i*w+k], m.value[j*w+j]);
		    euler.value[1] = (double)Math.atan2(-m.value[k*w+i], cy);
		    euler.value[2] = 0;
		}
	}
	
	public static final void getEuler(GeneralMatrixDouble  m, GeneralMatrixFloat euler) 
	{
		final int i=0;
		final int j=1;
		final int k=2;

		final int w=m.width;

		double cy = Math.sqrt(m.value[i*w+i]*m.value[i*w+i] + m.value[j*w+i]*m.value[j*w+i]);
		if (cy > 16*EPSILON) {
		    euler.value[0] = (float)Math.atan2(m.value[k*w+j], m.value[k*w+k]);
		    euler.value[1] = (float)Math.atan2(-m.value[k*w+i], cy);
		    euler.value[2] = (float)Math.atan2(m.value[j*w+i], m.value[i*w+i]);
		} else {
		    euler.value[0] = (float)Math.atan2(-m.value[i*w+k], m.value[j*w+j]);
		    euler.value[1] = (float)Math.atan2(-m.value[k*w+i], cy);
		    euler.value[2] = 0;
		}
	}
	
	public void calcBasisFromY(double x,double y,double z,
			double Xx,double Xy,double Xz)
	{
		value[width*1+0] = x;
		value[width*1+1] = y;
		value[width*1+2] = z;
		double total = x*x;
		total += y*y;
		total += z*z;
		if(total<=EPSILON)
		{
			value[width*1+0] = 1.0f;
		}
		else
		{
			total = Math.sqrt(total);
		}
		value[width*1+0] /= total;
		value[width*1+1] /= total;
		value[width*1+2] /= total;
		
		total = Xx*Xx;
		total += Xy*Xy;
		total += Xz*Xz;
		if(total<=EPSILON)
		{
			value[width*0+0] = 1.0f;
		}
		else
		{
			total = Math.sqrt(total);
			value[width*0+0] = Xx/total;
			value[width*0+1] = Xy/total;
			value[width*0+2] = Xz/total;
		}

		
		value[width*2+0] = -y*value[width*0+2]+value[width*0+1]*z;
		value[width*2+1] = -z*value[width*0+0]+value[width*0+2]*x;
		value[width*2+2] = -x*value[width*0+1]+value[width*0+0]*y;

		//Normalise
		total = value[width*2+0]*value[width*2+0];
		total += value[width*2+1]*value[width*2+1];
		total += value[width*2+2]*value[width*2+2];
		if(total<=EPSILON)
		{
			value[width*2+0] = 1.0f;
		}
		else
		{
			total = Math.sqrt(total);
			value[width*2+0] /= total;
			value[width*2+1] /= total;
			value[width*2+2] /= total;
		}
		
		value[width*0+0] = y*value[width*2+2]-value[width*2+1]*z;
		value[width*0+1] = z*value[width*2+0]-value[width*2+2]*x;
		value[width*0+2] = x*value[width*2+1]-value[width*2+0]*y;

		total = value[width*0+0]*value[width*0+0];
		total += value[width*0+1]*value[width*0+1];
		total += value[width*0+2]*value[width*0+2];
		if(total<=EPSILON)
		{
			value[width*0+0] = 1.0f;
		}
		else
		{
			total = Math.sqrt(total);
			value[width*0+0] /= total;
			value[width*0+1] /= total;
			value[width*0+2] /= total;
		}
	}

	public void calcBasisFromY(double x,double y,double z,
			double Xx,double Xy,double Xz,
			int width,int off)
	{
		value[width*1+0+off] = x;
		value[width*1+1+off] = y;
		value[width*1+2+off] = z;
		double total = x*x;
		total += y*y;
		total += z*z;
		if(total<=EPSILON)
		{
			value[width*1+0+off] = 1.0f;
		}
		else
		{
			total = Math.sqrt(total);
		}
		value[width*1+0+off] /= total;
		value[width*1+1+off] /= total;
		value[width*1+2+off] /= total;
		
		total = Xx*Xx;
		total += Xy*Xy;
		total += Xz*Xz;
		if(total<=EPSILON)
		{
			value[width*0+0+off] = 1.0f;
		}
		else
		{
			total = Math.sqrt(total);
			value[width*0+0+off] = Xx/total;
			value[width*0+1+off] = Xy/total;
			value[width*0+2+off] = Xz/total;
		}

		
		value[width*2+0+off] = -y*value[width*0+2+off]+value[width*0+1+off]*z;
		value[width*2+1+off] = -z*value[width*0+0+off]+value[width*0+2+off]*x;
		value[width*2+2+off] = -x*value[width*0+1+off]+value[width*0+0+off]*y;

		//Normalise
		total = value[width*2+0+off]*value[width*2+0+off];
		total += value[width*2+1+off]*value[width*2+1+off];
		total += value[width*2+2+off]*value[width*2+2+off];
		if(total<=EPSILON)
		{
			value[width*2+0+off] = 1.0f;
		}
		else
		{
			total = Math.sqrt(total);
			value[width*2+0+off] /= total;
			value[width*2+1+off] /= total;
			value[width*2+2+off] /= total;
		}
		
		value[width*0+0+off] = y*value[width*2+2+off]-value[width*2+1+off]*z;
		value[width*0+1+off] = z*value[width*2+0+off]-value[width*2+2+off]*x;
		value[width*0+2+off] = x*value[width*2+1+off]-value[width*2+0+off]*y;

		total = value[width*0+0+off]*value[width*0+0+off];
		total += value[width*0+1+off]*value[width*0+1+off];
		total += value[width*0+2+off]*value[width*0+2+off];
		if(total<=EPSILON)
		{
			value[width*0+0+off] = 1.0f;
		}
		else
		{
			total = Math.sqrt(total);
			value[width*0+0+off] /= total;
			value[width*0+1+off] /= total;
			value[width*0+2+off] /= total;
		}
	}

	public void setSubset(GeneralMatrixDouble subset,int xs,int ys)
	{
		int maxy = ys+subset.height;
		int maxx = xs+subset.width;
		for(int y=ys;y<maxy;y++)
		{
			for(int x=xs;x<maxx;x++)
			{
				value[x+y*width] = subset.value[(x-xs)+(y-ys)*subset.width];
			}
		}
	}
	public void clearSubset(int xs,int ys,int sw,int sh,double v)
	{
		int maxy = ys+sh;
		int maxx = xs+sw;
		for(int y=ys;y<maxy;y++)
		{
			for(int x=xs;x<maxx;x++)
			{
				value[x+y*width] = v;
			}
		}
	}

	//For setting a vec or mat from an array of vecs/mats
	public void setFromSubset(GeneralMatrixDouble full,int ys)
	{
		int i=0;
		int fi = ys*width;
		for(int y=0;y<height;y++)
		{
			for(int x=0;x<width;x++)
			{
				value[i] = full.value[fi];
				i++;
				fi++;
			}
		}
	}
	public void setFromSubset(GeneralMatrixFloat full,int ys)
	{
		int i=0;
		int fi = ys*width;
		for(int y=0;y<height;y++)
		{
			for(int x=0;x<width;x++)
			{
				value[i] = full.value[fi];
				i++;
				fi++;
			}
		}
	}

    public void setRow(int r,GeneralMatrixDouble row)
    {
    	System.arraycopy(row.value, 0, value, width*(r), width);
    }
	
	public static double norm(GeneralMatrixDouble a)
	{
		double total = 0.0f;
		for (int i = 0; i < a.width*a.height; i++)
		{
			total += a.value[i]*a.value[i];
		}
		total = (double)Math.sqrt(total);
		return total;
	}

	public void normalise()
	{
		double total = 0.0f;
		for (int i = 0; i < width*height; i++)
		{
			total += value[i]*value[i];
		}
		if(total<=EPSILON)
		{
			value[0] = 1.0f;
			return;
		}
		total = (double)Math.sqrt(total);
		for (int i = 0; i < width*height; i++)
		{
			value[i] /= total;
		}
	}

	public void rowNormalise(int row)
	{
		double total = 0.0f;
		for (int i = 0; i < width; i++)
		{
			total += value[i+row*width]*value[i+row*width];
		}
		if(total<=EPSILON)
		{
			value[0+row*width] = 1.0f;
			return;
		}
		total = (double)Math.sqrt(total);
		for (int i = 0; i < width; i++)
		{
			value[i+row*width] /= total;
		}
	}
	
	public void reverse()
	{
		  for (int i = 0; i < width*height; i++)
		  {
			  value[i] = -value[i];
		  }
	}

	public void scale(double s)
	{
		  for (int i = 0; i < width*height; i++)
		  {
			  value[i] = value[i]*s;
		  }
	}

	public void setIdentity()
	{
		int i=0;
		for(int y=0;y<height;y++)
		{
			for(int x=0;x<width;x++)
			{
				if(x==y)
					value[i] = 1.0f;
				else
					value[i] = 0.0f;
				i++;
			}
		}
	}

	//Must be square
	public static final boolean invert(GeneralMatrixDouble a,GeneralMatrixDouble ia)
	{
		GeneralMatrixDouble temp = new GeneralMatrixDouble(a.width,a.height);
		temp.set(a);

		ia.setIdentity();
		int i,j,k,swap;
		double t;
		for(i=0;i<a.width;i++)
		{
			swap = i;
			for (j = i + 1; j < a.height; j++) {
			    if (Math.abs(a.get(i, j)) > Math.abs(a.get(i, j)))
			    {
			    	swap = j;
			    }
			}

			if (swap != i)
			{
			    /*
			    ** Swap rows.
			    */
			    for (k = 0; k < a.width; k++)
			    {
					t = temp.get(k,i);
					temp.set(k,i,temp.get(k, swap));
					temp.set(k, swap,t);

					t = ia.get(k,i);
					ia.set(k,i,ia.get(k, swap));
					ia.set(k, swap,t);
			    }
			}

			t = temp.get(i,i);
			if (t == 0.0f)
			{
			    /*
			    ** No non-zero pivot.  The matrix is singular, which shouldn't
			    ** happen.  This means the user gave us a bad matrix.
			    */
			    return false;
			}

			for (k = 0; k < a.width; k++)
			{
				temp.set(k,i,temp.get(k,i) / t);
				ia.set(k,i,ia.get(k,i) / t);
			}
			for (j = 0; j < a.width; j++)
			{
			    if (j != i)
			    {
			    	t = temp.get(i,j);
			    	for (k = 0; k < a.width; k++)
			    	{
			    		temp.set(k,j, temp.get(k,j) - temp.get(k,i)*t);
			    		ia.set(k,j, ia.get(k,j) - ia.get(k,i)*t);
			    	}
			    }
			}
	    }
	    return true;
	}

	public static final void invertTransform(GeneralMatrixDouble cameraTransformMatrix,GeneralMatrixDouble modelMatrix)
	{
		//GeneralMatrixFloat.invert(cameraTransformMatrix,modelMatrix);
		//*
		modelMatrix.value[0*4+0] = cameraTransformMatrix.value[0*4+0];
		modelMatrix.value[1*4+0] = cameraTransformMatrix.value[0*4+1];
		modelMatrix.value[2*4+0] = cameraTransformMatrix.value[0*4+2];

		modelMatrix.value[0*4+1] = cameraTransformMatrix.value[1*4+0];
		modelMatrix.value[1*4+1] = cameraTransformMatrix.value[1*4+1];
		modelMatrix.value[2*4+1] = cameraTransformMatrix.value[1*4+2];

		modelMatrix.value[0*4+2] = cameraTransformMatrix.value[2*4+0];
		modelMatrix.value[1*4+2] = cameraTransformMatrix.value[2*4+1];
		modelMatrix.value[2*4+2] = cameraTransformMatrix.value[2*4+2];

		double x = -cameraTransformMatrix.value[3*4+0];
		double y = -cameraTransformMatrix.value[3*4+1];
		double z = -cameraTransformMatrix.value[3*4+2];
		
		//needs to be rotated like the rest of the points in space
		double tx = modelMatrix.value[0*4+0]*x+modelMatrix.value[1*4+0]*y+modelMatrix.value[2*4+0]*z;
		double ty = modelMatrix.value[0*4+1]*x+modelMatrix.value[1*4+1]*y+modelMatrix.value[2*4+1]*z;
		double tz = modelMatrix.value[0*4+2]*x+modelMatrix.value[1*4+2]*y+modelMatrix.value[2*4+2]*z;
		modelMatrix.value[3*4+0] = tx;
		modelMatrix.value[3*4+1] = ty;
		modelMatrix.value[3*4+2] = tz;		
		//*/
	}

	
	public static final void crossProduct3(GeneralMatrixDouble a,GeneralMatrixDouble b,GeneralMatrixDouble c)
	{
		c.value[0] = a.value[1]*b.value[2]-b.value[1]*a.value[2];
		c.value[1] = a.value[2]*b.value[0]-b.value[2]*a.value[0];
		c.value[2] = a.value[0]*b.value[1]-b.value[0]*a.value[1];
	}

	public static final void mult(GeneralMatrixDouble a,GeneralMatrixDouble b,GeneralMatrixDouble c)
	{
		int n = a.height;
		int p = a.width;
		int r = b.width;

		if(p!=b.height)
		{
			System.out.println("Mult error, matricies sizes dont match");
			return;
		}
		
	  double vdot;
	  int i,j,k,m;


      int r5 = r*5;
	  int rowI = 0;
	  int crowI = 0;
	  for (i = 0; i < n; i++) {

	     for (j = 0; j < r; j++) {

	        vdot = 0.0f;

	        m = p%5;

	        int rowK = 0;
	        for (k = 0; k < m; k++) {

	           vdot += a.value[rowI+k]*b.value[rowK+j];
	           rowK += r;
	        }

	        for (k = m; k < p; k += 5) {

	           vdot += a.value[rowI+k]*b.value[rowK+j] +
	                   a.value[rowI+k+1]*b.value[rowK+r+j] +
	                   a.value[rowI+k+2]*b.value[rowK+r*2+j] +
	                   a.value[rowI+k+3]*b.value[rowK+r*3+j] +
	                   a.value[rowI+k+4]*b.value[rowK+r*4+j];

	           rowK += r5;
	        }

	        c.value[crowI+j] = vdot;

	     }
	     rowI += p;
	     crowI += r;
	  }

	}

	public static final void mult(GeneralMatrixDouble a,double[] x,double[] b)
	{
		int n = a.height;
		int p = a.width;
		int r = 1;

		if(p!=x.length)
		{
			System.out.println("Mult error, matricies sizes dont match");
			return;
		}
		
	  double vdot;
	  int i,j,k,m;


      int r5 = r*5;
	  int rowI = 0;
	  int crowI = 0;
	  for (i = 0; i < n; i++) {

	     for (j = 0; j < r; j++) {

	        vdot = 0.0f;

	        m = p%5;

	        int rowK = 0;
	        for (k = 0; k < m; k++) {

	           vdot += a.value[rowI+k]*x[rowK+j];
	           rowK += r;
	        }

	        for (k = m; k < p; k += 5) {

	           vdot += a.value[rowI+k]*x[rowK+j] +
	                   a.value[rowI+k+1]*x[rowK+r+j] +
	                   a.value[rowI+k+2]*x[rowK+r*2+j] +
	                   a.value[rowI+k+3]*x[rowK+r*3+j] +
	                   a.value[rowI+k+4]*x[rowK+r*4+j];

	           rowK += r5;
	        }

	        b[crowI+j] = vdot;

	     }
	     rowI += p;
	     crowI += r;
	  }

	}

	public static final void rowmult(GeneralMatrixDouble a,GeneralMatrixDouble b,GeneralMatrixDouble c)
	{
		double vdot;
		  for (int i = 0; i < a.height; i++)
		  {
			  //Apply the matrix to this vector

			  for (int j = 0; j < a.width; j++)
			  {
				  vdot = 0.0f;
				  for (int k = 0; k < b.height; k++)
				  {
			           vdot += a.value[i*a.width+k]*b.value[k*b.width+j];
				  }
				  c.value[i*a.width+j] = vdot;
			  }
		  }
	}

	public static final void rowtransform(GeneralMatrixDouble a,GeneralMatrixDouble b,GeneralMatrixDouble c)
	{
		//     wxh
		//a is 3xn (h value of 1.0 is implied)
		//B is 3x4 (or at least the other values are unused)
		double vdot;
		  for (int i = 0; i < a.height; i++)
		  {
			  //Apply the matrix to this vector

			  for (int j = 0; j < a.width; j++)
			  {
				  vdot = 0.0f;
				  for (int k = 0; k < a.width; k++)
				  {
			           vdot += a.value[i*a.width+k]*b.value[k*b.width+j];
				  }
				  //Add what would be the homogenious value to translate correctly
				  vdot += b.value[a.width*b.width+j];
				  c.value[i*a.width+j] = vdot;
			  }
		  }
	}

	public static final void rowproject(GeneralMatrixDouble a,GeneralMatrixDouble b,GeneralMatrixDouble c)
	{
		//     wxh
		//a is 3xn (h value of 1.0 is implied)
		//B is 4x4 (or at least the other values are unused)
		double vdot;
		double h;
		  for (int i = 0; i < a.height; i++)
		  {
			  //Apply the matrix to this vector

			  for (int j = 0; j < a.width; j++)
			  {
				  vdot = 0.0f;
				  for (int k = 0; k < a.width; k++)
				  {
			           vdot += a.value[i*a.width+k]*b.value[k*b.width+j];
				  }
				  //Add what would be the homogenious value to translate correctly
				  vdot += b.value[a.width*b.width+j];
				  c.value[i*a.width+j] = vdot;
			  }
			  h = 0.0f;
			  for (int k = 0; k < a.width; k++)
			  {
		           h += a.value[i*a.width+k]*b.value[k*b.width+3];
			  }
			  //Add implicit h
			  h += b.value[a.width*b.width+3];
			  //Divide by h
			  for (int j = 0; j < a.width; j++)
			  {
				  c.value[i*a.width+j] /= h;
			  }
		  }
	}

	public static final double rowdot(GeneralMatrixDouble a,int ai,GeneralMatrixDouble b,int bi)
	{
		double result = 0.0f;
		for(int i=0;i<a.width;i++)
		{
			result += a.value[i+ai*a.width]*b.value[i+bi*b.width];
		}
		return result;
	}

	public static final void add(GeneralMatrixDouble a,GeneralMatrixDouble b,GeneralMatrixDouble c)
	{
		  for (int i = 0; i < a.width*a.height; i++)
		  {
			  c.value[i] = a.value[i]+b.value[i];
		  }
	}

	public static final void addWithScale(GeneralMatrixDouble a,GeneralMatrixDouble b,double s,GeneralMatrixDouble c)
	{
		  for (int i = 0; i < a.width*a.height; i++)
		  {
			  c.value[i] = a.value[i]+b.value[i]*s;
		  }
	}

	
	
	public static final void rowadd(GeneralMatrixDouble a,GeneralMatrixDouble b,GeneralMatrixDouble c)
	{
		int ai = 0;
		  for (int i = 0; i < a.height; i++)
		  {
			  for (int j = 0; j < a.width; j++)
			  {
				  c.value[ai] = a.value[ai]+b.value[j];
				  ai++;
			  }
		  }
	}

	public static final void sub(GeneralMatrixDouble a,GeneralMatrixDouble b,GeneralMatrixDouble c)
	{
		  for (int i = 0; i < a.width*a.height; i++)
		  {
			  c.value[i] = a.value[i]-b.value[i];
		  }
	}

	public static final void rowsub(GeneralMatrixDouble a,GeneralMatrixDouble b,GeneralMatrixDouble c)
	{
		int ai = 0;
		  for (int i = 0; i < a.height; i++)
		  {
			  for (int j = 0; j < a.width; j++)
			  {
				  c.value[ai] = a.value[ai]-b.value[j];
				  ai++;
			  }
		  }
	}

	public static final void scale(GeneralMatrixDouble a,double b,GeneralMatrixDouble c)
	{
		  for (int i = 0; i < a.width*a.height; i++)
		  {
			  c.value[i] = a.value[i]*b;
		  }
	}

	public static final void negate(GeneralMatrixDouble a,GeneralMatrixDouble b)
	{
		  for (int i = 0; i < a.width*a.height; i++)
		  {
			  b.value[i] = -a.value[i];
		  }
	}

	public static final void transpose(GeneralMatrixDouble a,GeneralMatrixDouble at)
	{
		int i,j;

		int ai = 0;
		int atjR;
	      for (i = 0; i < a.height; i++) {

	  		atjR = 0;
	         for (j = 0; j < a.width; j++) {

	            at.value[atjR+i] = a.value[ai];
	            ai++;
	            atjR += a.height;
	         }

	      }
	}

	public static final double trace(GeneralMatrixDouble a)
	{
		double sum = 0.0f;
		int index = 0;
		for (int i = 0; i < a.height; i++)
		{
			sum += a.value[index];
			index += a.height+1;
		}
		return sum;
	}

	public double[][] getAs2DArray()
	{
		double[][] result = new double[height][width];
		int i=0;
		for(int y=0;y<height;y++)
		{
			for(int x=0;x<width;x++)
			{
				result[y][x]=value[i];
				i++;
			}
		}
		return result;
	}

	public void setFrom2DArray(double[][] a)
	{
		int i=0;
		for(int y=0;y<height;y++)
		{
			for(int x=0;x<width;x++)
			{
				value[i]=a[y][x];
				i++;
			}
		}
	}
	  
	  public GeneralMatrixDouble(int width)
	  {
		  this.width = width;
	  }

	    public int appendRows(int numRows)
	    {
	    	int newSize = width*(height+numRows);
	    	if(value==null)
	    	{
	    		value = new double[newSize];
				height+=numRows;
				return height-numRows;
	    	}
	    	if(newSize>value.length)
	    	{
	    		ensureCapacity(newSize);
	    	}
			height+=numRows;
			return height-numRows;
	    }

	    public int appendRow()
	    {
	    	int newSize = width*(height+1);
	    	if(value==null)
	    	{
	    		value = new double[newSize];
				height++;
				return height-1;
	    	}
	    	if(newSize>value.length)
	    	{
	    		ensureCapacity(newSize);
	    	}
			height++;
			return height-1;
	    }

	    public void ensureCapacityNoCopy(int mincap)
	    {
	    	if(value==null)
	    	{
	    		value = new double[mincap];
	    	}
	    	else
	    	if(mincap>value.length)
	    	{
		        int newcap = (value.length * 3)/2 + 1;
		        double[] olddata = value;
		        value = new double[newcap < mincap ? mincap : newcap];
	    	}
	    }

	    public void ensureCapacity(int mincap)
	    {
	    	if(value==null)
	    	{
	    		value = new double[mincap];
	    	}
	    	else
	    	if(mincap>value.length)
	    	{
		        int newcap = (value.length * 3)/2 + 1;
		        double[] olddata = value;
		        value = new double[newcap < mincap ? mincap : newcap];
		        System.arraycopy(olddata,0,value,0,width*height);
	    	}
	    }

	    public void setDimensions(int w,int h)
	    {
	    	ensureCapacity(w*h);
	    	width = w;
	    	height = h;
	    }
	    public void setDimensionsNoCopy(int w,int h)
	    {
	    	ensureCapacityNoCopy(w*h);
	    	width = w;
	    	height = h;
	    }

	    public void push_back(double val)
	    {
	    	int ind = appendRow();
	    	value[ind] = val;
	    }
	    public void push_back_row(double val1,double val2)
	    {
	    	int ind = appendRow();
	    	value[ind*2+0] = val1;
	    	value[ind*2+1] = val2;
	    }
	    public void push_back_row(double val1,double val2,double val3)
	    {
	    	int ind = appendRow();
	    	value[ind*3+0] = val1;
	    	value[ind*3+1] = val2;
	    	value[ind*3+2] = val3;
	    }
	    public void push_back_row(double val1,double val2,double val3,double val4)
	    {
	    	int ind = appendRow();
	    	value[ind*4+0] = val1;
	    	value[ind*4+1] = val2;
	    	value[ind*4+2] = val3;
	    	value[ind*4+3] = val4;
	    }
	    public void push_back_row(double val1,double val2,double val3,double val4,double val5,double val6)
	    {
	    	int ind = appendRow();
	    	value[ind*6+0] = val1;
	    	value[ind*6+1] = val2;
	    	value[ind*6+2] = val3;
	    	value[ind*6+3] = val4;
	    	value[ind*6+4] = val5;
	    	value[ind*6+5] = val6;
	    }

	    public int push_back_row(double[] row)
	    {
	    	int ind = appendRow();
	    	System.arraycopy(row, 0, value, width*(height-1), width);
	    	return ind;
	    }
	    public int push_back_row(float[] row)
	    {
	    	int ind = appendRow();
	    	int off = width*ind;
	    	for(int i=0;i<width;i++)
	    		value[off+i] = row[i];
	    	return ind;
	    }

	    public int push_back_row(double[] row, int off)
	    {
	    	int ind = appendRow();
	    	System.arraycopy(row, off, value, width*(height-1), width);
	    	return ind;
	    }
	    public void push_back_rows(GeneralMatrixDouble rows)
	    {
	    	if(rows.height==0)
	    		return;
	    	appendRows(rows.height);
	    	System.arraycopy(rows.value, 0, value, width*(height-rows.height), width*rows.height);
	    }
}
 public class GeneralMatrixObject implements Serializable 
{
	private static final long serialVersionUID = 1L;

	public int width;
	public int height;
	public Object[] value;

    public GeneralMatrixObject()
    {
    	width = 0;
    	height = 0;
    	value = new Object[8];
    }
    public GeneralMatrixObject(Object a)
    {
    	width = 1;
    	height = 1;
    	value = new Object[1];
    	value[0] = a;
    }
    public GeneralMatrixObject(GeneralMatrixObject o)
    {
    	this.width = o.width;
    	this.height = o.height;
    	value = new Object[width*height];
    	if(value.length>0)
    		System.arraycopy(o.value, 0, value, 0, value.length);
    }
    public GeneralMatrixObject(Object[] o)
    {
    	this.width = 1;
    	this.height = o.length;
    	value = new Object[width*height];
    	if(value.length>0)
    		System.arraycopy(o, 0, value, 0, value.length);
    }
    public GeneralMatrixObject(int width)
    {
    	this.width = width;
    	height = 0;
    	value = new Object[8];
    }
    public GeneralMatrixObject(int width,int height)
    {
    	this.width = width;
    	this.height = height;
    	value = new Object[width*height];
    }
    public GeneralMatrixObject(int width,int height,int mincap)
    {
    	this.width = width;
    	this.height = height;
    	value = new Object[mincap];
    }
    
    public Object clone()
    {
    	GeneralMatrixObject o = new GeneralMatrixObject(width,height);
    	for(int i=0;i<(width*height);i++)
    	{
    		Object v = value[i];
    		if(v instanceof GeneralMatrixObject)
    		{
    			v = ((GeneralMatrixObject)v).clone();
    		}
    		else
    		if(v instanceof GeneralMatrixString)
    		{
    			v = ((GeneralMatrixString)v).clone();    			
    		}
    		o.value[i] = v;
    	}
    	return o;
    }

    
    public int push_back(Object o)
    {
    	int ind = appendRow();
    	value[ind] = o;
    	return ind;
    }
    
    public int appendRow()
    {
    	int newSize = width*(height+1);
    	if(newSize>value.length)
    	{
    		ensureCapacity(newSize);
    	}
		height++;
		return height-1;
    }
    
    public int appendRows(int size)
    {
    	int newSize = width*(height+size);
    	if(value==null)
    	{
    		value = new Object[newSize];
			height+=size;
			return height-size;
    	}
    	if(newSize>value.length)
    	{
    		ensureCapacity(newSize);
    	}
		height+=size;
		return height-size;
    }

    public void removeRow(int index)
    {
    	for(int i=index*width;i<((height-1))*width;i++)
    	{
    		value[i] = value[i+width];
    	}
    	height--;
    }

    public void ensureCapacity(int mincap)
    {
    	if(value==null)
    	{
    		value = new Object[mincap];
    	}
    	else
    	if(mincap>value.length)
    	{
	        int newcap = (value.length * 3)/2 + 1;
	        Object[] olddata = value;
	        value = new Object[newcap < mincap ? mincap : newcap];
	        System.arraycopy(olddata,0,value,0,olddata.length);
    	}
    }
//    public void ensureCapacity(int mincap)
//    {
//        int newcap = (value.length * 3)/2 + 1;
//        Object[] olddata = value;
//        value = new Object[newcap < mincap ? mincap : newcap];
//        System.arraycopy(olddata,0,value,0,olddata.length);    		
//    }


    public void clear()
	{
		for (int i = 0; i < width*height; i++)
		{
			value[i] = null;
		}
	}

	  public int find(Object v)
	  {
		  for(int i=0;i<(width*height);i++)
		  {
			  if(value[i]==v)
				  return i;
		  }
		  return -1;
	  }
    
	  public void set(GeneralMatrixObject o)
	  {
		  setDimensions(o.width, o.height);
		  int len = o.width*o.height;
    	  if(len>0)
    		  System.arraycopy(o.value, 0, value, 0, len);
	  }
		public void setFromSubset(GeneralMatrixObject full,int ys)
		{
			int i=0;
			int fi = ys*width;
			for(int y=0;y<height;y++)
			{
				for(int x=0;x<width;x++)
				{
					value[i] = full.value[fi];
					i++;
					fi++;
				}
			}
		}

    public void setDimensions(int w,int h)
    {
    	ensureCapacity(w*h);
    	width = w;
    	height = h;
    }

    public void setDimensionsAndClearNew(int w,int h)
    {
    	int oldl = width*height;
    	ensureCapacity(w*h);
    	width = w;
    	height = h;
    	int newl = w*h;
    	for(int i=oldl;i<newl;i++)
    	{
    		value[i] = null;
    	}
    }
    
    public int push_back_row(Object val1,Object val2)
    {
    	int ind = appendRow();
    	value[ind*2+0] = val1;
    	value[ind*2+1] = val2;
    	return ind;
    }

    public int push_back_row(Object val1,Object val2,Object val3)
    {
    	int ind = appendRow();
    	value[ind*3+0] = val1;
    	value[ind*3+1] = val2;
    	value[ind*3+2] = val3;
    	return ind;
    }

    public int push_back_row(Object val1,Object val2,Object val3,Object val4)
    {
    	int ind = appendRow();
    	value[ind*4+0] = val1;
    	value[ind*4+1] = val2;
    	value[ind*4+2] = val3;
    	value[ind*4+3] = val4;
    	return ind;
    }

    public int push_back_row(Object val1,Object val2,Object val3,Object val4,Object val5)
    {
    	int ind = appendRow();
    	value[ind*5+0] = val1;
    	value[ind*5+1] = val2;
    	value[ind*5+2] = val3;
    	value[ind*5+3] = val4;
    	value[ind*5+4] = val5;
    	return ind;
    }

    public int push_back_row(Object val1,Object val2,Object val3,Object val4,Object val5,Object val6)
    {
    	int ind = appendRow();
    	value[ind*6+0] = val1;
    	value[ind*6+1] = val2;
    	value[ind*6+2] = val3;
    	value[ind*6+3] = val4;
    	value[ind*6+4] = val5;
    	value[ind*6+5] = val6;
    	return ind;
    }

    public int push_back_row(Object val1,Object val2,Object val3,Object val4,Object val5,Object val6,Object val7,Object val8,Object val9)
    {
    	int ind = appendRow();
    	value[ind*9+0] = val1;
    	value[ind*9+1] = val2;
    	value[ind*9+2] = val3;
    	value[ind*9+3] = val4;
    	value[ind*9+4] = val5;
    	value[ind*9+5] = val6;
    	value[ind*9+6] = val7;
    	value[ind*9+7] = val8;
    	value[ind*9+8] = val9;
    	return ind;
    }

    public int push_back_row(Object val1,Object val2,Object val3,Object val4,Object val5,Object val6,Object val7,Object val8,Object val9,Object val10)
    {
    	int ind = appendRow();
    	value[ind*10+0] = val1;
    	value[ind*10+1] = val2;
    	value[ind*10+2] = val3;
    	value[ind*10+3] = val4;
    	value[ind*10+4] = val5;
    	value[ind*10+5] = val6;
    	value[ind*10+6] = val7;
    	value[ind*10+7] = val8;
    	value[ind*10+8] = val9;
    	value[ind*10+9] = val10;
    	return ind;
    }

    public void push_back_rows(GeneralMatrixObject rows)
    {
    	if(rows.height==0)
    		return;
    	appendRows(rows.height);
    	System.arraycopy(rows.value, 0, value, width*(height-rows.height), width*rows.height);
    }

    public void push_back_row_from_block(GeneralMatrixObject row,int yFromFull)
    {
    	appendRow();
    	System.arraycopy(row.value, yFromFull*width, value, width*(height-1), width);
    }

   }
 public class GeneralMatrixString implements Serializable 
{
	private static final long serialVersionUID = 1L;

	public int width = 0;
	public int height = 0;
	public String[] value;

    public GeneralMatrixString()
    {
    }
    public GeneralMatrixString(int width)
    {
    	this.width = width;
    	height = 0;
    	value = new String[8];
    }
    public GeneralMatrixString(int width,int height)
    {
    	this.width = width;
    	this.height = height;
    	value = new String[height*width];
    }
    public GeneralMatrixString(int width,int height,int mincap)
    {
    	this.width = width;
    	this.height = height;
    	value = new String[mincap];
    }
    public GeneralMatrixString(String val)
    {
    	this.width = 1;
    	this.height = 1;
    	value = new String[1];
    	value[0] = val;
    }
    public GeneralMatrixString(String val0,String val1)
    {
    	this.width = 1;
    	this.height = 2;
    	value = new String[2];
    	value[0] = val0;
    	value[1] = val1;
    }
    public GeneralMatrixString(String val0,String val1,String val2)
    {
    	this.width = 1;
    	this.height = 3;
    	value = new String[3];
    	value[0] = val0;
    	value[1] = val1;
    	value[2] = val2;
    }
    public GeneralMatrixString(String val0,String val1,String val2,String val3)
    {
    	this.width = 1;
    	this.height = 4;
    	value = new String[4];
    	value[0] = val0;
    	value[1] = val1;
    	value[2] = val2;
    	value[3] = val3;
    }
    public GeneralMatrixString(String val0,String val1,String val2,String val3,String val4)
    {
    	this.width = 1;
    	this.height = 5;
    	value = new String[5];
    	value[0] = val0;
    	value[1] = val1;
    	value[2] = val2;
    	value[3] = val3;
    	value[4] = val4;
    }
    public GeneralMatrixString(String val0,String val1,String val2,String val3,String val4,String val5)
    {
    	this.width = 1;
    	this.height = 6;
    	value = new String[6];
    	value[0] = val0;
    	value[1] = val1;
    	value[2] = val2;
    	value[3] = val3;
    	value[4] = val4;
    	value[5] = val5;
    }
    public GeneralMatrixString(String val0,String val1,String val2,String val3,String val4,String val5,String val6)
    {
    	this.width = 1;
    	this.height = 7;
    	value = new String[7];
    	value[0] = val0;
    	value[1] = val1;
    	value[2] = val2;
    	value[3] = val3;
    	value[4] = val4;
    	value[5] = val5;
    	value[6] = val6;
    }
    public GeneralMatrixString(String val0,String val1,String val2,String val3,String val4,String val5,String val6,String val7)
    {
    	this.width = 1;
    	this.height = 8;
    	value = new String[8];
    	value[0] = val0;
    	value[1] = val1;
    	value[2] = val2;
    	value[3] = val3;
    	value[4] = val4;
    	value[5] = val5;
    	value[6] = val6;
    	value[7] = val7;
    }
    public GeneralMatrixString(String val0,String val1,String val2,String val3,String val4,String val5,String val6,String val7,String val8)
    {
    	this.width = 1;
    	this.height = 9;
    	value = new String[9];
    	value[0] = val0;
    	value[1] = val1;
    	value[2] = val2;
    	value[3] = val3;
    	value[4] = val4;
    	value[5] = val5;
    	value[6] = val6;
    	value[7] = val7;
    	value[8] = val8;
    }
    public GeneralMatrixString(String val0,String val1,String val2,String val3,String val4,String val5,String val6,String val7,String val8,String val9,String val10,String val11,String val12)
    {
    	this.width = 1;
    	this.height = 13;
    	value = new String[13];
    	value[0] = val0;
    	value[1] = val1;
    	value[2] = val2;
    	value[3] = val3;
    	value[4] = val4;
    	value[5] = val5;
    	value[6] = val6;
    	value[7] = val7;
    	value[8] = val8;
    	value[9] = val9;
    	value[10] = val10;
    	value[11] = val11;
    	value[12] = val12;
    }
    public GeneralMatrixString(final GeneralMatrixString val)
    {
    	this.width = val.width;
    	this.height = val.height;
    	if(val.value!=null)
    		value = val.value.clone();
    }
    public GeneralMatrixString(int width,int height,String[] val)
    {
    	this.width = width;
    	this.height = height;
    	value = val;
    }
    public GeneralMatrixString(int width,int height,String val)
    {
    	this.width = width;
    	this.height = height;
    	int mincap = width*height;
    	value = new String[mincap];
    	for(int i=0;i<mincap;i++)
    		value[i] = ""+val;
    }
    public GeneralMatrixString(String[] val)
    {
    	this.width = 1;
    	if(val==null)
    	{
    		this.height = 0;
    		value = null;
    		return;
    	}
    	this.height = val.length;
    	value = val;
    }

    public Object clone()
    {
    	return new GeneralMatrixString(this);
    }
    
	public boolean isequal(GeneralMatrixString o)
	{
		if(!(width==o.width))
			return false;
		if(!(height==o.height))
			return false;
		for(int i=0;i<width*height;i++)
		{
			if(!(value[i]).contentEquals(o.value[i]))
				return false;
		}
		return true;

//		boolean is = true;
//		is = is && (width==o.width);
//		is = is && (height==o.height);
//		if(is)
//		{
//			for(int i=0;i<width*height;i++)
//			{
//				is = is && (value[i].contentEquals(o.value[i]));
//			}
//		}
//		if(!is)
//			System.out.println("diff!");
//		return is;
	}
	
    public void clear(String s)
    {
    	for(int i=0;i<(width*height);i++)
    	{
    		value[i] = ""+s;
    	}    	
    }
    
    public int contains(String v)
    {
    	for(int i=0;i<(width*height);i++)
    	{
    		if(value[i].contains(v))
    			return i;
    	}
    	return -1;    	
    }
    public int find(String v)
    {
    	for(int i=0;i<(width*height);i++)
    	{
    		if(value[i].equalsIgnoreCase(v))
    			return i;
    	}
    	return -1;
    }
    public boolean includes(GeneralMatrixString subset)
    {
    	for(int j=0;j<(subset.width*subset.height);j++)
    	{
    		boolean found = false;
	    	for(int i=0;i<(width*height);i++)
	    	{
	    		if(value[i].equalsIgnoreCase(subset.value[j]))
	    		{
	    			found = true;
	    			break;
	    		}
	    	}
			if(!found)
				return false;
    	}
    	return true;
    }
    public int findContaining(String v)
    {
    	for(int i=0;i<(width*height);i++)
    	{
    		if(value[i].contains(v))
    			return i;
    	}
    	return -1;
    }
    
    public void set(GeneralMatrixString o)
    {
    	width = o.width;
		height = o.height;
		ensureCapacity(height*width);	
		System.arraycopy(o.value, 0, value, 0, height*width);
    }
    public void set(String[] o,int h)
    {
		ensureCapacity(h);	
    	width = 1;
		height = h;
		System.arraycopy(o, 0, value, 0, height*width);
    }
    public int push_back(String o)
    {
    	int ind = appendRow();
    	value[ind] = o;
    	return ind;
    }
    
    public int appendRow()
    {
    	
    	int newSize = width*(height+1);
    	if((value == null)||(newSize>value.length))
    	{
    		ensureCapacity(newSize);
    	}
		height++;
		return height-1;
    }
    
    public int appendRows(int size)
    {
    	int newSize = width*(height+size);
    	if(value==null)
    	{
    		value = new String[newSize];
			height+=size;
			return height-size;
    	}
    	if(newSize>value.length)
    	{
    		ensureCapacity(newSize);
    	}
		height+=size;
		return height-size;
    }

    public void removeRow(int index)
    {
    	for(int i=index*width;i<((height-1))*width;i++)
    	{
    		value[i] = value[i+width];
    	}
    	height--;
    }

    public void ensureCapacity(int mincap)
    {
    	if(value == null)
    	{
    		value = new String[mincap];
    		return;
    	}
    	if(mincap<=value.length)
    		return;
        int newcap = (value.length * 3)/2 + 1;
        String[] olddata = value;
        value = new String[newcap < mincap ? mincap : newcap];
        System.arraycopy(olddata,0,value,0,olddata.length);    		
    }
    

	public static int find(String[] value,String v)
    {
		int height = value.length;
    	for(int i=0;i<(height);i++)
    	{
    		if(value[i].equalsIgnoreCase(v))
    			return i;
    	}
    	return -1;
    }
	public static int find(String[] value,int height,String v)
    {
    	for(int i=0;i<(height);i++)
    	{
    		if(value[i].equalsIgnoreCase(v))
    			return i;
    	}
    	return -1;
    }

    public void push_back_row(String o1,String o2)
    {
    	int ind = appendRow();
    	value[ind*2+0] = o1;
    	value[ind*2+1] = o2;
    }
    
    public void push_back_row(String o1,String o2,String o3)
    {
    	int ind = appendRow();
    	value[ind*3+0] = o1;
    	value[ind*3+1] = o2;
    	value[ind*3+2] = o3;
    }
    
    public void push_back_row(String o1,String o2,String o3,String o4)
    {
    	int ind = appendRow();
    	value[ind*4+0] = o1;
    	value[ind*4+1] = o2;
    	value[ind*4+2] = o3;
    	value[ind*4+3] = o4;
    }
    
    public void push_back_row(String o1,String o2,String o3,String o4,String o5)
    {
    	int ind = appendRow();
    	value[ind*5+0] = o1;
    	value[ind*5+1] = o2;
    	value[ind*5+2] = o3;
    	value[ind*5+3] = o4;
    	value[ind*5+4] = o5;
    }
    
    public void push_back_rows(GeneralMatrixString rows)
    {
    	appendRows(rows.height);
    	System.arraycopy(rows.value, 0, value, width*(height-rows.height), width*rows.height);
    }

    public void setDimensions(int w,int h)
    {
    	ensureCapacity(w*h);
    	width = w;
    	height = h;
    }

    public void insertRowAfter(int index)
    {
    	appendRow();
    	System.arraycopy(value, (index+1)*width, value, (index+2)*width, (height-1-(index+1))*width);
    }
    
  public void insertRowBefore(int index)
  {
  	int srcind = (index);
  	int destind = (index+1);
  	int length = (height-1-(index));
  	try{
	    	appendRow();
	    	System.arraycopy(value, srcind, value, destind, length);	    		
  	}
  	catch(Exception e)
  	{
  		System.out.println("insertRowBefore error");
  	}
  }
}
 public class Cuboid extends ParametricShape
{
	private static final long serialVersionUID = 1L;

	public static final String[] parameternames = {"width","depth","height"};
	public static final String[] sidenames = {"left","right","back","front","bottom","top"};
	public String[] getParameterNames() { return parameternames; }
	public String[] getSideNames() { return sidenames; }

	public Cuboid()
	{
		parameters = new double[]{4.5,3.5,2.4};
	}
	
	public void getShapePaths(String prefix,GeneralMatrixString paths)
	{
		paths.push_back(prefix+"/Cuboid");
	}

	public ParametricShape copy()
	{
		Cuboid c = new Cuboid();
		System.arraycopy(parameters, 0, c.parameters, 0, parameters.length);
		return c;
	}
	
	public void setCuboidDimensions(double x,double y,double z)
	{
		parameters[0] = x;
		parameters[1] = y;
		parameters[2] = z;
	}
	public void getCuboidDimensions(double[] xyz)
	{
		xyz[0] = parameters[0];
		xyz[1] = parameters[1];
		xyz[2] = parameters[2];
	}

} public class DeformedCuboid extends ParametricShape
{
	private static final long serialVersionUID = 1L;

	public static final String[] parameternames = 
		{
		"left_bottom_back_x",
		"left_bottom_back_y",
		"left_bottom_back_z",
		"right_bottom_back_x",
		"right_bottom_back_y",
		"right_bottom_back_z",
		"left_top_back_x",
		"left_top_back_y",
		"left_top_back_z",
		"right_top_back_x",
		"right_top_back_y",
		"right_top_back_z",
		"left_bottom_front_x",
		"left_bottom_front_y",
		"left_bottom_front_z",
		"right_bottom_front_x",
		"right_bottom_front_y",
		"right_bottom_front_z",
		"left_top_front_x",
		"left_top_front_y",
		"left_top_front_z",
		"right_top_front_x",
		"right_top_front_y",
		"right_top_front_z",
		};
	public static final String[] sidenames = {"left","right","back","front","bottom","top"};
	public String[] getParameterNames() { return parameternames; }
	public String[] getSideNames() { return sidenames; }

	public void getShapePaths(String prefix,GeneralMatrixString paths)
	{
		paths.push_back(prefix+"/DeformedCuboid");
	}

	public DeformedCuboid()
	{
		parameters = new double[]
				{
				0.0,0.0,0.0,
				1.0,0.0,0.0,
				0.0,1.0,0.0,
				1.0,1.0,0.0,

				0.0,0.0,1.0,
				1.0,0.0,1.0,
				0.0,1.0,1.0,
				1.0,1.0,1.0,
				};
	}
	
	public ParametricShape copy()
	{
		DeformedCuboid c = new DeformedCuboid();
		System.arraycopy(parameters, 0, c.parameters, 0, parameters.length);
		return c;
	}

	public void setCuboidDimensions(double x,double y,double z)
	{
		double minx = Float.MAX_VALUE;
		double miny = Float.MAX_VALUE;
		double minz = Float.MAX_VALUE;
		double maxx = -Float.MAX_VALUE;
		double maxy = -Float.MAX_VALUE;
		double maxz = -Float.MAX_VALUE;
		
		for(int vi=0;vi<8;vi++)
		{
			double vx = parameters[vi*3+0];
			double vy = parameters[vi*3+1];
			double vz = parameters[vi*3+2];
			
			if(vx<minx)
				minx = vx;
			if(vy<miny)
				miny = vy;
			if(vz<minz)
				minz = vz;
			
			if(vx>maxx)
				maxx = vx;
			if(vy>maxy)
				maxy = vy;
			if(vz>maxz)
				maxz = vz;
		}
		
		double dx = maxx-minx;
		double dy = maxy-miny;
		double dz = maxz-minz;
		
		double sx = x/dx;
		double sy = y/dy;
		double sz = z/dz;
		
		for(int vi=0;vi<8;vi++)
		{
			parameters[vi*3+0] *= sx;
			parameters[vi*3+1] *= sy;
			parameters[vi*3+2] *= sz;
		}
	}
	
	public void getAABB(double[] xyz)
	{
		double minx = Float.MAX_VALUE;
		double miny = Float.MAX_VALUE;
		double minz = Float.MAX_VALUE;
		double maxx = -Float.MAX_VALUE;
		double maxy = -Float.MAX_VALUE;
		double maxz = -Float.MAX_VALUE;
		
		for(int vi=0;vi<8;vi++)
		{
			double vx = parameters[vi*3+0];
			double vy = parameters[vi*3+1];
			double vz = parameters[vi*3+2];
			
			if(vx<minx)
				minx = vx;
			if(vy<miny)
				miny = vy;
			if(vz<minz)
				minz = vz;
			
			if(vx>maxx)
				maxx = vx;
			if(vy>maxy)
				maxy = vy;
			if(vz>maxz)
				maxz = vz;
		}
		xyz[0] = minx;
		xyz[1] = miny;
		xyz[2] = minz;
		xyz[3] = maxx;
		xyz[4] = maxy;
		xyz[5] = maxz;
	}
	
	public void getCuboidDimensions(double[] xyz)
	{
		double minx = Float.MAX_VALUE;
		double miny = Float.MAX_VALUE;
		double minz = Float.MAX_VALUE;
		double maxx = -Float.MAX_VALUE;
		double maxy = -Float.MAX_VALUE;
		double maxz = -Float.MAX_VALUE;
		
		for(int vi=0;vi<8;vi++)
		{
			double vx = parameters[vi*3+0];
			double vy = parameters[vi*3+1];
			double vz = parameters[vi*3+2];
			
			if(vx<minx)
				minx = vx;
			if(vy<miny)
				miny = vy;
			if(vz<minz)
				minz = vz;
			
			if(vx>maxx)
				maxx = vx;
			if(vy>maxy)
				maxy = vy;
			if(vz>maxz)
				maxz = vz;
		}
		
		xyz[0] = maxx-minx;
		xyz[1] = maxy-miny;
		xyz[2] = maxz-minz;
	}
} public class HumanShape extends ParametricShape 
{
	private static final long serialVersionUID = 1L;

	public static final String[] parameternames = 
	{
		"Root_x",
		"Root_y",
		"Root_z",
		"HipstoChest0_rx",
		"HipstoChest0_ry",
		"HipstoChest0_rz",
		"HipstoChest1_rx",
		"HipstoChest1_ry",
		"HipstoChest1_rz",
		"HipstoChest2_rx",
		"HipstoChest2_ry",
		"HipstoChest2_rz",
		"ChesttoNeck_rx",
		"ChesttoNeck_ry",
		"ChesttoNeck_rz",
		"NecktoHead_rx",
		"NecktoHead_ry",
		"NecktoHead_rz",
		"HeadtoHeadend_rx",
		"HeadtoHeadend_ry",
		"HeadtoHeadend_rz",
		"Hip_L_rx",
		"Hip_L_ry",
		"Hip_L_rz",
		"LeftUpLegtoLeftLowLeg_rx",
		"LeftUpLegtoLeftLowLeg_ry",
		"LeftUpLegtoLeftLowLeg_rz",
		"LeftLowLegtoLeftFoot_rx",
		"LeftLowLegtoLeftFoot_ry",
		"LeftLowLegtoLeftFoot_rz",
		"LeftFoottoLeftFootend_rx",
		"LeftFoottoLeftFootend_ry",
		"LeftFoottoLeftFootend_rz",
		"Toe_L_rx",
		"Toe_L_ry",
		"Toe_L_rz",
		"Hip_R_rx",
		"Hip_R_ry",
		"Hip_R_rz",
		"RightUpLegtoRightLowLeg_rx",
		"RightUpLegtoRightLowLeg_ry",
		"RightUpLegtoRightLowLeg_rz",
		"RightLowLegtoRightFoot_rx",
		"RightLowLegtoRightFoot_ry",
		"RightLowLegtoRightFoot_rz",
		"RightFoottoRightFootend_rx",
		"RightFoottoRightFootend_ry",
		"RightFoottoRightFootend_rz",
		"Toe_R_rx",
		"Toe_R_ry",
		"Toe_R_rz",
		"HeadtoMouth_rx",
		"HeadtoMouth_ry",
		"HeadtoMouth_rz",
		"HeadtoLEye_rx",
		"HeadtoLEye_ry",
		"HeadtoLEye_rz",
		"HeadtoREye_rx",
		"HeadtoREye_ry",
		"HeadtoREye_rz",
		"Spine3toLClavicle_rx",
		"Spine3toLClavicle_ry",
		"Spine3toLClavicle_rz",
		"Spine3toRClavicle_rx",
		"Spine3toRClavicle_ry",
		"Spine3toRClavicle_rz",
		"Jaw_rx",
		"Jaw_ry",
		"Jaw_rz",
		"Eye_R_rx",
		"Eye_R_ry",
		"Eye_R_rz",
		"Eye_L_rx",
		"Eye_L_ry",
		"Eye_L_rz",
		"UpLid_R_rx",
		"UpLid_R_ry",
		"UpLid_R_rz",
		"LoLid_R_rx",
		"LoLid_R_ry",
		"LoLid_R_rz",
		"UpLid_L_rx",
		"UpLid_L_ry",
		"UpLid_L_rz",
		"LoLid_L_rx",
		"LoLid_L_ry",
		"LoLid_L_rz",
		"LeftCollartoLeftUpArm_rx",
		"LeftCollartoLeftUpArm_ry",
		"LeftCollartoLeftUpArm_rz",
		"LeftUpArmtoLeftLowArm_rx",
		"LeftUpArmtoLeftLowArm_ry",
		"LeftUpArmtoLeftLowArm_rz",
		"LeftLowArmtoLeftHand_rx",
		"LeftLowArmtoLeftHand_ry",
		"LeftLowArmtoLeftHand_rz",
		"LeftHandtoLeftHandend_rx",
		"LeftHandtoLeftHandend_ry",
		"LeftHandtoLeftHandend_rz",
		"Wrist-1_L_rx",
		"Wrist-1_L_ry",
		"Wrist-1_L_rz",
		"Wrist-2_L_rx",
		"Wrist-2_L_ry",
		"Wrist-2_L_rz",
		"Palm-1_L_rx",
		"Palm-1_L_ry",
		"Palm-1_L_rz",
		"Palm-2_L_rx",
		"Palm-2_L_ry",
		"Palm-2_L_rz",
		"Palm-3_L_rx",
		"Palm-3_L_ry",
		"Palm-3_L_rz",
		"Palm-4_L_rx",
		"Palm-4_L_ry",
		"Palm-4_L_rz",
		"Palm-5_L_rx",
		"Palm-5_L_ry",
		"Palm-5_L_rz",
		"RightCollartoRightUpArm_rx",
		"RightCollartoRightUpArm_ry",
		"RightCollartoRightUpArm_rz",
		"RightUpArmtoRightLowArm_rx",
		"RightUpArmtoRightLowArm_ry",
		"RightUpArmtoRightLowArm_rz",
		"RightLowArmtoRightHand_rx",
		"RightLowArmtoRightHand_ry",
		"RightLowArmtoRightHand_rz",
		"RightHandtoRightHandend_rx",
		"RightHandtoRightHandend_ry",
		"RightHandtoRightHandend_rz",
		"Wrist-1_R_rx",
		"Wrist-1_R_ry",
		"Wrist-1_R_rz",
		"Wrist-2_R_rx",
		"Wrist-2_R_ry",
		"Wrist-2_R_rz",
		"Palm-1_R_rx",
		"Palm-1_R_ry",
		"Palm-1_R_rz",
		"Palm-2_R_rx",
		"Palm-2_R_ry",
		"Palm-2_R_rz",
		"Palm-3_R_rx",
		"Palm-3_R_ry",
		"Palm-3_R_rz",
		"Palm-4_R_rx",
		"Palm-4_R_ry",
		"Palm-4_R_rz",
		"Palm-5_R_rx",
		"Palm-5_R_ry",
		"Palm-5_R_rz",
		"Finger-1-1_L_rx",
		"Finger-1-1_L_ry",
		"Finger-1-1_L_rz",
		"Finger-1-2_L_rx",
		"Finger-1-2_L_ry",
		"Finger-1-2_L_rz",
		"Finger-1-3_L_rx",
		"Finger-1-3_L_ry",
		"Finger-1-3_L_rz",
		"Finger-2-1_L_rx",
		"Finger-2-1_L_ry",
		"Finger-2-1_L_rz",
		"Finger-2-2_L_rx",
		"Finger-2-2_L_ry",
		"Finger-2-2_L_rz",
		"Finger-2-3_L_rx",
		"Finger-2-3_L_ry",
		"Finger-2-3_L_rz",
		"Finger-3-1_L_rx",
		"Finger-3-1_L_ry",
		"Finger-3-1_L_rz",
		"Finger-3-2_L_rx",
		"Finger-3-2_L_ry",
		"Finger-3-2_L_rz",
		"Finger-3-3_L_rx",
		"Finger-3-3_L_ry",
		"Finger-3-3_L_rz",
		"Finger-4-1_L_rx",
		"Finger-4-1_L_ry",
		"Finger-4-1_L_rz",
		"Finger-4-2_L_rx",
		"Finger-4-2_L_ry",
		"Finger-4-2_L_rz",
		"Finger-4-3_L_rx",
		"Finger-4-3_L_ry",
		"Finger-4-3_L_rz",
		"Finger-5-1_L_rx",
		"Finger-5-1_L_ry",
		"Finger-5-1_L_rz",
		"Finger-5-2_L_rx",
		"Finger-5-2_L_ry",
		"Finger-5-2_L_rz",
		"Finger-5-3_L_rx",
		"Finger-5-3_L_ry",
		"Finger-5-3_L_rz",
		"Finger-1-1_R_rx",
		"Finger-1-1_R_ry",
		"Finger-1-1_R_rz",
		"Finger-1-2_R_rx",
		"Finger-1-2_R_ry",
		"Finger-1-2_R_rz",
		"Finger-1-3_R_rx",
		"Finger-1-3_R_ry",
		"Finger-1-3_R_rz",
		"Finger-2-1_R_rx",
		"Finger-2-1_R_ry",
		"Finger-2-1_R_rz",
		"Finger-2-2_R_rx",
		"Finger-2-2_R_ry",
		"Finger-2-2_R_rz",
		"Finger-2-3_R_rx",
		"Finger-2-3_R_ry",
		"Finger-2-3_R_rz",
		"Finger-3-1_R_rx",
		"Finger-3-1_R_ry",
		"Finger-3-1_R_rz",
		"Finger-3-2_R_rx",
		"Finger-3-2_R_ry",
		"Finger-3-2_R_rz",
		"Finger-3-3_R_rx",
		"Finger-3-3_R_ry",
		"Finger-3-3_R_rz",
		"Finger-4-1_R_rx",
		"Finger-4-1_R_ry",
		"Finger-4-1_R_rz",
		"Finger-4-2_R_rx",
		"Finger-4-2_R_ry",
		"Finger-4-2_R_rz",
		"Finger-4-3_R_rx",
		"Finger-4-3_R_ry",
		"Finger-4-3_R_rz",
		"Finger-5-1_R_rx",
		"Finger-5-1_R_ry",
		"Finger-5-1_R_rz",
		"Finger-5-2_R_rx",
		"Finger-5-2_R_ry",
		"Finger-5-2_R_rz",
		"Finger-5-3_R_rx",
		"Finger-5-3_R_ry",
		"Finger-5-3_R_rz",
		"JawtoTongue_rx",
		"JawtoTongue_ry",
		"JawtoTongue_rz",
		"TongueBase_rx",
		"TongueBase_ry",
		"TongueBase_rz",
		"TongueMid_rx",
		"TongueMid_ry",
		"TongueMid_rz",
		"TongueTip_rx",
		"TongueTip_ry",
		"TongueTip_rz",		
	};
	public static final String[] sidenames = {};
	public String[] getParameterNames() { return parameternames; }
	public String[] getSideNames() { return sidenames; }

	public void getShapePaths(String prefix,GeneralMatrixString paths)
	{
		for(int i=0;i<HumanDeformedCuboids.cuboid_names.length;i++)
		{
			paths.push_back(prefix+"/"+HumanDeformedCuboids.cuboid_names[i]+"/DeformedCuboid");
		}
	}

	public GeneralMatrixFloat pos = new GeneralMatrixFloat(3);
	public Skeleton skel = new Skeleton();
	public Skin skin = new Skin();

	public static void createMorphedBody(
//			GeneralMatrixString morphnames,
//			GeneralMatrixFloat morphmagnitudes,
			GeneralMatrixFloat pos,
			GeneralMatrixDouble vpos,
			GeneralMatrixDouble bmats,
			GeneralMatrixDouble bonelengths,
			GeneralMatrixDouble localbindbmats)
	{
		float[] fpos = HumanPos.pos; 
		pos.setDimensions(3,fpos.length/3);
		pos.set(fpos);
		
//		for(int i=0;i<morphnames.height;i++)
//		{
//			GeneralMatrixFloat mpos = new GeneralMatrixFloat();
//			GeneralMatrixInt mind = new GeneralMatrixInt();
//			Makehuman.getMorphIndex(morphnames.value[i], mpos, mind);
//			float mag = morphmagnitudes.value[i];
//			if((mind.value==null)&&(mpos.height>0))
//			{
//				for(int ind=0;ind<pos.height*3;ind+=3)
//				{
//					pos.value[ind+0] += mpos.value[ind+0]*mag;
//					pos.value[ind+1] += mpos.value[ind+1]*mag;
//					pos.value[ind+2] += mpos.value[ind+2]*mag;
//				}
//			}
//			else
//			{
//				for(int ei=0;ei<mind.height;ei++)
//				{
//					int ind = mind.value[ei]*3;
//					int eind = ei*3;
//					pos.value[ind+0] += mpos.value[eind+0]*mag;
//					pos.value[ind+1] += mpos.value[eind+1]*mag;
//					pos.value[ind+2] += mpos.value[eind+2]*mag;
//				}
//			}
//		}

		vpos.setDimensions(3, HumanJoints.names.length);
		//Calc the bind vpos positions
		 for(int vi=0;vi<vpos.height;vi++)
		 {
			 vpos.value[vi*3+0] = 0.0f;
			 vpos.value[vi*3+1] = 0.0f;
			 vpos.value[vi*3+2] = 0.0f;
		
			for(int i=0;i<6;i++)
			{
				int ind = HumanJoints.jointverts[vi*6+i];
				vpos.value[vi*3+0] += pos.value[ind*3+0];
				vpos.value[vi*3+1] += pos.value[ind*3+1];
				vpos.value[vi*3+2] += pos.value[ind*3+2];    			
			}
			vpos.value[vi*3+0] /= 6;
			vpos.value[vi*3+1] /= 6;
			vpos.value[vi*3+2] /= 6;
		 }

		 //Build the bind skeleton
		 GeneralMatrixDouble bmat = new GeneralMatrixDouble(3,3);
    	int bonen = HumanBones.bones.length/2;
		 bmats.setDimensions(9, bonen);
		 bonelengths.setDimensions(1, bonen);
    	for(int bi=0;bi<bonen;bi++)
    	{
    		int v0 = HumanBones.bones[bi*2+0];
    		int v1 = HumanBones.bones[bi*2+1];
	    	bmat.width = 3;
	    	bmat.height = 3;

			double dx = vpos.value[v1*3+0]-vpos.value[v0*3+0];
			double dy = vpos.value[v1*3+1]-vpos.value[v0*3+1];
			double dz = vpos.value[v1*3+2]-vpos.value[v0*3+2];
			double bls = Math.sqrt(dx*dx+dy*dy+dz*dz);
			bonelengths.value[bi] = bls;
	    	bmat.calcBasisFromY(dx, dy, dz,
	    			HumanBBmats.bbmatx[3*bi+0],HumanBBmats.bbmatx[3*bi+1],HumanBBmats.bbmatx[3*bi+2]);
	    	bmat.width = 9;
	    	bmat.height = 1;
	    	bmats.setRow(bi, bmat);
    	}

    	//Now recalculate the localbindbmats and bonelengths for the morphed body shape
		Animate.calcLocalBindMats(HumanBones.bones, HumanBones.bprnts, bmats, localbindbmats);
	}
	
	public HumanShape()
	{
		skel.boneParents.setDimensions(1, HumanBones.names.length);
		skel.boneParents.set(HumanBones.bprnts);
		skel.boneJoints.setDimensions(2,HumanBones.names.length);
		skel.boneJoints.set(HumanBones.bones);
		createMorphedBody(//morphnames,morphmagnitudes, 
				skin.bpos, 
				skel.vpos,skel.bmats,skel.bonelengths,skel.localbindbmats);

		skel.lpos.setDimensions(3,1+(HumanBones.bones.length/2));
		parameters = new double[skel.lpos.width*skel.lpos.height];

		skin.sb = HumanSkinToBoneMapping.sknb;
		skin.sw = HumanSkinToBoneWeights.sknw;

		skel.tvpos.setDimensions(skel.vpos.width,skel.vpos.height);
		skel.tbmats.setDimensions(skel.bmats.width,skel.bmats.height);

		float[] fpos = HumanPos.pos; 
		pos.setDimensions(3,fpos.length/3);
		
		parametersUpdated();
	}
	
	public ParametricShape copy()
	{
		HumanShape c = new HumanShape();
		System.arraycopy(parameters, 0, c.parameters, 0, parameters.length);
		c.parametersUpdated();
		return c;
	}

	public void parametersUpdated()
	{
		if(parameters==null)
		{
			parameters = new double[skel.lpos.width*skel.lpos.height];
			System.arraycopy(skel.lpos.value, 0, parameters, 0, parameters.length);
		}
		final double DEG2RAD = Math.PI/180.0;
		skel.lpos.set(parameters);
		for(int i=3;i<parameters.length;i++)
			skel.lpos.value[i] *= DEG2RAD;
		Animate.transformWithParams(skel.boneJoints.value, skel.boneParents.value,
				skel.bonelengths, skel.localbindbmats,
				skel.tvpos, skel.tbmats, skel.lpos);

    	Animate.updateSkinUsingSkeleton(skel.tvpos, skel.tbmats, skel.vpos, skel.bmats, skin.bpos, HumanBones.bones, skin.sb, skin.sw, pos);
	}
	
	public void setCuboidDimensions(double x,double y,double z)
	{
	}
	
	public void getCuboidDimensions(double[] xyz)
	{
		double minx = Float.MAX_VALUE;
		double miny = Float.MAX_VALUE;
		double minz = Float.MAX_VALUE;
		double maxx = -Float.MAX_VALUE;
		double maxy = -Float.MAX_VALUE;
		double maxz = -Float.MAX_VALUE;
		
		for(int vi=0;vi<pos.height;vi++)
		{
			double vx = pos.value[vi*3+0];
			double vy = pos.value[vi*3+1];
			double vz = pos.value[vi*3+2];
			
			if(vx<minx)
				minx = vx;
			if(vy<miny)
				miny = vy;
			if(vz<minz)
				minz = vz;
			
			if(vx>maxx)
				maxx = vx;
			if(vy>maxy)
				maxy = vy;
			if(vz>maxz)
				maxz = vz;
		}
		
		xyz[0] = maxx-minx;
		xyz[2] = maxy-miny;
		xyz[1] = maxz-minz;
	}

	public void getAABB(double[] xyz)
	{
		double minx = Float.MAX_VALUE;
		double miny = Float.MAX_VALUE;
		double minz = Float.MAX_VALUE;
		double maxx = -Float.MAX_VALUE;
		double maxy = -Float.MAX_VALUE;
		double maxz = -Float.MAX_VALUE;
		
		for(int vi=0;vi<pos.height;vi++)
		{
			double vx = pos.value[vi*3+0];
			double vy = pos.value[vi*3+1];
			double vz = pos.value[vi*3+2];
			
			if(vx<minx)
				minx = vx;
			if(vy<miny)
				miny = vy;
			if(vz<minz)
				minz = vz;
			
			if(vx>maxx)
				maxx = vx;
			if(vy>maxy)
				maxy = vy;
			if(vz>maxz)
				maxz = vz;
		}
		
		xyz[0] = minx;
		xyz[2] = miny;
		xyz[1] = minz;
		xyz[3] = maxx;
		xyz[5] = maxy;
		xyz[4] = maxz;

	}
} public abstract class ParametricShape implements Serializable 
{
	private static final long serialVersionUID = 1L;

	public static final String[] buildingShapes = {"RectangularBuildingFloor","ReverseRBuildingFloor"};
	public static final String[] roomShapes = {"RectangularRoom","RRoom"};
	public static final String[] portalShapes = {"RectangularPortal"};
	public static final String[] objectShapes = {"Cuboid","CSGShape","DeformedCuboid","QuadraticCurvedCuboid","ModifiedCopyShape","HumanShape"};
	public static final String[] characterShapes = {"HumanShape"};

	public static final String[] allsidenames = {"Left","Right","Bottom","Top","Back","Front"};

	public static String[] getShapesByType(String type)
	{
		if(type.equalsIgnoreCase("Building"))
		{
			return buildingShapes;
		}
		else
		if(type.equalsIgnoreCase("Room"))
		{
			return roomShapes;
		}
		else
		if(type.equalsIgnoreCase("Portal"))
		{
			return portalShapes;
		}
		else
		if(type.equalsIgnoreCase("Object"))
		{
			return objectShapes;
		}
		else
		if(type.equalsIgnoreCase("Character"))
		{
			return characterShapes;
		}
		return null;
	}
	
	public double[] parameters;
	
	public abstract void getShapePaths(String prefix,GeneralMatrixString paths);
	
	public abstract String[] getParameterNames();
	public abstract String[] getSideNames();
	
	public abstract void setCuboidDimensions(double x,double y,double z);
	public abstract void getCuboidDimensions(double[] xyz);
	
	public void getAABB(double[] xyzminandmax)
	{
		getCuboidDimensions(xyzminandmax);
		xyzminandmax[3] = xyzminandmax[0];
		xyzminandmax[4] = xyzminandmax[1];
		xyzminandmax[5] = xyzminandmax[2];
		xyzminandmax[0] = 0;
		xyzminandmax[1] = 0;
		xyzminandmax[2] = 0;
	}
	
	public abstract ParametricShape copy();
	
	public void parametersUpdated() {}
	
	public void makeSimilarSize(ParametricShape oldShape)
	{
		double[] xyz = new double[3];
		oldShape.getCuboidDimensions(xyz);
		setCuboidDimensions(xyz[0], xyz[1], xyz[2]);
	}

	public static ParametricShape get(String name)
	{
		if(name.equalsIgnoreCase("HumanShape"))
		{
			return new HumanShape();
		}
		else
		if(name.equalsIgnoreCase("ModifiedCopyShape"))
		{
			return new ModifiedCopyShape();
		}
		else
		if(name.equalsIgnoreCase("QuadraticCurvedCuboid"))
		{
			return new QuadraticCurvedCuboid();
		}
		else
		if(name.equalsIgnoreCase("DeformedCuboid"))
		{
			return new DeformedCuboid();
		}
		else
		if(name.equalsIgnoreCase("CSGShape"))
		{
			return new CSGShape();
		}
		else
		if(name.equalsIgnoreCase("Cuboid"))
		{
			return new Cuboid();
		}
		else
		if(name.equalsIgnoreCase("RectangularPortal"))
		{
			return new RectangularPortal();
		}
		else
		if(name.equalsIgnoreCase("RectangularRoom"))
		{
			return new RectangularRoom();
		}
		else
		if(name.equalsIgnoreCase("RRoom"))
		{
			return new RRoom();
		}
		else
		if(name.equalsIgnoreCase("RectangularBuildingFloor"))
		{
			return new RectangularBuildingFloor();
		}
		else
		if(name.equalsIgnoreCase("ReverseRBuildingFloor"))
		{
			return new ReverseRBuildingFloor();
		}
		return null;
	}
} public class HumanDeformedCuboids 
{
	public static final int[] head = 
		{
				186,
				310,
				142,
				328,
				180,
				316,
				219,
				302,
		};

		public static final int[] skullfront = 
		{
				180,
				316,
				221,
				300,
				271,
				463,
				177,
				317,
		};

		public static final int[] skullfront2 = 
		{
				271,
				463,
				177,
				317,
				56,
				489,
				154,
				326,
		};

		public static final int[] nose = 
		{
				56,
				489,
				75,
				338,
				57,
				344,
				58,
				978,
		};

		public static final int[] earl = 
		{
				213,
				223,
				217,
				220,
				215,
				225,
				216,
				222,
		};

		public static final int[] eartopl = 
		{
				214,
				224,
				213,
				223,
				228,
				229,
				215,
				225,
		};

		public static final int[] earbackl = 
		{
				212,
				226,
				218,
				227,
				213,
				223,
				217,
				220,
		};

		public static final int[] earr = 
		{
				298,
				308,
				301,
				304,
				296,
				306,
				299,
				305,
		};

		public static final int[] eartopr = 
		{
				297,
				307,
				298,
				308,
				292,
				293,
				296,
				306,
		};

		public static final int[] earbackr = 
		{
				295,
				309,
				294,
				303,
				298,
				308,
				301,
				304,
		};

		public static final int[] skullfronttop = 
		{
				180,
				316,
				271,
				463,
				44,
				356,
				56,
				489,
		};

		public static final int[] skullmidtop = 
		{
				185,
				311,
				186,
				310,
				184,
				312,
				44,
				356,
		};

		public static final int[] skullmidleft = 
		{
				182,
				185,
				183,
				186,
				181,
				184,
				180,
				44,
		};

		public static final int[] skullmidright = 
		{
				311,
				314,
				310,
				313,
				312,
				315,
				356,
				316,
		};

		public static final int[] skullleft = 
		{
				183,
				186,
				148,
				142,
				180,
				180,
				219,
				219,
		};

		public static final int[] skullright = 
		{
				310,
				313,
				328,
				327,
				316,
				316,
				302,
				302,
		};

		public static final int[] jaw = 
		{
				221,
				300,
				219,
				302,
				68,
				339,
				53,
				347,
		};

		public static final int[] chin = 
		{
				68,
				339,
				53,
				347,
				45,
				355,
				46,
				354,
		};

		public static final int[] neck = 
		{
				142,
				328,
				143,
				414,
				148,
				327,
				124,
				431,
		};

		public static final int[] neck2 = 
		{
				143,
				414,
				141,
				415,
				124,
				431,
				188,
				404,
		};

		public static final int[] uppertorso = 
		{
				141,
				415,
				97,
				454,
				188,
				404,
				139,
				417,
		};

		public static final int[] ribcage = 
		{
				139,
				417,
				96,
				455,
				140,
				416,
				153,
				406,
		};

		public static final int[] breastl = 
		{
				188,
				187,
				140,
				151,
				110,
				109,
				98,
				150,
		};

		public static final int[] breastr = 
		{
				405,
				404,
				408,
				416,
				442,
				441,
				409,
				453,
		};

		public static final int[] chinfront = 
		{
				219,
				302,
				148,
				327,
				46,
				354,
				133,
				329,
		};

		public static final int[] neckfront = 
		{
				148,
				327,
				124,
				431,
				133,
				329,
				134,
				422,
		};

		public static final int[] neckfront2 = 
		{
				124,
				431,
				188,
				404,
				134,
				422,
				187,
				405,
		};

		public static final int[] pelvis = 
		{
				120,
				284,
				119,
				285,
				144,
				283,
				111,
				287,
		};

		public static final int[] pelvis2 = 
		{
				119,
				285,
				282,
				282,
				111,
				287,
				112,
				286,
		};

		public static final int[] midsection = 
		{
				152,
				407,
				120,
				284,
				153,
				406,
				144,
				283,
		};

		public static final int[] back = 
		{
				96,
				455,
				189,
				277,
				152,
				407,
				120,
				284,
		};

		public static final int[] upperlegl = 
		{
				119,
				282,
				192,
				163,
				111,
				112,
				190,
				191,
		};

		public static final int[] upperlegr = 
		{
				282,
				285,
				281,
				274,
				286,
				287,
				275,
				276,
		};

		public static final int[] buttockl = 
		{
				230,
				176,
				231,
				165,
				120,
				189,
				164,
				282,
		};

		public static final int[] buttockr = 
		{
				278,
				273,
				279,
				272,
				277,
				284,
				282,
				280,
		};

		public static final int[] lowerlegl = 
		{
				192,
				163,
				167,
				232,
				190,
				191,
				169,
				168,
		};

		public static final int[] lowerlegr = 
		{
				281,
				274,
				491,
				502,
				275,
				276,
				501,
				500,
		};

		public static final int[] footl = 
		{
				167,
				232,
				166,
				233,
				204,
				210,
				205,
				208,
		};

		public static final int[] footr = 
		{
				491,
				502,
				490,
				503,
				493,
				499,
				495,
				498,
		};

		public static final int[] toesl = 
		{
				204,
				210,
				205,
				208,
				211,
				209,
				206,
				207,
		};

		public static final int[] toesr = 
		{
				493,
				499,
				495,
				498,
				494,
				492,
				496,
				497,
		};

		public static final int[] shoulderl = 
		{
				149,
				141,
				145,
				97,
				118,
				110,
				114,
				113,
		};

		public static final int[] upperarml = 
		{
				194,
				149,
				193,
				145,
				196,
				118,
				195,
				114,
		};

		public static final int[] lowerarml = 
		{
				197,
				194,
				203,
				193,
				126,
				196,
				127,
				195,
		};

		public static final int[] handl = 
		{
				123,
				197,
				131,
				203,
				130,
				126,
				136,
				127,
		};

		public static final int[] thumb1l = 
		{
				115,
				128,
				89,
				90,
				135,
				95,
				117,
				116,
		};

		public static final int[] thumb2l = 
		{
				138,
				116,
				132,
				117,
				137,
				95,
				125,
				135,
		};

		public static final int[] finger1_1l = 
		{
				239,
				129,
				244,
				91,
				240,
				130,
				243,
				136,
		};

		public static final int[] finger1_2l = 
		{
				246,
				239,
				242,
				244,
				245,
				240,
				241,
				243,
		};

		public static final int[] finger1_3l = 
		{
				238,
				246,
				235,
				242,
				237,
				245,
				236,
				241,
		};

		public static final int[] finger2_1l = 
		{
				106,
				199,
				101,
				92,
				105,
				198,
				102,
				93,
		};

		public static final int[] finger2_2l = 
		{
				99,
				106,
				103,
				101,
				100,
				105,
				104,
				102,
		};

		public static final int[] finger2_3l = 
		{
				107,
				99,
				202,
				103,
				108,
				100,
				201,
				104,
		};

		public static final int[] finger3_1l = 
		{
				251,
				200,
				256,
				147,
				252,
				121,
				255,
				94,
		};

		public static final int[] finger3_2l = 
		{
				258,
				251,
				254,
				256,
				257,
				252,
				253,
				255,
		};

		public static final int[] finger3_3l = 
		{
				250,
				258,
				247,
				254,
				249,
				257,
				248,
				253,
		};

		public static final int[] finger4_1l = 
		{
				266,
				123,
				261,
				131,
				265,
				122,
				262,
				146,
		};

		public static final int[] finger4_2l = 
		{
				259,
				266,
				263,
				261,
				260,
				265,
				264,
				262,
		};

		public static final int[] finger4_3l = 
		{
				267,
				259,
				270,
				263,
				268,
				260,
				269,
				264,
		};

		public static final int[] shoulderr = 
		{
				415,
				410,
				454,
				413,
				441,
				435,
				440,
				439,
		};

		public static final int[] upperarmr = 
		{
				410,
				402,
				413,
				403,
				435,
				400,
				439,
				401,
		};

		public static final int[] lowerarmr = 
		{
				402,
				399,
				403,
				393,
				400,
				429,
				401,
				428,
		};

		public static final int[] handr = 
		{
				399,
				432,
				393,
				424,
				429,
				425,
				428,
				420,
		};

		public static final int[] thumb1r = 
		{
				427,
				438,
				461,
				462,
				456,
				421,
				437,
				436,
		};

		public static final int[] thumb2r = 
		{
				437,
				418,
				436,
				423,
				456,
				419,
				421,
				430,
		};

		public static final int[] finger1_1r = 
		{
				426,
				388,
				460,
				383,
				425,
				387,
				420,
				384,
		};

		public static final int[] finger1_2r = 
		{
				388,
				381,
				383,
				385,
				387,
				382,
				384,
				386,
		};

		public static final int[] finger1_3r = 
		{
				381,
				389,
				385,
				392,
				382,
				390,
				386,
				391,
		};

		public static final int[] finger2_1r = 
		{
				397,
				445,
				459,
				450,
				398,
				446,
				458,
				449,
		};

		public static final int[] finger2_2r = 
		{
				445,
				452,
				450,
				448,
				446,
				451,
				449,
				447,
		};

		public static final int[] finger2_3r = 
		{
				452,
				444,
				448,
				394,
				451,
				443,
				447,
				395,
		};

		public static final int[] finger3_1r = 
		{
				396,
				376,
				411,
				371,
				434,
				375,
				457,
				372,
		};

		public static final int[] finger3_2r = 
		{
				376,
				369,
				371,
				373,
				375,
				370,
				372,
				374,
		};

		public static final int[] finger3_3r = 
		{
				369,
				377,
				373,
				380,
				370,
				378,
				374,
				379,
		};

		public static final int[] finger4_1r = 
		{
				432,
				361,
				424,
				366,
				433,
				362,
				412,
				365,
		};

		public static final int[] finger4_2r = 
		{
				361,
				368,
				366,
				364,
				362,
				367,
				365,
				363,
		};

		public static final int[] finger4_3r = 
		{
				368,
				360,
				364,
				357,
				367,
				359,
				363,
				358,
		};
		
		public static final String[] cuboid_names =
			{
				"head",
				"skullfront",
				"skullfront2",
				"nose",
				"earl",
				"eartopl",
				"earbackl",
				"earr",
				"eartopr",
				"earbackr",
				"skullfronttop",
				"skullmidtop",
				"skullmidleft",
				"skullmidright",
				"skullleft",
				"skullright",
				"jaw",
				"chin",
				
				"neck",
				"neck2",
				"uppertorso",
				"ribcage",
				"breastl",
				"breastr",
				
				"chinfront",
				"neckfront",
				"neckfront2",
				
				"pelvis",
				"pelvis2",
				"midsection",
				"back",
				"upperlegl",
				"upperlegr",
				"buttockl",
				"buttockr",
				"lowerlegl",
				"lowerlegr",
				"footl",
				"footr",
				"toesl",
				"toesr",
				"shoulderl",
				"upperarml",
				"lowerarml",
				
				"handl",
				"thumb1l",
				"thumb2l",
				"finger1_1l",
				"finger1_2l",
				"finger1_3l",
				
				"finger2_1l",
				"finger2_2l",
				"finger2_3l",
				
				"finger3_1l",
				"finger3_2l",
				"finger3_3l",
				
				"finger4_1l",
				"finger4_2l",
				"finger4_3l",
				
				"shoulderr",
				"upperarmr",
				"lowerarmr",
				
				"handr",
				"thumb1r",
				"thumb2r",
				"finger1_1r",
				"finger1_2r",
				"finger1_3r",
				
				"finger2_1r",
				"finger2_2r",
				"finger2_3r",
				
				"finger3_1r",
				"finger3_2r",
				"finger3_3r",
				
				"finger4_1r",
				"finger4_2r",
				"finger4_3r",
			};
			public static final int[][] cuboid = 
			{
				head,
				skullfront,
				skullfront2,
				nose,
				earl,
				eartopl,
				earbackl,
				earr,
				eartopr,
				earbackr,
				skullfronttop,
				skullmidtop,
				skullmidleft,
				skullmidright,
				skullleft,
				skullright,
				jaw,		
				chin,		
				
				neck,
				neck2,
				uppertorso,
				ribcage,
				breastl,
				breastr,
				
				chinfront,
				neckfront,
				neckfront2,
				
				pelvis,
				pelvis2,
				midsection,
				back,
				upperlegl,
				upperlegr,
				buttockl,
				buttockr,
				lowerlegl,
				lowerlegr,
				footl,
				footr,
				toesl,
				toesr,
				shoulderl,
				upperarml,
				lowerarml,

				handl,
				thumb1l,
				thumb2l,
				finger1_1l,
				finger1_2l,
				finger1_3l,

				finger2_1l,
				finger2_2l,
				finger2_3l,

				finger3_1l,
				finger3_2l,
				finger3_3l,

				finger4_1l,
				finger4_2l,
				finger4_3l,
				
				shoulderr,
				upperarmr,
				lowerarmr,

				handr,
				thumb1r,
				thumb2r,
				finger1_1r,
				finger1_2r,
				finger1_3r,

				finger2_1r,
				finger2_2r,
				finger2_3r,

				finger3_1r,
				finger3_2r,
				finger3_3r,

				finger4_1r,
				finger4_2r,
				finger4_3r,
				
			};
}
import java.io.Serializable;
import ai.knowledge_representation.state_instance.ObjectLanguageInstance;
import utils.FileStoreInterface;
import utils.GeneralMatrixDouble;
import utils.GeneralMatrixObject;
import utils.GeneralMatrixString;
import utils.shapes.Cuboid;
import utils.shapes.DeformedCuboid;
import utils.shapes.HumanShape;
import utils.shapes.ParametricShape;
import utils.shapes.human.HumanDeformedCuboids;
package utils.constraints;



public abstract class PhysicalConstraint implements Serializable
{
	private static final long serialVersionUID = 1L;

	public static final String[] empty = {};
	public static final String[] constraintnames = {"AbsoluteTransform","AbsoluteLocalBoneRotation","RootTranslationRelativeToShapeSurface","IKRelativeToShapeSurface","AbsoluteWorldBoneFacingRelativeToBindMatrix","IKRelativeToSelf","AbsoluteRootTranslation"};

	public double[] parameters;
	public abstract String[] getParameterNames();
	public int[] constrainees;
	public abstract String[] getConstraineeNames();
	public int[] bones;
	public String[] getBoneNames() {return empty;}
	public String[][] shapes;
	public String[] getShapeNames() {return empty;}
	public String[] shape_side;
	public String[] getShapeSideNames() {return empty;}
//	public String[] shape_corner;
//	public String[] getShapeCornerNames() {return empty;}
	
	public abstract int numConstrainees();
	public abstract int numErrorTerms();
	public abstract void calcErrors(FileStoreInterface fs,double[] e,int offset,GeneralMatrixObject characters,GeneralMatrixObject objects,GeneralMatrixDouble transforms);
	public abstract void minimiseErrors(FileStoreInterface fs,GeneralMatrixObject characters,GeneralMatrixObject objects,GeneralMatrixDouble transforms);
	
	public abstract void minimiseErrors(GeneralMatrixString objectpaths,GeneralMatrixObject objects,GeneralMatrixString posepaths,GeneralMatrixObject poses,
			GeneralMatrixObject pcharacters,GeneralMatrixObject pobjects,GeneralMatrixDouble transforms);			
	
	public void parametersUpdated() {}
	
	public static PhysicalConstraint[] add(PhysicalConstraint[] v, PhysicalConstraint i)
	{
		PhysicalConstraint[] nv = new PhysicalConstraint[v.length+1];
		System.arraycopy(v, 0, nv, 0, v.length);
		nv[v.length] = i;
		return nv;
	}
	public static PhysicalConstraint[] remove(PhysicalConstraint[] v, int ind,int len)
	{
		PhysicalConstraint[] nv = new PhysicalConstraint[v.length-len];
		if(ind!=0)
			System.arraycopy(v, 0, nv, 0, ind);
		if((v.length-(ind+len))>=0)
			System.arraycopy(v, ind+len, nv, ind, v.length-(ind+len));
		return nv;
	}

	public static PhysicalConstraint get(String name)
	{
		if(name.equalsIgnoreCase("AbsoluteTransform"))
			return new AbsoluteTransform();
		if(name.equalsIgnoreCase("AbsoluteLocalBoneRotation"))
			return new AbsoluteLocalBoneRotation();
		if(name.equalsIgnoreCase("RootTranslationRelativeToShapeSurface"))
			return new RootTranslationRelativeToShapeSurface();
		if(name.equalsIgnoreCase("IKRelativeToShapeSurface"))
			return new IKRelativeToShapeSurface();
		if(name.equalsIgnoreCase("AbsoluteWorldBoneFacingRelativeToBindMatrix"))
			return new AbsoluteWorldBoneFacingRelativeToBindMatrix();
		if(name.equalsIgnoreCase("IKRelativeToSelf"))
			return new IKRelativeToSelf();
		if(name.equalsIgnoreCase("AbsoluteRootTranslation"))
			return new AbsoluteRootTranslation();
		return null;
	}
	
	public static void getShapeTransform(
			double[] deformedCuboidVerts,
			double rx,double ry,double rz,
			GeneralMatrixDouble surfaceTransform)
	{
		//Back
		//Front
		double dx,dy,dz;
		double xdx=0,xdy=0,xdz=0;
		double ydx=0,ydy=0,ydz=0;
		double zdx=0,zdy=0,zdz=0;
		
		double[] zprojectCorners = new double[4*3];
		for(int i=0;i<4;i++)
		{
			dx = (deformedCuboidVerts[3*i+0+3*4]-deformedCuboidVerts[3*i+0])*rz;
			dy = (deformedCuboidVerts[3*i+1+3*4]-deformedCuboidVerts[3*i+1])*rz;
			dz = (deformedCuboidVerts[3*i+2+3*4]-deformedCuboidVerts[3*i+2])*rz;

			zdx += dx;
			zdy += dy;
			zdz += dz;
			
			zprojectCorners[i*3+0] = dx+deformedCuboidVerts[3*i+0];
			zprojectCorners[i*3+1] = dy+deformedCuboidVerts[3*i+1];
			zprojectCorners[i*3+2] = dz+deformedCuboidVerts[3*i+2];
		}
		
		
		double[] yprojectCorners = new double[2*3];
		for(int i=0;i<2;i++)
		{
			dx = (zprojectCorners[i*3+0+3*2]-zprojectCorners[i*3+0])*ry;
			dy = (zprojectCorners[i*3+1+3*2]-zprojectCorners[i*3+1])*ry;
			dz = (zprojectCorners[i*3+2+3*2]-zprojectCorners[i*3+2])*ry;
			
			ydx += dx;
			ydy += dy;
			ydz += dz;
			
			yprojectCorners[i*3+0] = dx+zprojectCorners[3*i+0];
			yprojectCorners[i*3+1] = dy+zprojectCorners[3*i+1];
			yprojectCorners[i*3+2] = dz+zprojectCorners[3*i+2];
		}
		
		dx = (yprojectCorners[0+3]-yprojectCorners[0])*rx;
		dy = (yprojectCorners[1+3]-yprojectCorners[1])*rx;
		dz = (yprojectCorners[2+3]-yprojectCorners[2])*rx;
		
		xdx += dx;
		xdy += dy;
		xdz += dz;

		surfaceTransform.set3DTransformPosition(dx+yprojectCorners[0], 
				dy+yprojectCorners[1], 
				dz+yprojectCorners[2]);
	}
	
	static final int[] humanCuboidToDeformedCuboid = {
//		6,7,
//		2,3,
//		4,5,
//		0,1,
		0,1,
		2,3,
		4,5,
		6,7,
	};
	
	
	
	public static void getShapeTransform(GeneralMatrixString objectpaths,GeneralMatrixObject wobjects,
			ObjectLanguageInstance o,String[] relativeShape,int offset,String side,