using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.ComponentModel;

using System.Drawing;
using MyGeometry;


using System.IO;


namespace FunctionalSubstructure
{
	public enum MutualRelation { PARALLEL, COPLANAR, CONTAINER, ONOBJECT, REPETITION, SYMMETRY, JOINT, HEIGHTPRESERVE, NONE };
	public enum TransformStyle { Translation = 0, Rotation, Scaling, None }
	public enum ShapeType2D { TRIANGLE, QUAD, POLYGON, CIRCLLE, NONE };
	public enum ProxyConstrutionMode { AXISALIGNED, SPECIFYHEIGHT, OTHERS, NONE }
	public enum ProxyType { BOX, QUAD, POLY, CYLINDER, CONE };
	public struct Transform
	{
		public Matrix4d T;
		public TransformStyle style;
		public Transform(Matrix4d t, TransformStyle s)
		{
			this.T = t;
			this.style = s;
		}
	}
	public unsafe class HelperFunction
	{
		public static PointF Vector2PointF(Vector2d v)
		{
			return new PointF((float)v.x, (float)v.y);
		}
		public static Vector2d PointF2Vecter2d(PointF p)
		{
			return new Vector2d(p.X, p.Y);
		}
		public static double Distance2Line(Vector2d p, Vector2d u, Vector2d v)
		{
			Vector2d uv = (v - u).Normalize();
			Vector2d up = p - u;
			return (up - up.Dot(uv) * uv).Length();
		}
		public static double Distance2LineSegment(Vector2d p, Vector2d u, Vector2d v)
		{
			bool isProjectionIn = !((p - u).Dot(v - u) < 0 || (p - v).Dot(u - v) < 0);
			if (!isProjectionIn)
			{
				return Math.Min((p - u).Length(), (p - v).Length());
			}
			else
			{
				Vector2d uv = (v - u).Normalize();
				Vector2d up = p - u;
				return (up - up.Dot(uv) * uv).Length();
			}
		}
		public static double Distance2LineSegment(Vector3d p, Vector3d u, Vector3d v, out double r)
		{
			bool isProjectionIn = !((p - u).Dot(v - u) < 0 || (p - v).Dot(u - v) < 0);
			Vector3d uv = (v - u).Normalize();
			Vector3d up = p - u;
			double l = up.Dot(uv);
			r = l / (v - u).Length();
			if (!isProjectionIn)
			{
				return Math.Min(up.Length(), (p - v).Length());
			}
			else
			{
				return (up - l * uv).Length();
			}
		}
		public static double Distance2LineSegment(Vector2d p, Vector2d u, Vector2d v, out double projectionRatio)
		{
			Vector2d uv = (v - u).Normalize();
			Vector2d up = (p - u);
			double l = up.Dot(uv);
			projectionRatio = l / (v - u).Length();

			return (up - l * uv).Length();

		}
		public static Vector2d FindLinesIntersectPoint(LineSegment l1, LineSegment l2)
		{
			Vector3d u1 = new Vector3d(l1.u.pos, 1), v1 = new Vector3d(l1.v.pos, 1);
			Vector3d u2 = new Vector3d(l2.u.pos, 1), v2 = new Vector3d(l2.v.pos, 1);
			Vector3d uv = u1.Cross(v1).Cross(u2.Cross(v2)).HomogenousNormalize();
			return uv.ToVector2d();
		}
		public static Matrix4d ReflectionalMatrix(Vector3d plane_normal)
		{
			// create an coordinates sys, with plane_normal as x-axis
			Vector3d x = plane_normal;
			Vector3d y;
			if (x.x == 0 && x.y == 0)
				y = new Vector3d(1, 0, 0);
			else
				y = (new Vector3d(-x.y, x.x, 0)).Normalize();
			Vector3d z = x.Cross(y).Normalize();
			Matrix3d R = new Matrix3d(x, y, z).Transpose();
			Matrix3d InvR = R.Inverse();
			Matrix4d U = new Matrix4d(R);
			Matrix4d V = new Matrix4d(InvR);
			Matrix4d I = Matrix4d.IdentityMatrix();
			I[0, 0] = -1; // reflect matrix along yz plane
			return V * I * U;
		}
		public static Vector3d GetMirrorSymmetryPoint(Vector3d pt, Vector3d normal, Vector3d center)
		{
			Matrix4d R = ReflectionalMatrix(normal);
			return center + (R * new Vector4d(pt - center, 0)).XYZ();
		}
		public static Vector3d GetMirrorSymmetryVector(Vector3d vec, Vector3d normal)
		{
			Matrix4d R = ReflectionalMatrix(normal);
			return (R * new Vector4d(vec, 0)).XYZ();
		}
	}
	public class CoordinateFrame
	{
		public Vector3d o, x, y, z;
		public CoordinateFrame(Vector3d o_, Vector3d x_, Vector3d y_, Vector3d z_)
		{
			this.o = o_;
			this.x = x_;
			this.y = y_;
			this.z = z_;
			this.R_inv = new Matrix3d(x_, y_, z_);
		}
		public Matrix3d R_inv = null;
		public Vector3d GetPointLocalCoord(Vector3d p)
		{
			Vector3d po = (p - this.o);
			return new Vector3d(po.Dot(this.x), po.Dot(this.y), po.Dot(this.z));
		}
		public Vector3d GetPointSpaceCoord(Vector3d p)
		{
			return this.R_inv * p + this.o;
		}
		public Vector3d PointAtCoord(Vector2d coord)
		{
			return this.R_inv * new Vector3d(coord, 0) + this.o;
		}

	}
	public class Point2 : ICloneable
	{
		public bool occluded = false;
		public int index = -1;
		public Vector2d pos, oldpos;
		public Vector2d texcoord;
		public Point2(Vector2d pt)
		{
			this.oldpos = this.pos = pt;
			this.texcoord = this.pos;
		}
		public Point2()
		{
			this.oldpos = this.pos = new Vector2d();
			this.texcoord = this.pos;
		}
		public void Translate(Vector2d off)
		{
			this.pos = this.oldpos + off;
		}
		public void UpdateOldPos()
		{
			this.oldpos = this.pos;
		}
		public object Clone()
		{
			Point2 another = new Point2(this.pos);
			another.index = this.index;
			another.texcoord = this.texcoord;
			another.occluded = this.occluded;
			return another;
		}
	}
	public class Point3 : ICloneable
	{
		public int index = -1;
		public List<Point3> symmpoints = null;
		public Matrix3d R = null; // local frame encoding for manipulation, R does not change unless rotated
		public Vector3d pos, oldpos;
		public Vector3d normal;
		public Point3(Vector3d pt)
		{
			this.oldpos = this.pos = pt;
		}
		public Point3()
		{
			this.pos = new Vector3d();
		}
		public void UpdateOldPos()
		{
			this.oldpos = this.pos;
		}
		public object Clone()
		{
			Point3 another = new Point3(this.pos);
			if (this.R != null)
				another.R = new Matrix3d(this.R);
			another.index = this.index;
			another.normal = this.normal;
			return another;
		}
	}
	public class LineSegment : IComparable<LineSegment>
	{
		public Point2 u, v;
		public Point3 u3 = null, v3 = null;
		public int vanishingindex = -1;			// which vanishing direction it corresponds
		public double length = 0;
		public double fiterror = 0;
		public LineSegment(Point2 _u, Point2 _v)
		{
			this.u = _u;
			this.v = _v;
			this.vanishingindex = -1;
			this.length = this.GetLength();
		}
		public LineSegment(Point2 _u, Point2 _v, int _i)
		{
			this.u = _u;
			this.v = _v;
			this.vanishingindex = _i;
			this.length = this.GetLength();
		}
		public void Translate(Vector2d off)
		{
			this.u.Translate(off);
			this.v.Translate(off);
		}
		public double GetLength()
		{
			return (this.v.pos - this.u.pos).Length();
		}
		#region IComparable<LineSegment> Members
		public int CompareTo(LineSegment other)
		{
			return Math.Sign(length - other.length);
		}

		#endregion
	}
	public class Shape2D
	{
		public Vector2d center2;
		public Vector3d normal;
		public double radius2 = 0;
		public int textureindex = -1;
		public bool visible = true;
		public ShapeType2D shaptype = ShapeType2D.NONE;
		public List<Point2> points = null;
		public List<int> boxptindice = null;
		public List<LineSegment> linesegments = new List<LineSegment>();
		public Shape2D()
		{
		}
		public void AddPoint(Point2 pt)
		{
			if (this.points == null)
			{
				this.points = new List<Point2>();
				this.linesegments = new List<LineSegment>();
			}
			this.points.Add(pt);
			int n = this.points.Count;
			if (n < 2) return;
			this.linesegments.Add(
				new LineSegment(this.points[n - 2], this.points[n - 1])
			);
		}
		public void Close()
		{
			this.linesegments.Add(
				new LineSegment(this.points[this.points.Count - 1], this.points[0])
			);
			this.ComputeCenter();
		}
		public void ComputeCenter()
		{
			Vector2d c = new Vector2d();
			foreach (Point2 p in this.points)
			{
				c += p.pos;
			}
			this.center2 = c / this.points.Count;
		}
		public void ComputeRadius()
		{
			Vector2d c = new Vector2d();
			foreach (Point2 p in this.points)
			{
				c += p.pos;
			}
			this.center2 = c / this.points.Count;
			double sum = 0;
			foreach (Point2 p in this.points)
			{
				sum += (p.pos - center2).Length();
			}
			this.radius2 = sum / this.points.Count;
		}

		public static bool PointInPoly(Vector2d p, Shape2D shape)
		{
			return PointInPoly(p, shape.points);
		}
		public static bool PointInPoly(Vector2d p, List<Point2> points)
		{
			bool c = false;
			int n = points.Count;
			for (int i = 0, j = n - 1; i < n; j = i++)
			{
				if (((points[i].pos.y > p.y) != (points[j].pos.y > p.y)) &&
					(p.x < (points[j].pos.x - points[i].pos.x) * (p.y - points[i].pos.y) / (points[j].pos.y - points[i].pos.y) + points[i].pos.x))
					c = !c;
			}
			return c;
		}
		public static bool PointInPoly(Vector2d p, Point2[] points)
		{
			bool c = false;
			int n = points.Length;
			for (int i = 0, j = n - 1; i < n; j = i++)
			{
				if (((points[i].pos.y > p.y) != (points[j].pos.y > p.y)) &&
					(p.x < (points[j].pos.x - points[i].pos.x) * (p.y - points[i].pos.y) / (points[j].pos.y - points[i].pos.y) + points[i].pos.x))
					c = !c;
			}
			return c;
		}
		public static bool PointInPoly(Vector2d p, Vector2d[] shape)
		{
			int n = shape.Length;
			Vector2d o = shape[0];
			for (int i = 1; i < n - 1; ++i)
			{
				if (PointInTriangle(p, o, shape[i], shape[i + 1]))
					return true;
			}
			return false;
		}
		//public static bool PointInPoly(Vector2d p, Shape2D shape)
		//{
		//    int n = shape.points.Count;
		//    Vector2d o = shape.points[0].pos;
		//    for (int i = 1; i < n-1; ++i)
		//    {
		//        if (PointInTriangle(p,o,shape.points[i].pos,shape.points[i+1].pos))
		//            return true;
		//    }
		//    return false;
		//}
		public static bool PointInTriangle(Vector2d p, Vector2d a, Vector2d b, Vector2d c)
		{
			if (SameSide(p, a, b, c) && SameSide(p, b, a, c) && SameSide(p, c, a, b))
				return true;
			return false;
		}
		public static bool SameSide(Vector2d p1, Vector2d p2, Vector2d a, Vector2d b)
		{
			Vector3d A = new Vector3d(a);
			Vector3d B = new Vector3d(b);
			Vector3d P = new Vector3d(p1);
			Vector3d Q = new Vector3d(p2);
			Vector3d E = B - A;
			Vector3d cp1 = E.Cross(P - A);
			Vector3d cp2 = E.Cross(Q - A);
			if (cp1.Dot(cp2) >= 0)
				return true;
			return false;
		}
	}
	public class Quad : Shape2D
	{
		public int vanishingindex = -1;
		public Point3[] points3 = new Point3[4];
		public Vector3d center;
		public Quad()
		{
			this.shaptype = ShapeType2D.QUAD;
		}
		public Quad(Vector2d a, Vector2d b, Vector2d c, Vector2d d)
		{
			this.shaptype = ShapeType2D.QUAD;
			this.points = new List<Point2>();
			this.points.Add(new Point2(a));
			this.points.Add(new Point2(b));
			this.points.Add(new Point2(c));
			this.points.Add(new Point2(d));
		}
		public Quad(Point2 a, Point2 b, Point2 c, Point2 d)
		{
			this.shaptype = ShapeType2D.QUAD;
			this.points = new List<Point2>();
			this.points.Add(a);
			this.points.Add(b);
			this.points.Add(c);
			this.points.Add(d);
		}
		public Quad(Point2[] pts)
		{
			this.shaptype = ShapeType2D.QUAD;
			this.points = new List<Point2>();
			this.points.AddRange(pts);
		}
	}
	public class Polygon : Shape2D, ICloneable
	{
		public Polygon opposite = null;
		public Polygon assignedpolygon = null;
		public Vector2d[] assignedtextures = new Vector2d[4] {
			new Vector2d(0,0),
			new Vector2d(1,0),
			new Vector2d(1,1),
			new Vector2d(0,1)
		};
		public uint[] textid = null;

		public static double propagationpointdensity = 0;
		public CoordinateFrame localframe = null;
		public Matrix3d H = null; // homography matrix
		public Point3 center;
		public int prevattachindex = -1, attachindex = -1;
		public double depth = 0;
		public double sliceratio = 0; // for decomposition of a box
		public int sliceindex = -1; // for decomposition of a box
		public int index = -1;
		public int globalindex = -1;
		public int occludcount = 0;
		public PolyProxy hostproxy = null;
		public List<Polygon> hostpolygons = new List<Polygon>(); // the polygons that contains it
		public List<Polygon> mirrorsymmetries = null;
		public List<Symmetry> symmelements = null;
		public List<int> groupindex = new List<int>();
		public bool visited = false;
		public List<Point3> points3 = new List<Point3>();
		public List<Point3> propagatepoints3 = null; // use for propagation, much denser than the original points
		public List<int> propagateKs = null; // use for propagation
		public Polygon()
		{
			this.shaptype = ShapeType2D.POLYGON;
		}
		public Polygon(List<Point2> pts)
		{
			this.shaptype = ShapeType2D.POLYGON;
			this.points = pts;
			this.linesegments = new List<LineSegment>();
			int n = pts.Count;
			for (int i = 0; i < n; ++i)
			{
				this.linesegments.Add(
					new LineSegment(pts[i], pts[(i + 1) % n])
				);
			}
			this.ComputeCenter();
		}
		public Polygon(Point2[] pts)
		{
			this.shaptype = ShapeType2D.POLYGON;
			this.points = new List<Point2>();
			this.points.AddRange(pts);
			this.linesegments = new List<LineSegment>();
			int n = pts.Length;
			for (int i = 0; i < n; ++i)
			{
				this.linesegments.Add(
					new LineSegment(pts[i], pts[(i + 1) % n])
				);
			}
			this.ComputeCenter();
		}
		public Polygon(Point3[] pts)
		{
			this.shaptype = ShapeType2D.POLYGON;
			this.points3.AddRange(pts);
		}
		public void Compute()
		{
			this.ComputeCenter3();
			this.ComputeNormal();
		}
		public void ComputeCenter3()
		{
			Vector3d cc = new Vector3d();
			foreach (Point3 p in this.points3)
			{
				cc += p.pos;
			}
			cc /= this.points3.Count;
			if (this.center == null)
				this.center = new Point3(cc);
			else
				this.center.pos = cc;
		}
		public void ComputeNormal()
		{
			if (this.points3.Count < 2) return;
			this.normal = (this.points3[0].pos - this.points3[1].pos).Cross(
					this.points3[2].pos - this.points3[1].pos).Normalize();
		}
		public void ComputeLocalFrame()
		{
			if (this.points3 == null) return;
			Vector3d o = this.points3[0].pos;
			Vector3d u = this.points3[1].pos;
			Vector3d v = this.points3[this.points3.Count-1].pos;
			Vector3d z = (v - o).Cross(u - o).Normalize();
			if (double.IsNaN(z.x)) z = new Vector3d(0, 0, 1);
			Vector3d x = (u - o).Normalize();
			Vector3d y = z.Cross(x).Normalize();
			this.localframe = new CoordinateFrame(o, x, y, z);
		}
		public void ComputeLinesegmentsP3()
		{
			int i = 0, n = this.points3.Count;
			foreach (LineSegment line in this.linesegments)
			{
				line.u3 = this.points3[i];
				line.v3 = this.points3[(i + 1) % n];
				i++;
			}
		}
		public void CreatePropagatingPoints()
		{
			this.propagatepoints3 = new List<Point3>();
			this.propagateKs = new List<int>();
			int n = this.points3.Count;
			for (int i = 0; i < n; ++i)
			{
				Vector3d p = this.points3[i].pos, q = this.points3[(i + 1) % n].pos;
				double l = (p - q).Length();
				int K = (int)(l / propagationpointdensity);
				if (K < 2)
				{
		//			Program.OutputText("@CreatePropagatingPoints, K < 1",true);
					K = 2;
				}
		//		if (K > 10) K = 10;
				this.propagateKs.Add(K);
				double step = 1.0 / (K - 1);
				for (int j = 0; j < K; ++j)
				{
					double r = step * j;
					Vector3d pos = q * r + p * (1 - r);
					this.propagatepoints3.Add(new Point3(pos));
				}
			}
		}
		public void UpdatePropagatingPoints()
		{
			int n = this.points3.Count; int index = 0;
			for (int i = 0; i < n; ++i)
			{
				Vector3d p = this.points3[i].pos, q = this.points3[(i + 1) % n].pos;
				double l = (p - q).Length();
				int K = this.propagateKs[i];
				double step = 1.0 / (K - 1);
				for (int j = 0; j < K; ++j)
				{
					double r = step * j;
					Vector3d pos = q * r + p * (1 - r);
					this.propagatepoints3[index++].pos = pos;
				}
			}
		}
		public void PropagationUpdate()
		{
			this.ComputeCenter3();
			this.UpdatePropagatingPoints();
		}
		public object Clone()
		{
			List<Point3> points3 = new List<Point3>();
			foreach (Point3 p in this.points3)
			{
				points3.Add(p.Clone() as Point3);
			}
			Polygon another = new Polygon();
			another.points3 = points3;

			// normals & textures
			another.normal = this.normal;
			another.textureindex = this.textureindex;
			if (this.textid != null)
				another.textid = this.textid;

			// centers, points3 & propagating points
			if (this.center != null) another.center = this.center.Clone() as Point3;
			if (this.points3.Count > 0)
			another.ComputeLinesegmentsP3();
			another.CreatePropagatingPoints();
			return another;
		}
	}
	public class Hull : Shape2D, ICloneable
	{
		public int index = -1;
		public Hull host = null;
		public Hull attachhost = null;
		public Vector2d[] vps = null;
		public int occludcount = 0;
		public double depth = 0;
		public bool[] occluded = null;
		public Hull()
		{
			this.shaptype = ShapeType2D.POLYGON;
		}
		public Hull(List<Point2> pts)
		{
			this.shaptype = ShapeType2D.POLYGON;
			this.points = pts;
			this.linesegments = new List<LineSegment>();
			int n = pts.Count;
			for (int i = 0; i < n; ++i)
			{
				this.linesegments.Add(
					new LineSegment(pts[i], pts[(i + 1) % n])
				);
			}
			this.ComputeCenter();
		}
		public Hull(Point2[] pts)
		{
			this.shaptype = ShapeType2D.POLYGON;
			this.points = new List<Point2>();
			this.points.AddRange(pts);
			this.linesegments = new List<LineSegment>();
			int n = pts.Length;
			for (int i = 0; i < n; ++i)
			{
				this.linesegments.Add(
					new LineSegment(pts[i], pts[(i + 1) % n])
				);
			}
			this.ComputeCenter();
		}
		public object Clone()
		{
			List<Point2> points = new List<Point2>();
			foreach (Point2 pt in this.points)
			{
				points.Add(new Point2(pt.pos));
			}
			Hull another = new Hull(points);
			if (this.vps != null)
				another.vps = this.vps.Clone() as Vector2d[];
			another.depth = this.depth;
			another.attachhost = this.attachhost;
			another.host = this.host;
			another.index = this.index;
			if (this.boxptindice != null)
			{
				another.boxptindice = new List<int>();
				another.boxptindice.AddRange(this.boxptindice);
			}
			int index = 0;
			foreach (LineSegment line in another.linesegments)
			{
				line.vanishingindex = this.linesegments[index++].vanishingindex;
			}
			return another;
		}
	}
	public class PolygonGroup
	{
		public MutualRelation groupstyle = MutualRelation.NONE;
		public List<Polygon> polygons = null;
		public bool visited = false;
		public int index = -1;
		public PolygonGroup(List<Polygon> polys, int id)
		{
			this.polygons = polys;
			this.index = id;
		}
	}
	public class ProxyGroup
	{
		// group for which proxies form one object
		public List<ProxyGroup> reps = null;
		public int groupindex = -1;
		public List<PolyProxy> proxies = null;
		public bool visited = false;
		public ProxyGroup(List<PolyProxy> pxs, int id)
		{
			this.groupindex = id;
			this.proxies = pxs;
		}
	}
	public class PolyProxy : ICloneable, IComparable<PolyProxy>
	{
		// attach index refers to floor plane, ceiling plane and 3 wall planes.
		// 0 - floor, 1 - ceiling, 2 - left wall, 3 - middle wall 4 - right wall
		public List<int> enclosingindex = null; // currently not used any more
		public List<Vector2d> srctxtpos = null;

		// mesh component adapt to it
		public Mesh mesh = null;

		public ProxyType type = ProxyType.BOX;

		public bool isfixed = false;
		public bool deleted = false;
		public bool occuluded = false;
		public PolyProxy jointproxy = null;
		public List<PolyProxy> mirrorsymmetry = new List<PolyProxy>();
		public List<Symmetry> symmelement = new List<Symmetry>();

		public List<Symmetry> symmplanes = null;
		public void ComputeSymmetryPlanes()
		{
			// assume the proxy has reflectional symmetry planes
			// select from the opposite faces.
			this.symmplanes = new List<Symmetry>();
			Polygon top = polygons[0];
			int n = top.points3.Count;
			if (n % 2 != 0) { Console.WriteLine("no mirror symmetry palnes for this proxy!"); return; }
			int k = n / 2;
			for (int i = 0; i < k; ++i)
			{
				int s = i + 2;
				int t = s + k;
				Polygon plg1 = polygons[s], plg2 = polygons[t];
				Symmetry symm = new Symmetry((plg1.center.pos + plg2.center.pos) / 2,
					plg1.normal);
				this.symmplanes.Add(symm);
			}
		}

		public bool isjointhoster = false;
		public LineSegment jointline = null; // for joint object
		public bool excluedforpropagation = false;

		public void Transform(Matrix4d tran)
		{
			for (int i = 0; i < this.points3.Count; ++i)
			{
				Vector3d pos = this.points3[i].pos;
				Vector4d v = new Vector4d(pos, 1.0);
				v = tran * v;
				this.points3[i].pos[0] = v.x;
				this.points3[i].pos[1] = v.y;
				this.points3[i].pos[2] = v.z;
			}
			this.Compute3DInfo();
		}

		//  for MRF selection
		public int maxlevel = 0;
		public List<PolyProxy> kids = null;
		public void AddKid(PolyProxy px)
		{
			if (this.kids == null) this.kids = new List<PolyProxy>();
			this.kids.Add(px);
		}
		public int groupindex = -1; // for inter-obejct propagation
		public int index = -1;
		public int segindex = -1;
		public int refindex = -1;
		public int tmpindex = -1;
		public int level = 0;
		public double fittingcost = 0;

		
		public CoordinateFrame frame = null;

		public List<Polygon> bendingslices = null; // for bending operation
		public List<PolyProxy> slicingboxes = null; // for bending operation

		public double xlen, ylen, zlen;
		public bool IntersectRoom(PolyProxy room)
		{
			double x = room.points3[0].pos.x;
			double y = room.points3[2].pos.y;
			double z = room.points3[4].pos.z;
			foreach (Point3 p in this.points3)
			{
				if (p.pos.x * x < 0 || Math.Abs(p.pos.x) > Math.Abs(x) + 1e-4 ||
					p.pos.y * y < 0 || Math.Abs(p.pos.y) > Math.Abs(y) + 1e-4 ||
					p.pos.z * z < 0 || Math.Abs(p.pos.z) > Math.Abs(z) + 1e-4)
					return true;
			}
			return false;
		}
		public bool Intersect(PolyProxy another)
		{
			foreach (Point3 p in this.points3)
			{
				if (another.Contains(p.pos))
					return true;
			}
			foreach (Point3 p in another.points3)
			{
				if (this.Contains(p.pos))
					return true;
			}
			return false;
		}
		public bool Contains(Vector3d p)
		{
			Vector3d abc = this.frame.GetPointLocalCoord(p);
			double e1 = Math.Abs(abc.x) - this.xlen / 2;
			double e2 = Math.Abs(abc.y) - this.ylen / 2;
			double e3 = Math.Abs(abc.z) - this.zlen / 2;
			return  e1 < -1e-4 && e2 < -1e-4 && e3 < -1e-4;
		}

		public Vector2d[] vps = null;

		public Hull convexhull = null;
		public int attachindex = 0;
		public int prevattachindex = 0;
		public bool visited = false;
		public double volume = -1;
		public double depth = 0;
		public Vector3d scale;
		public Point3 center;
		public Point2 center2;
		public List<int> hexagonindice = null;
		public List<Polygon> txtquads = null;
		public List<LineSegment> enclosingimglines = null;
		public Quad srcenclosingtxtquad = null; // texture mapping, source enclosing quad, for cylindric/sphere objects
		public Quad tarenclosingtxtquad = null; // texture mapping, target enclosing quad
		public List<Polygon> polygons = new List<Polygon>();
		public List<Point2> points = new List<Point2>();
		public List<Point3> points3 = new List<Point3>();
		public List<Point[]> objprofile = null;
		public PolyProxy onproxy = null; // object that it lies on
		public Polygon attachingFace = null;
		public List<PolyProxy> onobjects = null; // objects that lies on it
		public void AddOnObject(PolyProxy px)
		{
			if (this.onobjects == null) this.onobjects = new List<PolyProxy>();
			this.onobjects.Add(px);
		}
		public PolyProxy() { }
		public PolyProxy(Point2[] pts, Point3[] pts3)
		{
			this.Create(pts, pts3);
		}
		public PolyProxy(List<Point2> pts, List<Point3> pts3)
		{
			this.Create(pts, pts3);
		}
		public PolyProxy(Polygon bot, Polygon top)
		{
			List<Point3> pts3 = new List<Point3>();
			pts3.AddRange(bot.points3);
			pts3.AddRange(top.points3);

			this.points3 = pts3;

			int index = 0;
			foreach (Point3 pt in this.points3)
				pt.index = index++;

			this.AddPolygon(bot);
			this.AddPolygon(top);

			int k = bot.points3.Count;
			for (int i = 0; i < k; ++i)
			{
				int j = i, l = (i + 1) % k;
				if (i > 1) { j = (i + 1) % k; l = i; }

				Point3[] points3 = new Point3[4] {
			        bot.points3[j],bot.points3[l], top.points3[l],top.points3[j],
			    };

				Polygon plg = new Polygon(points3);
				this.AddPolygon(plg);
			}

			this.type = ProxyType.POLY;
			this.Compute3DInfo();

		}
		protected void Create(Point2[] pts, Point3[] pts3)
		{
			this.points.AddRange(pts);
			this.points3.AddRange(pts3);
			this.Make();
			this.AssignIndex();
		}
		protected void Create(List<Point2> pts, List<Point3> pts3)
		{
			this.points = pts;
			this.points3 = pts3;
			this.Make();
			this.AssignIndex();
		}
		public bool MakeBox()
		{
			int n = this.points.Count;
			if (n != 8) return false;
			this.Make();
			this.FindVanishingPoints();
			this.type = ProxyType.BOX;
			this.Compute3DInfo();
			this.EstablishOpposite();
			this.EstablishHexagonIndices();
			return true;
		}
		public bool MakePlanarQuad()
		{
			int n = this.points.Count;
			if (n != 4) return false;
			this.polygons.Clear();
			Polygon poly = new Polygon(this.points);
			poly.points3 = this.points3;
			this.AddPolygon(poly);
			this.type = ProxyType.QUAD;
			this.Compute3DInfo();
			this.FindVanishingPoints();
			return true;
		}
		public void Make()
		{
			int n = this.points.Count;
			int k = n / 2;
			this.polygons.Clear();
			Point2[] pts2 = new Point2[k];
			for (int i = 0; i < k; ++i) {
				pts2[i] = this.points[i];
			}
			Point3[] pts3 = new Point3[k];
			for (int i = 0; i < k; ++i){
				pts3[i] = this.points3[i];
			}
			Polygon bot = new Polygon(pts2);
			bot.points3.AddRange(pts3);

			for (int i = 0; i < k; ++i) {
				pts2[i] = this.points[k+i];
			}
			for (int i = 0; i < k; ++i) {
				pts3[i] = this.points3[k+i];
			}
			Polygon top = new Polygon(pts2);
			top.points3.AddRange(pts3);
			this.AddPolygon(bot);
			this.AddPolygon(top);

			for (int i = 0; i < k; ++i)
			{
				int j = i, l = (i + 1) % k;
				if (i > 1) { j = (i + 1) % k; l = i; }
				Point2[] points2 = new Point2[4] {
			        bot.points[j],bot.points[l], top.points[l],top.points[j],
			    };
				Point3[] points3 = new Point3[4] {
			        bot.points3[j],bot.points3[l], top.points3[l],top.points3[j],
			    };
				List<Point2> tmplist = new List<Point2>();
				tmplist.AddRange(points2);
				Polygon plg = new Polygon(tmplist);
				plg.points3.AddRange(points3);
				this.AddPolygon(plg);
			}

			this.type = ProxyType.POLY;
			this.Compute3DInfo();
		}
		public void AssignIndex()
		{
			int index = 0;
			foreach (Point2 p in this.points)
			{
				p.index = index++;
			}
		}
		public void ComputeVolume()
		{
			if (this.polygons.Count < 2)  
				this.volume = 0;
			Polygon bot = this.polygons[0];
			Polygon top = this.polygons[1];
			double h = (top.points3[0].pos - bot.points3[0].pos).Length();
			int n = bot.points3.Count;
			Vector3d o = bot.points3[0].pos;
			double sum = 0;
			for (int i = 1; i < n - 1; ++i) {
				Vector3d u = bot.points3[i].pos, v = bot.points3[i+1].pos;
				sum += ((u-o).Cross(v-o)).Length() / 2.0;
			}
			this.volume = sum*h;
		}
		public void ComputeCenter()
		{
			Vector3d c = new Vector3d();
			foreach (Point3 pt in this.points3)
			{
				c += pt.pos;
			}
			if (this.center == null)
				this.center = new Point3(c / this.points3.Count);
			else
				this.center.pos = c / this.points3.Count;
		}
		public void AddPolygon(Polygon plg)
		{
			this.polygons.Add(plg);
			plg.hostproxy = this;
		}
		public void Compute3DInfo()
		{
			this.ComputeCenter();
			Vector2d c2 = new Vector2d();
			foreach (Point2 pt in this.points)
			{
				c2 += pt.pos;
			}
			if (this.center2 != null)
				this.center2.pos = c2 / this.points.Count;
			else
				this.center2 = new Point2(c2 / this.points.Count);
			foreach (Polygon poly in this.polygons)
			{
				poly.ComputeCenter3();
				poly.ComputeNormal();
				if (poly.normal.Dot(poly.center.pos - this.center.pos) < 0)
					poly.normal = new Vector3d() - poly.normal;
				poly.ComputeLocalFrame();
				poly.ComputeLinesegmentsP3();
			}
			if (this.polygons.Count == 6) // a box
			{
				Vector3d x = (this.polygons[2].center.pos - this.polygons[4].center.pos);
				Vector3d y = (this.polygons[3].center.pos - this.polygons[5].center.pos);
				Vector3d z = (this.polygons[1].center.pos - this.polygons[0].center.pos);
				this.xlen = x.Length();
				this.ylen = y.Length();
				this.zlen = z.Length();
				x = x.Normalize();
				z = z.Normalize();
				y = y.Normalize();// z.Cross(x).Normalize(); // make sure it is right-hand convention

				this.scale = new Vector3d(xlen, ylen, zlen);

				this.frame = new CoordinateFrame(this.center.pos, x, y, z);
			}

			if (double.IsNaN(this.center.pos.x) || double.IsNaN(this.scale.x)) throw new Exception();

			this.ComputePointNormals();
		}
		private void ComputePointNormals()
		{
			foreach (Point3 p3 in this.points3)
			{
				Vector3d normal = new Vector3d();
				foreach (Polygon plg in this.polygons)
				{
					if (plg.points3.Contains(p3))
					{
						normal += plg.normal;
					}
				}
				p3.normal = normal.Normalize();
			}
		}
		private void FindVanishingPoints()
		{
			this.vps = new Vector2d[3];
			if (this.points.Count == 4) // planar
			{
				Vector3d x1 = new Vector3d(this.points[1].pos, 1).Cross(new Vector3d(this.points[0].pos, 1));
				Vector3d x2 = new Vector3d(this.points[3].pos, 1).Cross(new Vector3d(this.points[2].pos, 1));
				this.vps[0] = x1.Cross(x2).HomogenousNormalize().ToVector2d();

				Vector3d y1 = new Vector3d(this.points[0].pos, 1).Cross(new Vector3d(this.points[3].pos, 1));
				Vector3d y2 = new Vector3d(this.points[1].pos, 1).Cross(new Vector3d(this.points[2].pos, 1));
				this.vps[1] = y1.Cross(y2).HomogenousNormalize().ToVector2d();

				this.vps[2] = new Vector2d(200, 1e6);
			}

			if (this.points.Count == 8)
			{
				Vector3d x1 = new Vector3d(this.points[1].pos, 1).Cross(new Vector3d(this.points[0].pos, 1));
				Vector3d x2 = new Vector3d(this.points[5].pos, 1).Cross(new Vector3d(this.points[4].pos, 1));
				this.vps[0] = x1.Cross(x2).HomogenousNormalize().ToVector2d();

				Vector3d y1 = new Vector3d(this.points[2].pos, 1).Cross(new Vector3d(this.points[1].pos, 1));
				Vector3d y2 = new Vector3d(this.points[6].pos, 1).Cross(new Vector3d(this.points[5].pos, 1));
				this.vps[1] = y1.Cross(y2).HomogenousNormalize().ToVector2d();

				Vector3d z1 = new Vector3d(this.points[0].pos, 1).Cross(new Vector3d(this.points[4].pos, 1));
				Vector3d z2 = new Vector3d(this.points[1].pos, 1).Cross(new Vector3d(this.points[5].pos, 1));
				this.vps[2] = z1.Cross(z2).HomogenousNormalize().ToVector2d();
			}
		}
		private void EstablishOpposite()
		{
			if (this.type != ProxyType.BOX)
			{
				return;
			}
			this.polygons[0].opposite = this.polygons[1];
			this.polygons[1].opposite = this.polygons[0];
			this.polygons[2].opposite = this.polygons[3];
			this.polygons[4].opposite = this.polygons[2];
			this.polygons[3].opposite = this.polygons[5];
			this.polygons[5].opposite = this.polygons[3];
		}
		private void EstablishHexagonIndices()
		{
			this.hexagonindice = new List<int>();
			this.hexagonindice.Add(4);
			this.hexagonindice.Add(0);
			this.hexagonindice.Add(1);
			this.hexagonindice.Add(2);
			this.hexagonindice.Add(6);
			this.hexagonindice.Add(7);
		}
		public object Clone_old()
		{
			PolyProxy another = new PolyProxy();
			Polygon bot = this.polygons[0].Clone() as Polygon;
			if (this.polygons.Count == 1)
			{
				another.AddPolygon(bot);
				another.points = bot.points;
				another.points3 = bot.points3;
			}
			else
			{
				Polygon top = this.polygons[1].Clone() as Polygon;
				another = MakePolyProxy(bot, top);
			}
			int j = 0;
			foreach (Polygon plg in another.polygons)
			{
				plg.textid = this.polygons[j++].textid;
			}
			another.prevattachindex = another.attachindex
					= this.attachindex;
			another.center = new Point3(this.center.pos);
			another.txtquads = new List<Polygon>();
			another.index = this.index;
			another.type = this.type;
			foreach (Polygon plg in another.polygons)
			{
				plg.CreatePropagatingPoints();
			}
			if (this.txtquads != null)
			foreach (Polygon plg in this.txtquads)
			{
				int index = this.polygons.IndexOf(plg);
				Polygon plg2 = another.polygons[index];
				plg2.textureindex = plg.textureindex;
				another.txtquads.Add(plg2);
			}
			another.Compute3DInfo();
			return another;
		}
		public object Clone()
		{
			PolyProxy another = new PolyProxy();
			List<Point2> pts2 = new List<Point2>();
			List<Point3> pts3 = new List<Point3>();
			foreach (Point2 pt in this.points)
			{
				pts2.Add(pt.Clone() as Point2);
			}
			foreach (Point3 pt in this.points3)
			{
				pts3.Add(pt.Clone() as Point3);
			}
			another.points = pts2;
			another.points3 = pts3;

			List<Polygon> plgs = new List<Polygon>();
			foreach (Polygon plg in this.polygons)
			{
				plgs.Add(plg.Clone() as Polygon);
			}

			// polygons share points
			int j = 0;
			foreach (Polygon plg in this.polygons)
			{
				Polygon ply = plgs[j];
				for (int i = 0; i < plg.points3.Count; ++i)
				{
					int index = plg.points3[i].index;
					ply.points3[i] = pts3[index];
				}
				++j;
			}
			another.polygons = plgs;
			foreach (Polygon ply in plgs)
				ply.hostproxy = another;

			// 0--
			another.prevattachindex = another.attachindex
					= this.attachindex;
			another.center = new Point3(this.center.pos);
			another.index = this.index;
			another.type = this.type;
			another.txtquads = new List<Polygon>();
			if (this.txtquads != null) // copy texture quads
				foreach (Polygon plg in this.txtquads)
				{
					int index = this.polygons.IndexOf(plg);
					Polygon plg2 = another.polygons[index];
					plg2.textureindex = plg.textureindex;
					another.txtquads.Add(plg2);
				}
			another.Compute3DInfo();
			return another;
		}
		public static PolyProxy MakePolyProxy(Polygon bot, Polygon top)
		{
			if (top.points.Count != bot.points.Count) return null;
			int n = bot.points.Count;
			List<Point2> pts2 = new List<Point2>();
			pts2.AddRange(bot.points);
			pts2.AddRange(top.points);
			List<Point3> pts3 = new List<Point3>();
			pts3.AddRange(bot.points3);
			pts3.AddRange(top.points3);
			int index = 0;
			foreach (Point2 pt in pts2)
			{
				pt.index = index++;
			}
			PolyProxy proxy = new PolyProxy(pts2, pts3);
			proxy.AddPolygon(bot);
			proxy.AddPolygon(top);
			for (int i = 0; i < n; ++i)
			{
				Point2[] points2 = new Point2[4] {
					bot.points[i],bot.points[(i+1)%n], top.points[(i+1)%n],top.points[i],
				};
				Point3[] points3 = new Point3[4] {
					bot.points3[i],bot.points3[(i+1)%n], top.points3[(i+1)%n],top.points3[i],
				};
				List<Point2> tmplist = new List<Point2>();
				tmplist.AddRange(points2);
				Polygon plg = new Polygon(tmplist);
				plg.points3.AddRange(points3);
				proxy.AddPolygon(plg);
			}
			proxy.Compute3DInfo();
			return proxy;
		}
		public static void OptimizeBox(PolyProxy box, Vector3d c, double xlen, double ylen, double zlen)
		{
			Vector3d xdir = box.polygons[2].center.pos - box.polygons[4].center.pos;
			Vector3d ydir = box.polygons[3].center.pos - box.polygons[5].center.pos;
			Vector3d zdir = box.polygons[1].center.pos - box.polygons[0].center.pos;
			int sign = box.polygons[2].normal.Dot(box.polygons[4].normal) < 0 ? -1 : 1;
			Vector3d xaxis = (box.polygons[2].normal + box.polygons[4].normal * sign).Normalize();
			if (xaxis.Dot(xdir) < 0) xaxis = new Vector3d() - xaxis;
			sign = box.polygons[3].normal.Dot(box.polygons[5].normal) < 0 ? -1 : 1;
			Vector3d yaxis = (box.polygons[3].normal + box.polygons[5].normal * sign).Normalize();
			if (yaxis.Dot(ydir) < 0) yaxis = new Vector3d() - yaxis;
			sign = box.polygons[1].normal.Dot(box.polygons[0].normal) < 0 ? -1 : 1;
			Vector3d zaxis = (box.polygons[1].normal + box.polygons[0].normal * sign).Normalize();
			if (zaxis.Dot(zdir) < 0) zaxis = new Vector3d() - zaxis;
			xlen /= 2;
			ylen /= 2;
			zlen /= 2;
			box.points3[0].pos = c + new Vector3d(xlen, -ylen, -zlen);
			box.points3[1].pos = c + new Vector3d(xlen, ylen, -zlen);
			box.points3[2].pos = c + new Vector3d(-xlen, ylen, -zlen);
			box.points3[3].pos = c + new Vector3d(-xlen, -ylen, -zlen);
			box.points3[4].pos = c + new Vector3d(xlen, -ylen, zlen);
			box.points3[5].pos = c + new Vector3d(xlen, ylen, zlen);
			box.points3[6].pos = c + new Vector3d(-xlen, ylen, zlen);
			box.points3[7].pos = c + new Vector3d(-xlen, -ylen, zlen);
		}

		#region IComparable<PolyProxy> Members
		public int CompareTo(PolyProxy other)
		{
			return Math.Sign(other.depth - this.depth);
		}
		#endregion

	}

	public class Symmetry
	{
		public Vector3d center;
		public Vector3d axis;
		public Symmetry(Vector3d c, Vector3d ax)
		{
			this.center = c;
			this.axis = ax;
		}
	}

	public class CuboidMeshComponent
	{
		public int index = -1;
		public string name = "Unkown";
		public Mesh mesh = null;
		public ComponentGraphNode node = null;
		public PolyProxy boundingBox = null;
		public double groundvalue = 0;
		public CuboidMeshComponent(ComponentGraphNode node, Mesh m)
		{
			this.node = node;
			this.mesh = m;
			this.node.cuboidMeshComponent = this;

			this.FindBoundingBox();

			Matrix4d T = Matrix4d.TranslationMatrix(node.pos);
			this.mesh.Transform(T);
			this.boundingBox.Transform(T);

			if (node.isgroundcontact)
				this.groundvalue = this.boundingBox.polygons[5].center.pos.y;
		}
		public CuboidMeshComponent(ComponentGraphNode node, string meshfile)
		{
			StreamReader sr = new StreamReader(meshfile);
			Mesh m = new Mesh(sr, false);
			sr.Close();
			this.mesh = m;
			this.node = node;
			this.node.cuboidMeshComponent = this;
			this.FindBoundingBox();

			Matrix4d T = Matrix4d.TranslationMatrix(node.pos);
			this.mesh.Transform(T);
			this.boundingBox.Transform(T);

			if (node.isgroundcontact)
				this.groundvalue = this.boundingBox.polygons[5].center.pos.y;
		}
		public void FindBoundingBox()
		{
			if (this.mesh == null) return;

			int vertexCount = this.mesh.VertexCount;

			Vector3d maxCoord = new Vector3d(double.MinValue, double.MinValue, double.MinValue);
			Vector3d minCoord = new Vector3d(double.MaxValue, double.MaxValue, double.MaxValue);
			for (int i = 0, j = 0; i < vertexCount; i++, j += 3)
			{
				Vector3d v = new Vector3d(mesh.VertexPos, j);
				maxCoord = Vector3d.Max(maxCoord, v);
				minCoord = Vector3d.Min(minCoord, v);
			}

			// the other six points
			Vector3d u1 = new Vector3d(minCoord.x, maxCoord.y, minCoord.z);
			Vector3d u2 = new Vector3d(maxCoord.x, minCoord.y, minCoord.z);
			Vector3d u3 = new Vector3d(maxCoord.x, maxCoord.y, minCoord.z);
			Vector3d v1 = new Vector3d(minCoord.x, maxCoord.y, maxCoord.z);
			Vector3d v2 = new Vector3d(maxCoord.x, minCoord.y, maxCoord.z);
			Vector3d v3 = new Vector3d(minCoord.x, minCoord.y, maxCoord.z);

			Point3[] botpts = new Point3[4] {
				new Point3(minCoord),
				new Point3(u1),
				new Point3(u3),
				new Point3(u2),
			};

			Point3[] toppts = new Point3[4] {
				new Point3(v3),
				new Point3(v1),
				new Point3(maxCoord),
				new Point3(v2),
			};

			Polygon top = new Polygon(toppts);
			Polygon bot = new Polygon(botpts);

			this.boundingBox = new PolyProxy(bot, top);
			this.boundingBox.Compute3DInfo();

		}
		public void Transform(Matrix4d T)
		{
			this.mesh.Transform(T);
			this.boundingBox.Transform(T);
			this.node.TransformSlots(T);
		}

		public Vector3d GetScale()
		{
			if (this.boundingBox == null) 
				return new Vector3d();
			Vector3d diag = this.boundingBox.points3[6].pos - this.boundingBox.points3[0].pos;
			return new Vector3d(Math.Abs(diag.x), Math.Abs(diag.y), Math.Abs(diag.z));
		}
		public void ResetSlots()
		{
			foreach (Slot s in node.contactSlots)
			{
				s.Reset();
			}
		}
	}

	public class MeshComponent
	{
		public int index = -1;
		public string name = "Unkown";
		public int vertexCount;
		public double[] vertexPos = null;
		public int[] faceIndex = null;
		public int[] vertexIndex = null;
		public PolyProxy boundingBox = null;
		public MeshComponent(List<int> vindex, List<double> vpos, List<int> findex)
		{
			this.vertexIndex = vindex.ToArray();
			this.vertexPos = vpos.ToArray();
			this.faceIndex = findex.ToArray();
		}
		public MeshComponent(int[] vindex, double[] vpos, int[] findex)
		{
			this.vertexIndex = vindex;
			this.vertexPos = vpos;
			this.faceIndex = findex;
		}
		public void FindBoundingBox()
		{
			if (this.vertexPos == null) return;

			this.vertexCount = this.vertexPos.Length / 3;

			Vector3d maxCoord = new Vector3d(double.MinValue, double.MinValue, double.MinValue);
			Vector3d minCoord = new Vector3d(double.MaxValue, double.MaxValue, double.MaxValue);
			for (int i = 0, j = 0; i < vertexCount; i++, j += 3)
			{
				Vector3d v = new Vector3d(vertexPos, j);
				maxCoord = Vector3d.Max(maxCoord, v);
				minCoord = Vector3d.Min(minCoord, v);
			}

			// the other six points
			Vector3d u1 = new Vector3d(minCoord.x, maxCoord.y, minCoord.z);
			Vector3d u2 = new Vector3d(maxCoord.x, minCoord.y, minCoord.z);
			Vector3d u3 = new Vector3d(maxCoord.x, maxCoord.y, minCoord.z);
			Vector3d v1 = new Vector3d(minCoord.x, maxCoord.y, maxCoord.z);
			Vector3d v2 = new Vector3d(maxCoord.x, minCoord.y, maxCoord.z);
			Vector3d v3 = new Vector3d(minCoord.x, minCoord.y, maxCoord.z);

			Point3[] botpts = new Point3[4] {
				new Point3(minCoord),
				new Point3(u1),
				new Point3(u3),
				new Point3(u2),
			};

			Point3[] toppts = new Point3[4] {
				new Point3(v3),
				new Point3(v1),
				new Point3(maxCoord),
				new Point3(v2),
			};

			Polygon top = new Polygon(toppts);
			Polygon bot = new Polygon(botpts);

			this.boundingBox = new PolyProxy(bot, top);
			this.boundingBox.Compute3DInfo();

		}
		
	}

	public class Slot
	{
		public ComponentGraphNode start = null, end = null;
		public List<Vector3d> contactPoints = null;
		public List<Vector3d> oldContactPoints = null;
		public Vector3d center, oldcenter;
		public int index = -1;
		public Slot symmetry = null;
		public Slot clonned = null;
		public bool modified = false;
		public Functionality symmetrySupport = Functionality.none;
		public bool issuperslot = false;
		public bool used = false;
		public Slot(ComponentGraphNode node1, ComponentGraphNode node2, List<Vector3d> spoints, int index)
		{
			this.start = node1;
			this.end = node2;
			this.contactPoints = spoints;
			this.index = index;
			this.ComputeCenter();
			this.oldContactPoints = new List<Vector3d>();
			this.oldContactPoints.AddRange(spoints);
			this.oldcenter = center;
		}
		public void ComputeCenter()
		{
			Vector3d maxCoord = new Vector3d(double.MinValue, double.MinValue, double.MinValue);
			Vector3d minCoord = new Vector3d(double.MaxValue, double.MaxValue, double.MaxValue);
			foreach (Vector3d v in this.contactPoints)
			{
				maxCoord = Vector3d.Max(maxCoord, v);
				minCoord = Vector3d.Min(minCoord, v);
			}

			Vector3d c = (minCoord + maxCoord)/2;

			this.center = c;
		}
		public void CopyCenter()
		{
			this.oldcenter = this.center;
		}
		public void Transform(Matrix4d tran)
		{
			for (int i = 0; i < this.contactPoints.Count; ++i)
			{
				Vector3d pos = this.contactPoints[i];
				Vector4d v = new Vector4d(pos, 1.0);
				v = tran * v;
				this.contactPoints[i] = v.XYZ();
			}
			this.ComputeCenter();
		}
		public void Reset()
		{
			this.contactPoints = new List<Vector3d>();
			this.contactPoints.AddRange(this.oldContactPoints);
			this.center = this.oldcenter;
			this.used = false;
		}
	}
	public enum ComponentEdgeType { Symmetry, Underneath, Upon, Contact };
	public class ComponentGraphNode
	{
		public bool isgroundcontact = false;
		public bool tag = false;
		public string meshfile = "";
		public string name = "";
		public int componentIndex = -1;
		public Vector3d pos, scale;
		public List<ComponentGraphNode> kids = null; // if not null means a super node, for replacing
		public ComponentGraphNode symmetry = null;
		public ComponentGraphNode parent = null;
		public ComponentGraphNode replace = null;
		public CuboidMeshComponent cuboidMeshComponent = null;
		public List<Slot> contactSlots = new List<Slot>();
		public List<ComponentGraphEdge> edges = new List<ComponentGraphEdge>();
		public List<ComponentGraphNode> compatibleNodes = new List<ComponentGraphNode>();
		public List<Functionality> functionalities = null;
		public ComponentGraphNode(int index)
		{
			this.componentIndex = index;
		}
		public ComponentGraphNode(ComponentGraphNode node1, ComponentGraphNode node2)
		{
			this.kids = new List<ComponentGraphNode>();
			this.kids.Add(node1);
			this.kids.Add(node2);

			this.isgroundcontact = node1.isgroundcontact;
			this.name = node1.name;

			CuboidMeshComponent cmc1 = new CuboidMeshComponent(node1, node1.meshfile);
			CuboidMeshComponent cmc2 = new CuboidMeshComponent(node2, node2.meshfile);

			List<CuboidMeshComponent> cmcs = new List<CuboidMeshComponent>();
			cmcs.Add(cmc1);
			cmcs.Add(cmc2);

			Vector3d[] vec = MeshView.GetBoundingBoxCenterAndScale(cmcs);
			this.pos = vec[0];
			this.scale = vec[1];
		}
		public void getSymmetry()
		{
			foreach (ComponentGraphEdge e in this.edges)
			{
				if (e.type == ComponentEdgeType.Symmetry)
				{
					symmetry = e.start == this ? e.end : e.start;
				}
			}
		}
		public void AddFunctionality(Functionality f)
		{
			if (!this.functionalities.Contains(f))
			{
				this.functionalities.Add(f);
			}
		}
		public void TransformSlots(Matrix4d T)
		{
			foreach (Slot s in this.contactSlots)
			{
			    s.Transform(T);
			}
		}
	}
	public class ComponentGraphEdge
	{
		public ComponentEdgeType type = ComponentEdgeType.Contact;
		public ComponentGraphNode start = null, end = null;
		public bool directional = false;
		public ComponentGraphEdge(ComponentGraphNode s, ComponentGraphNode t, bool direct)
		{
			this.start = s;
			this.end = t;
			this.directional = direct;
		}
	}
	public class ComponentGraph
	{
		public string classname = "";
		public string meshfile = "";
		public List<ComponentGraphNode> supernodes = null;
		public List<ComponentGraphNode> nodes = null;
		public List<ComponentGraphEdge> edges = null;
		public List<SymmetryTriplet> symmtryTriplets = null;
		public ComponentGraph(List<ComponentGraphNode> _nodes, List<ComponentGraphEdge> _edges)
		{
			this.nodes = _nodes;
			this.edges = _edges;
		}
		public ComponentGraph()
		{
			this.nodes = new List<ComponentGraphNode>();
			this.edges = new List<ComponentGraphEdge>();
		}
		public void GetSymmetryTriplets()
		{
			if (this.nodes == null) return;

			foreach (ComponentGraphNode node in this.nodes)
			{
				node.tag = false;
				if (node.symmetry == null)
					node.getSymmetry();
			}
			this.symmtryTriplets = new List<SymmetryTriplet>();
			foreach (ComponentGraphNode node in this.nodes)
			{
				if (node.tag) continue;
				if (node.symmetry != null)
				{
					foreach (ComponentGraphEdge e in node.edges)
					{
						ComponentGraphNode other = e.start == node ? e.end : e.start;
						// check validity
						bool valid = false;
						foreach (ComponentGraphEdge ee in other.edges)
						{
							ComponentGraphNode nd = ee.start == other ? ee.end : ee.start;
							if (nd == node.symmetry)
							{
								valid = true;
								break;
							}
						}
						if (!valid) continue;

						switch (e.type)
						{
							case ComponentEdgeType.Contact:
								{
									SymmetryTriplet tri = new SymmetryTriplet(node, node.symmetry, other, Functionality.contacting);
									this.symmtryTriplets.Add(tri);
								}
								break;
							case ComponentEdgeType.Underneath:
								{
									SymmetryTriplet tri = new SymmetryTriplet(node, node.symmetry, other, Functionality.supportive);
									this.symmtryTriplets.Add(tri);
								}
								break;
							case ComponentEdgeType.Upon:
								{
									SymmetryTriplet tri = new SymmetryTriplet(node, node.symmetry, other, Functionality.anti_supportive);
									this.symmtryTriplets.Add(tri);
								}
								break;
						}
					}
					node.tag = node.symmetry.tag = true;
				}
			}
			foreach (ComponentGraphNode node in this.nodes) node.tag = false;

			this.GetExtraSymmetryTriplets();

		}
		public void GetExtraSymmetryTriplets()
		{
			if (this.symmtryTriplets == null) return;

			this.supernodes = new List<ComponentGraphNode>();

			int n = this.symmtryTriplets.Count;
			for (int i = 0; i < n-1; ++i)
			{
				SymmetryTriplet tri = this.symmtryTriplets[i];
				for (int j = i + 1; j < n; ++j)
				{
					SymmetryTriplet tri2 = this.symmtryTriplets[j];
					this.TryMergeSymmetryTriplets(tri, tri2);
				}
			}

			foreach (SymmetryTriplet tri in this.symmtryTriplets)
				tri.tag = false;
		}
		private void TryMergeSymmetryTriplets(SymmetryTriplet t1, SymmetryTriplet t2)
		{
			if (t1.nodes[2] != t2.nodes[2] || t1.functionality != t2.functionality) return;

			if (t1.tag || t2.tag) return;
			t1.tag = t2.tag = true;

			// i think here node1 and node3 are already index-aligned, so (node1 - node3) (node2 - node4)
			ComponentGraphNode node1 = t1.nodes[0], node2 = t1.nodes[1];
			ComponentGraphNode node3 = t2.nodes[0], node4 = t2.nodes[1];
			ComponentGraphNode cnode = t2.nodes[2];

			ComponentGraphNode super_node1 = null;
			ComponentGraphNode super_node2 = null;

			foreach (ComponentGraphNode snode in this.supernodes)
			{
				if (snode.kids.Contains(node1) && snode.kids.Contains(node3))
					super_node1 = snode;
				else if (snode.kids.Contains(node2) && snode.kids.Contains(node4))
					super_node2 = snode;
			}

			if (super_node1 == null) super_node1 = new ComponentGraphNode(node1, node3);
			if (super_node2 == null) super_node2 = new ComponentGraphNode(node2, node4);

			Functionality func = t1.functionality;

			// merge the slots
			foreach (Slot s in node1.contactSlots)
			{
				if (s.symmetrySupport != func) continue;
				Slot t = null;
				foreach (Slot s2 in node3.contactSlots)
				{
					if (s.symmetrySupport == s2.symmetrySupport)
					{
						t = s2;
						break;
					}
				}
				List<Vector3d> pts = new List<Vector3d>();
				pts.AddRange(s.contactPoints);
				pts.AddRange(t.contactPoints);
				Slot q = new Slot(super_node1, cnode, pts, 0);
				q.symmetrySupport = func;
				q.issuperslot = true;
				q.ComputeCenter();
				super_node1.contactSlots.Add(q);
				cnode.contactSlots.Add(q);
			}
			foreach (Slot s in node2.contactSlots)
			{
				if (s.symmetrySupport != func) continue;
				Slot t = null;
				foreach (Slot s2 in node4.contactSlots)
				{
					if (s.symmetrySupport == s2.symmetrySupport)
					{
						t = s2;
						break;
					}
				}
				List<Vector3d> pts = new List<Vector3d>();
				pts.AddRange(s.contactPoints);
				pts.AddRange(t.contactPoints);
				Slot q = new Slot(super_node2, cnode, pts, 0);
				q.symmetrySupport = func;
				q.issuperslot = true;
				q.ComputeCenter();
				super_node2.contactSlots.Add(q);
				cnode.contactSlots.Add(q);
			}

			// graph topology
			ComponentGraphEdge e1 = new ComponentGraphEdge(super_node1, cnode, false);
			ComponentGraphEdge e2 = new ComponentGraphEdge(super_node2, cnode, false);
			super_node1.edges.Add(e1);
			super_node2.edges.Add(e2);
			super_node1.symmetry = super_node2;
			super_node2.symmetry = super_node1;
			cnode.edges.Add(e1);
			cnode.edges.Add(e2);

			if (!this.supernodes.Contains(super_node1))
				this.supernodes.Add(super_node1);
			if (!this.supernodes.Contains(super_node2))
				this.supernodes.Add(super_node2);

			SymmetryTriplet tri = new SymmetryTriplet(super_node1, super_node2, cnode, t1.functionality);
			tri.isgroundtouching = t1.isgroundtouching;
			tri.topple = false;

			tri.kidsTriplets = new List<SymmetryTriplet>();
			tri.kidsTriplets.Add(t1);
			tri.kidsTriplets.Add(t2);

			for (int i = 0; i < 3; ++i)
			{
				ComponentGraphNode node = tri.nodes[i];
				if (node.functionalities == null) node.functionalities = new List<Functionality>();
				node.functionalities.Add(tri.functionality);
			}

	//		Program.OutputText("adding extra sFarr-s " + t1.functionality + " " + cnode.name, true);
			this.symmtryTriplets.Insert(0,tri);
		}
		public void ComputeNodeFunctionalities()
		{
			// find clusters, and further classify node's functionality
			foreach (SymmetryTriplet tri in this.symmtryTriplets)
			{
				foreach (ComponentGraphNode node in tri.nodes)
				{
					node.functionalities = new List<Functionality>();
					node.AddFunctionality(tri.functionality);
				}
			}
			int k = this.symmtryTriplets.Count;
			for (int i = 0; i < k-1; ++i)
			{
				SymmetryTriplet tri = this.symmtryTriplets[i];
				for (int j = i + 1; j < k; ++j)
				{
					SymmetryTriplet tri2 = this.symmtryTriplets[j];
					
					Functionality f1 = tri.functionality, f2 = tri2.functionality;
					if (f1 == f2 || (
						 (f1 == Functionality.anti_supportive && f2 == Functionality.contacting) || 
						 (f1 == Functionality.contacting && f2 == Functionality.anti_supportive)
						 )
						)
						continue;

					var intersect = tri.nodes.Intersect(tri2.nodes);
					foreach (ComponentGraphNode node in intersect)
					{
						if (!node.functionalities.Contains(f1))
						{
							node.AddFunctionality(f1);
						}
						if (!node.functionalities.Contains(f2))
						{
							node.AddFunctionality(f2);
						}
					}
				}
			}
		}
	}
	public enum Functionality { supportive, contacting, anti_supportive, none }
	public class SymmetryTriplet
	{
		public bool isgroundtouching = false;
		public bool topple = false;
		public bool tag = false;
		public List<ComponentGraphNode> nodes = null;
		public Functionality functionality = Functionality.contacting;
		public List<SymmetryTriplet> compatibleTriplets = new List<SymmetryTriplet>();
		public List<SymmetryTriplet> kidsTriplets = null;
		public SymmetryTriplet(ComponentGraphNode _node1, ComponentGraphNode _node2, ComponentGraphNode _cnode, Functionality t)
		{
			// arrange the nodes, 1 - left, 2 - right, 3 - contact
			Vector3d nor = (_node2.pos - _node1.pos).Normalize();
			Vector3d ctr = (_node1.pos + _node2.pos)/2;
			Vector3d x_axis = new Vector3d(1,0,0);
			Vector3d z_axis = new Vector3d(0,0,1);
			if (Math.Abs(nor.Dot(x_axis)) > 0.5) // coincide with x axis
			{
				if ((_node1.pos - ctr).Dot(x_axis) > 0)
				{
					ComponentGraphNode tmp = _node1;
					_node1 = _node2;
					_node2 = tmp;
				}
			}
			else
			{
				if ((_node1.pos - ctr).Dot(z_axis) > 0)
				{
					ComponentGraphNode tmp = _node1;
					_node1 = _node2;
					_node2 = tmp;
				}
			}


			this.nodes = new List<ComponentGraphNode>();
			this.nodes.Add(_node1);
			this.nodes.Add(_node2);
			this.nodes.Add(_cnode);
			this.functionality = t;
			this.isgroundtouching = _node1.isgroundcontact || _cnode.isgroundcontact;

			// tag contact slots
			foreach (Slot s in _node1.contactSlots)
			{
				ComponentGraphNode other = s.start == _node1 ? s.end : s.start;
				if (other == _cnode)
				{
					s.symmetrySupport = t;
					if (s.clonned != null)
						s.clonned.symmetrySupport = t;
				}
			}
			foreach (Slot s in _node2.contactSlots)
			{
				ComponentGraphNode other = s.start == _node2 ? s.end : s.start;
				if (other == _cnode)
				{
					s.symmetrySupport = t;
					if (s.clonned != null)
						s.clonned.symmetrySupport = t;
				}
			}
		}
	}
}
