Eric Is Stupid

Eric Is Stupid

public class FlapControl
{
		bool ifFlip = false;
		
		#region some costant values
		const int IMG_WIDTH = 640;
		const int IMG_HEIGHT = 480;
		const int IMG_SIZE = IMG_WIDTH * IMG_HEIGHT;
		const int STEP = 3;
		#endregion
		
		#region basic variables
		List<double> m_abc;
		String m_dir = "none";
		bool m_ifGroundCalibrate = false;
		bool m_ifHumanCalibrate = false;
		bool m_ifHuman = false;
		
		double m_minDist = 1.5;
		double m_maxDist = 3.0;
		double m_limitOffsetHead = 2;
		double m_limitOffsetFoot = -2;
		
		double m_legL;
		List<int> m_humanCenter;
		List<double> m_gd;
		List<int> m_feetLowerPixel = new List<int>(0);
		
		double m_angleLeg;
		double m_speedLeg;
		
		public double m_lastReliableDetection;
		public List<double> m_lastFive = new List<double>(0);
		public List<long> m_lastFiveTime = new List<long>(0);
		public List<double> m_angleHistory = new List<double>(0);
		
		public bool m_ifLoadGD = false;
		
		
		
		#endregion
		
		#region face detection for direction determination (Face mush at left!)
		
		#endregion
		
		#region filter background
		int extract_floor_plane(short[] shortDepth)
		{
			short[] depth = shortDepth;
			List<double> ptPlane = new List<double>(shortDepth.Length);
			for (int u = 0; u < IMG_WIDTH; u += 10)
			{
				for (int v = 0; v < IMG_HEIGHT; v += 10)
				{
					if (Math.Abs(u - IMG_WIDTH / 2) < IMG_WIDTH / 8 && v > IMG_HEIGHT - IMG_HEIGHT / 8)
					{
						List<double> xyz = depth_to_xyz(u, v, depth[u + v * IMG_WIDTH]);
						if (xyz[1] >= 0 && xyz[2] < 0 && xyz[0] == xyz[0])
						{
							ptPlane.Add(xyz[0]);
							ptPlane.Add(xyz[1]);
							ptPlane.Add(xyz[2]);
						}
					}
				}
			}
			List<double> planePara = get_plane_from_point_set(ptPlane);
			m_gd = planePara;
			
			m_ifGroundCalibrate = true;
			
			StreamWriter outfilep = new StreamWriter("plane.txt");
			StreamWriter outfiled = new StreamWriter("depth.txt");
			
			for (int i = 0; i < 3; i++ )
			{
				outfilep.WriteLine(planePara[i].ToString());
			}
			
			outfilep.Close();
			
			//short[] dd = change_arr_direction(shortDepth, 1);
			
			for (int i = 0; i < shortDepth.Length; i++)
			{
				outfiled.WriteLine(shortDepth[i].ToString());
			}
			
			outfiled.Close();
			
			return 0;
		}
		
		short[] filter_background(short[] shortDepth)
		{
			double fx_d = 594.21434211923247;
			double fy_d = 591.04053696870778;
			double cx_d = 339.30780975300314;
			double cy_d = 242.73913761751615;
			
			short[] depth = shortDepth;
			int t = 0;
			
			for (int u = 0; u < IMG_WIDTH; u++)
			{
				for (int v = 0; v < IMG_HEIGHT; v++)
				{
					if (depth[v * IMG_WIDTH + u] < m_minDist * 1000 || depth[v * IMG_WIDTH + u] > m_maxDist * 1000)
					{
						depth[v * IMG_WIDTH + u] = 0; t++;
					}
					else
					{
						double x = (u - cx_d) * (1.0 * depth[v * IMG_WIDTH + u]) / fx_d / 1000;
						double y = 1.0 * depth[v * IMG_WIDTH + u] / 1000;
						double z = -(v - cy_d) * (1.0 * depth[v * IMG_WIDTH + u]) / fy_d / 1000;
						double A = m_gd[0];
						double B = m_gd[1];
						double C = m_gd[2];
						m_abc = m_gd;
						//A = 0.00547722535782924;
						//B = 0.340717078392629;
						//C = -1.1487329093282;
						if (z < A * x + B * y + C + 0.06)
							//if (z < -0.95)
						{
							depth[v * IMG_WIDTH + u] = 0; t++;
						}
					}
				}
			}
			
			short[] res = depth;
			return res;
		}
		
		short[] refine_filter_scale(short[] shortDepth)
		{
			short[] res = new short[IMG_SIZE];
			double mindist = 1000;
			double maxdist = 0;
			double minx = 1000;
			double maxx = -1000;
			
			short[] depth = shortDepth;
			
			for (int u = 0; u < IMG_WIDTH; u++)
			{
				for (int v = 0; v < IMG_HEIGHT; v++)
				{
					int id = v * IMG_WIDTH + u;
					if (depth[id] > 0)
					{
						List<double> xyz = depth_to_xyz(u, v, depth[id]);
						double x = xyz[0];
						double y = xyz[1];
						double z = xyz[2];
						
						if (x < minx) { minx = x; }
						if (x > maxx) { maxx = x; }
						if (y < mindist) { mindist = y; }
						if (y > maxdist) { maxdist = y; }
					}
				}
			}
			
			m_limitOffsetHead = maxx + 0.15;
			m_limitOffsetFoot = minx - 0.15;
			m_minDist = mindist - 0.1;
			m_maxDist = maxdist + 0.1;
			
			res = depth;
			return res;
		}
		#endregion
		
		#region human part detection
		List<int> detect_human_center(short[] shortDepth)
		{
			List<int> res = new List<int>(4) { 0, 0, 0, 0 };
			
			int ut = 0;
			int t = 0;
			short[] depth = shortDepth;
			
			for (int u = 0; u < IMG_WIDTH; u++)
			{
				for (int v = 0; v < IMG_HEIGHT; v++)
				{
					if (depth[v * IMG_WIDTH + u] > 0)
					{
						ut += u;
						t++;
					}
				}
			}
			ut /= t;
			
			t = 0;
			int vt = 0;
			for (int v = 0; v < IMG_HEIGHT; v++)
			{
				if (depth[v * IMG_WIDTH + ut] > 0)
				{
					vt += v;
					t++;
				}
			}
			vt /= t;
			
			int ve = 0;
			for (int v = IMG_HEIGHT - 1; v >= 0; v--)
			{
				if (depth[v * IMG_WIDTH + ut] > 0)
				{
					ve = v;
					break;
				}
			}
			
			res[0] = ut;
			res[1] = vt;
			res[2] = ut;
			res[3] = ve;
			
			return res;
		}
		
		List<int> detect_body_upper_end(short[] depthHuman, int uc, int vc)
		{
			List<int> res = new List<int>(2) { 0, 0 };
			int STEP = 3;
			bool f = true;
			int[] uf = { -STEP, -STEP, 0, 0, STEP, STEP, STEP };
			int[] vf = { -STEP, 0, -STEP, STEP, 0, -STEP, STEP };
			int ct = 0;
			int uend = 0;
			int vend = 0;
			
			int CUBE = 3;
			while (f && ct < 1000)
			{
				f = false;
				
				for (int n = 0; n < uf.Length; n++)
				{
					int ur = uc + uf[n];
					int vr = vc + vf[n];
					int t = 0;
					
					for (int i = -CUBE / 2; i <= CUBE / 2; i++)
					{
						for (int j = -CUBE / 2; j <= CUBE / 2; j++)
						{
							if (depthHuman[(vr + j) * IMG_WIDTH + ur + i] > 0)
							{
								t++;
							}
						}
					}
					if (t == CUBE * CUBE)
					{
						uc = ur;
						vc = vr;
						uend = ur;
						vend = vr;
						f = true;
						break;
					}
				}
				ct++;
			}
			
			res[0] = uend;
			res[1] = vend;
			return res;
		}
		
		List<int> detect_body_flat_end(short[] depthHuman, int uc, int vc)
		{
			List<int> res = new List<int>(2) { 0, 0 };
			int STEP = 3;
			bool f = true;
			int[] uf = { -STEP, -STEP, 0, 0, STEP, STEP, STEP };
			int[] vf = { 0, STEP, STEP, -STEP, STEP, -STEP, 0 };
			int ct = 0;
			int uend = 0;
			int vend = 0;
			
			int CUBE = 3;
			while (f && ct < 1000)
			{
				f = false;
				
				for (int n = 0; n < uf.Length; n++)
				{
					int ur = uc + uf[n];
					int vr = vc + vf[n];
					int t = 0;
					
					for (int i = -CUBE / 2; i <= CUBE / 2; i++)
					{
						for (int j = -CUBE / 2; j <= CUBE / 2; j++)
						{
							if (depthHuman[(vr + j) * IMG_WIDTH + ur + i] > 0)
							{
								t++;
							}
						}
					}
					if (t == CUBE * CUBE)
					{
						uc = ur;
						vc = vr;
						uend = ur;
						vend = vr;
						f = true;
						break;
					}
				}
				ct++;
			}
			
			res[0] = uend;
			res[1] = vend;
			return res;
		}
		
		List<int> detect_body_lower_end(short[] depthHuman, int uc, int vc)
		{
			List<int> res = new List<int>(2) { 0, 0 };
			int STEP = 3;
			bool f = true;
			int[] uf = { -STEP, -STEP, 0, 0, STEP, STEP, STEP };
			int[] vf = { STEP, 0, STEP, -STEP, 0, STEP, -STEP };
			int ct = 0;
			int uend = 0;
			int vend = 0;
			
			int CUBE = 3;
			while (f && ct < 1000)
			{
				f = false;
				
				for (int n = 0; n < uf.Length; n++)
				{
					int ur = uc + uf[n];
					int vr = vc + vf[n];
					int t = 0;
					
					for (int i = -CUBE / 2; i <= CUBE / 2; i++)
					{
						for (int j = -CUBE / 2; j <= CUBE / 2; j++)
						{
							if (depthHuman[(vr + j) * IMG_WIDTH + ur + i] > 0)
							{
								t++;
							}
						}
					}
					if (t == CUBE * CUBE)
					{
						uc = ur;
						vc = vr;
						uend = ur;
						vend = vr;
						f = true;
						break;
					}
				}
				ct++;
			}
			
			res[0] = uend;
			res[1] = vend;
			return res;
		}
		
		double get_leg_length(short[] shortDepth, int uc, int vc) // or imgDepth with commented code
		{
			double res = 0;
			int uter = 0;
			int vter = 0;
			
			bool b = false;
			for (int u = 0; u < IMG_WIDTH - 10; u++)
			{
				for (int v = IMG_HEIGHT - 1; v >= 5; v--)
				{
					int t = 0;
					for (int i = 0; i < 10; i++)
					{
						for (int j = 0; j < 5; j++)
						{
							if (shortDepth[(v - j) * IMG_WIDTH + u + i] > 0)
							{
								t++;
							}
						}
					}
					if (t == 50)
					{
						uter = u + 5;
						vter = v - 2;
						b = true;
					}
					if (b) break;
				}
				if (b) break;
			}
			
			List<double> xyzc = depth_to_xyz(uc, vc, shortDepth[uc + vc * IMG_WIDTH]);
			List<double> xyzter = depth_to_xyz(uter, vter, shortDepth[uter + vter * IMG_WIDTH]);
			double length = Math.Sqrt(Math.Pow(xyzter[0] - xyzc[0], 2) + Math.Pow(xyzter[1] - xyzc[1], 2));
			
			//cout << "Leg Length: " << length << endl;
			res = length;
			//*********************************************
			
			return res;
		}
		
		List<double> get_flap_angle(short[] shortDepth, int uc, int vc, int ue, int ve)
		{
			List<double> res = new List<double>(2) { 0, 0 };
			List<double> xyzc = depth_to_xyz(uc, vc, shortDepth[uc + vc * IMG_WIDTH]);
			List<double> xyze = depth_to_xyz(ue, ve, shortDepth[ue + ve * IMG_WIDTH]);
			
			/*********/
			double u1 = xyze[0] - xyzc[0];
			double u2 = xyze[1] - xyzc[1];
			double u3 = xyze[2] - xyzc[2];
			
			double A = m_gd[0];
			double B = m_gd[1];
			double C = -1;
			
			double a = Math.Asin(Math.Abs(A * u1 + B * u2 + C * u3) / ((A * A + B * B + C / C) * (u1 * u1 + u2 * u2 + u3 * u3)));
			if (a > 3.1415926 / 2)
			{
				a -= 3.1415926 / 2;
			}
			if (a < -3.1415926 / 2)
			{
				a += 3.1415926 / 2;
			}
			/***end***/
			
			double angle = Math.Atan((xyze[2] - xyzc[2]) / (Math.Sqrt(Math.Pow(xyze[0] - xyzc[0], 2) + Math.Pow(xyze[1] - xyzc[1], 2))));
			
			double legL = Math.Sqrt(Math.Pow(xyze[0] - xyzc[0], 2) + Math.Pow(xyze[1] - xyzc[1], 2) + Math.Pow(xyze[2] - xyzc[2], 2));
			
			if (legL < 0.5 * m_legL)
			{
				angle = m_lastReliableDetection;
			}
			else
			{
				m_lastReliableDetection = angle;
			}
			
			res[0] = angle * 180 / 3.1415926;
			res[1] = legL;
			return res;
		}
		
		List<int> detect_upper_feet(short[] depthHuman)
		{
			List<int> res = new List<int>(2) { 0, 0 };
			int UW = 32;
			int VW = 24;
			int US = UW / 8;
			int VS = VW / 8;
			List<int> uvs = new List<int>(2);
			for (int u = 0; u <= IMG_WIDTH - UW; u += US)
			{
				for (int v = 0; v <= IMG_HEIGHT - VW; v += VS)
				{
					float r1 = 0;
					for (int i = 0; i < VW; i++)
					{
						r1 += depthHuman[(v + i) * IMG_WIDTH + u + UW - 1] > 0 ? 1 : 0;
					}
					r1 /= VW;
					
					float r2 = 0;
					for (int i = 0; i < VW; i++)
					{
						r2 += depthHuman[(v + i) * IMG_WIDTH + u] > 0 ? 1 : 0;
					}
					r2 /= VW;
					
					float r3 = 0;
					for (int i = 0; i < UW; i++)
					{
						r3 += depthHuman[v * IMG_WIDTH + u + i] > 0 ? 1 : 0;
					}
					r3 /= UW;
					
					float r4 = 0;
					for (int i = 0; i < UW; i++)
					{
						r1 += depthHuman[(v + VW - 1) * IMG_WIDTH + u + i] > 0 ? 1 : 0;
					}
					r4 /= UW;
					
					if (r1 >= 0 && r2 == 0 && r3 == 0 && r4 > 0 && v + VW / 2 < m_feetLowerPixel[1])
					{
						uvs[0] = u + UW;
						uvs[1] = v + VW / 2;
					}
				}
			}
			
			int um = IMG_WIDTH;
			int vm = 0;
			for (int i = 0; i < uvs.Count / 2; i++)
			{
				if (uvs[2 * i] < um && uvs[2 * i] < IMG_WIDTH)
				{
					um = uvs[2 * i];
					vm = uvs[2 * i + 1];
				}
			}
			
			int xf = 0;
			int yf = 0;
			int tf = 0;
			for (int u = 0; u < UW; u++)
			{
				for (int v = 0; v < VW; v++)
				{
					if (depthHuman[(vm + v) * IMG_WIDTH + um + u] > 0)
					{
						xf += um + u;
						yf += vm + v;
						tf++;
					}
				}
			}
			xf /= tf;
			yf /= tf;
			
			res[0] = xf;
			res[1] = yf;
			
			return res;
		}
		
		List<int> detect_raised_feet(short[] depthHuman, int uc, int vc)
		{
			List<int> res = new List<int>(2) { 0, 0 };
			int uter = 0;
			int vter = 0;
			
			bool b = false;
			for (int v = 2; v < vc; v++)
			{
				for (int u = 2; u < uc; u++)
				{
					int t = 0;
					for (int i = -2; i < 3; i++)
					{
						for (int j = -2; j < 3; j++)
						{
							if (depthHuman[(v - j) * IMG_WIDTH + u + i] > 0)
							{
								t++;
							}
						}
					}
					if (t == 25)
					{
						uter = u;
						vter = v;
						b = true;
					}
					if (b) break;
				}
				if (b) break;
			}
			
			res[0] = uter;
			res[1] = vter;
			return res;
		}
		
		double get_flap_speed(List<double> lastFive, List<long> lastFiveTime)
		{
			double res = 0;
			double path = (lastFive[lastFive.Count - 1] - lastFive[0]);
			long time = (lastFiveTime[lastFiveTime.Count - 1] - lastFiveTime[0]);
			double speed = path / time * 1000;
			res = speed;
			return res;
		}
		
		#endregion
		
		#region task
		public int ground_calibration(short[] shortDepth)
		{
			short[] depthArr = shortDepth;
			depthArr = shift_arr(depthArr);
			if (ifFlip == true)
			{
				depthArr = flip_array_vertical(shortDepth);
			}
			extract_floor_plane(depthArr);
			m_ifGroundCalibrate = true;
			return 0;
		}
		
		public int human_calibration(short[] shortDepth)
		{
			if (m_ifLoadGD == false)
			{
				load_gd("plane.txt");
			}
			
			int res = 0;
			short[] depthArr = shortDepth;
			depthArr = shift_arr(depthArr);
			if (ifFlip == true)
			{
				depthArr = flip_array_vertical(shortDepth);
			}
			
			depthArr = filter_background(depthArr);
			
			///------ get human center and leg length
			m_humanCenter = detect_human_center(depthArr);
			m_legL = get_leg_length(depthArr, m_humanCenter[0], m_humanCenter[1]);
			detect_body_flat_end(depthArr, m_humanCenter[0], m_humanCenter[1]);
			
			m_ifHumanCalibrate = true;
			
			return res;
		}
		
		public int task(short[] shortDepth)
		{
			DateTime now1 = DateTime.Now;
			short[] depthArr = shortDepth;
			depthArr = shift_arr(depthArr);
			if (ifFlip == true)
			{
				depthArr = flip_array_vertical(shortDepth);
			}
			
			depthArr = filter_background(depthArr);
			
			///------ detect body ends
			List<int> uvend = detect_body_upper_end(depthArr, m_humanCenter[0], m_humanCenter[1]);
			List<double> angle = get_flap_angle(depthArr, m_humanCenter[0] + 10, m_humanCenter[1], uvend[0], uvend[1]);
			List<int> uvend2 = detect_raised_feet(depthArr, m_humanCenter[0], m_humanCenter[1]);
			List<double> angle2 = get_flap_angle(depthArr, m_humanCenter[0] + 10, m_humanCenter[1], uvend2[0], uvend2[1]);
			
			//------ compute angle
			double angleInput = angle[0];
			double legLInput = angle[1];
			if (m_lastReliableDetection > 30)
			{
				angleInput = angle2[0];
				legLInput = angle2[1];
			}
			double integratedAngle = kalman_filter(angleInput, m_lastFive, m_legL, legLInput);
			collect_latest_angles(integratedAngle);
			
			m_angleLeg = integratedAngle;
			m_speedLeg = get_flap_speed(m_lastFive, m_lastFiveTime);
			
			//------ record and show angle
			m_angleHistory.Add(m_angleLeg);
			DateTime now2 = DateTime.Now;
			
			return 0;
		}
		
		public short[] unique_task(short[] shortDepth)
		{
			DateTime now1 = DateTime.Now;
			short[] depthArr = shortDepth;
			depthArr = shift_arr(depthArr);
			if (m_ifGroundCalibrate == false)
			{
				extract_floor_plane(depthArr);
			}
			depthArr = get_person_map(depthArr);
			
			if (m_ifHuman == true)
			{
				m_humanCenter = detect_human_center(depthArr);
				m_legL = get_leg_length(depthArr, m_humanCenter[0], m_humanCenter[1]);
				
				depthArr = filter_background(depthArr);
				
				List<int> uvend = detect_body_upper_end(depthArr, m_humanCenter[0], m_humanCenter[1]);
				List<double> angle = get_flap_angle(depthArr, m_humanCenter[0] + 10, m_humanCenter[1], uvend[0], uvend[1]);
				List<int> uvend2 = detect_raised_feet(depthArr, m_humanCenter[0], m_humanCenter[1]);
				List<double> angle2 = get_flap_angle(depthArr, m_humanCenter[0] + 10, m_humanCenter[1], uvend2[0], uvend2[1]);
				
				//------ compute angle
				double angleInput = angle[0];
				double legLInput = angle[1];
				if (m_lastReliableDetection > 30)
				{
					angleInput = angle2[0];
					legLInput = angle2[1];
				}
				double integratedAngle = kalman_filter(angleInput, m_lastFive, m_legL, legLInput);
				collect_latest_angles(integratedAngle);
				
				m_angleLeg = integratedAngle;
				m_speedLeg = get_flap_speed(m_lastFive, m_lastFiveTime);
				
				//------ record and show angle
				m_angleHistory.Add(m_angleLeg);
			}
			DateTime now2 = DateTime.Now;
			
			short[] res = depthArr;
			return res;
		}
		
		public double get_angle()
		{
			return m_angleLeg;
		}
		
		public double get_speed()
		{
			return m_speedLeg;
		}
		
		#endregion
		
		#region assistive functions
		public short[] get_person_map(short[] depth)
		{
			short[] res = depth;
			int XX = 20;
			int YY = 20;
			double xx = 4.0 / 20;
			double yy = 4.0 / 20;
			int[,] map = new int[YY, XX];
			
			double fx_d = 594.21434211923247F;
			double fy_d = 591.04053696870778F;
			double cx_d = 339.30780975300314F;
			double cy_d = 242.73913761751615F;
			int u, v;
			double A = m_gd[0];
			double B = m_gd[1];
			double C = m_gd[2];
			for (int i = 0; i < IMG_SIZE; i++)
			{
				v = i / IMG_WIDTH;
				u = i % IMG_WIDTH;
				double df = depth[i] * 1.0 / 1000;
				
				double x = (u - cx_d) * df / fx_d;
				double z = -(v - cy_d) * df / fy_d;
				double y = df;
				
				if ( x > -2 && x < 2 && y > 1 && y < 4 && z > A * x + B * y + C + 0.10)
				{
					map[(int)(y / yy), (int)((x + 2) / xx)] = 1;
				}
			}
			
			//for (int y = 0; y < 40; y++)
			//{
			//    for (int x = 0; x < 40; x++)
			//    {
			//        Console.Write(map[y, x].ToString());
			//    }
			//    Console.Write("\n");
			//}
			//Console.Write("\n");
			
			ConnectedComponent cc = new ConnectedComponent();
			int[,] lmap = cc.connect_component(map, YY, XX);
			int[] mt = new int[YY*XX];
			
			for (int y = 0; y < YY; y++)
			{
				for (int x = 0; x < XX; x++)
				{
					mt[lmap[y, x]]++;
					//Console.Write(lmap[y, x].ToString());
				}
				//Console.Write("\n");
			}
			int maxidx = 1;
			int max = mt[1];
			for (int i = 1; i < YY * XX; i++)
			{ 
				if (mt[i] > max)
				{
					maxidx = i;
					max = mt[i];
				}
			}
			
			if (max < 10)
			{
				m_ifHuman = false;
				Console.WriteLine(max);
			}
			else 
			{
				m_ifHuman = true;
			}
			
			for (int i = 0; i < IMG_SIZE; i++)
			{
				v = i / IMG_WIDTH;
				u = i % IMG_WIDTH;
				double df = depth[i] * 1.0 / 1000;
				
				double x = (u - cx_d) * df / fx_d;
				double z = -(v - cy_d) * df / fy_d;
				double y = df;
				
				if (x > -2 && x < 2 && y > 1 && y < 4 && z > A * x + B * y + C + 0.06)
				{
					if (lmap[(int)(y / yy), (int)((x + 2) / xx)] != maxidx)
					{
						res[i] = 0;
					}
				}
				else
				{
					res[i] = 0;
				}
			}
			return res;
		}
		
		
		
		List<double> depth_to_xyz(int u, int v, short depth)
		{
			List<double> res = new List<double>(3) { 0, 0, 0 };
			double df = depth * 1.0 / 1000;
			
			double fx_d = 594.21434211923247F;
			double fy_d = 591.04053696870778F;
			double cx_d = 339.30780975300314F;
			double cy_d = 242.73913761751615F;
			
			res[0] = (u - cx_d) * df / fx_d;
			res[2] = -(v - cy_d) * df / fy_d;
			res[1] = df;
			
			return res;
		}
		
		List<double> get_plane_from_point_set(List<double> pt)
		{
			List<double> res = new List<double>(3) { 0, 0, 0 };
			
			double xy = 0;
			double xz = 0;
			double yz = 0;
			double x2 = 0;
			double y2 = 0;
			double x = 0;
			double y = 0;
			double z = 0;
			double n = pt.Count / 3;
			for (int i = 0; i < pt.Count / 3; i++)
			{
				double xf = pt[3 * i];
				double yf = pt[3 * i + 1];
				double zf = pt[3 * i + 2];
				x += xf;
				y += yf;
				z += zf;
				//cout << xf << " " << yf << " " << zf << endl;
				x2 += xf * xf;
				y2 += yf * yf;
				
				xy += xf * yf;
				xz += xf * zf;
				yz += yf * zf;
			}
			
			double[,] m1Arr = new double[3, 3] {{x2, xy, x}, 
				{xy, y2, y},
				{x,  y,  n}};
			double[,] m1ArrInv = MatrixInv.InvertMatrix(m1Arr, 3);
			
			//for (int i = 0; i < 3; i++)
			//{
			//    for (int j = 0; j < 3; j++)
			//    {
			//        Console.Write(m1Arr[i, j].ToString() + " ");
			//    }
			//    Console.WriteLine(" ");
			//}
			
			//for (int i = 0; i < 3; i++)
			//{
			//    for (int j = 0; j < 3; j++)
			//    {
			//        Console.Write(m1ArrInv[i, j].ToString() + " ");
			//    }
			//    Console.WriteLine(" ");
			//}
			
			double[,] m1i = m1ArrInv;
			double[,] m2 = new double[3, 1] {{xz}, 
				{yz},
				{z}};
			
			
			res[0] = m1i[0, 0] * m2[0, 0] + m1i[0, 1] * m2[1, 0] + m1i[0, 2] * m2[2, 0];
			res[1] = m1i[1, 0] * m2[0, 0] + m1i[1, 1] * m2[1, 0] + m1i[1, 2] * m2[2, 0];
			res[2] = m1i[2, 0] * m2[0, 0] + m1i[2, 1] * m2[1, 0] + m1i[2, 2] * m2[2, 0];
			
			Console.WriteLine("gd:" + res[0].ToString() + " " + res[1].ToString() + " " + res[2].ToString());
			return res;
		}
		
		int collect_latest_angles(double angle)
		{
			m_lastFive.Add(angle);
			DateTime now = DateTime.Now;
			int time = now.Second * 1000 + now.Millisecond;
			m_lastFiveTime.Add(time);
			if (m_lastFive.Count > 5)
			{
				m_lastFive.RemoveAt(0);
				m_lastFiveTime.RemoveAt(0);
			}
			return m_lastFive.Count;
		}
		
		double kalman_filter(double angle, List<double> lastFive, double legLength, double observeLength)
		{
			double res;
			
			if (m_lastFive.Count >= 5)
			{
				double probObserve = 1 - Math.Pow(Math.Abs(observeLength / legLength - 1), 3);
				
				double speed = (lastFive[4] - lastFive[3]) + (lastFive[4] - lastFive[3]) - (lastFive[3] - lastFive[2]);
				double predictAngle = lastFive[4] + speed;
				
				double integratedAngle = probObserve * angle + (1 - probObserve) * predictAngle;
				res = integratedAngle;
			}
			else
			{
				res = angle;
			}
			
			return res;
		}
		
		public short[] flip_array_vertical(short[] shortDepth)
		{
			short[] res = new short[IMG_SIZE];
			
			for (int u = 0; u < IMG_WIDTH; u++)
			{
				for (int v = 0; v < IMG_HEIGHT; v++)
				{
					int id1 = v * IMG_WIDTH + u;
					int id2 = (IMG_HEIGHT - v - 1) * IMG_WIDTH + u;
					res[id1] = shortDepth[id2];
				}
			}
			
			return res;
		}
		
		public short[] change_arr_direction(short[] a, int dir)
		{
			short[] r = new short[IMG_SIZE];
			if (dir == 0)
				return a;
			
			if (dir > 0)
			{
				for (int u = 0; u < IMG_WIDTH; u++)
				{
					for (int v = 0; v < IMG_HEIGHT; v++)
					{
						r[v + u * IMG_HEIGHT] = a[u + v * IMG_WIDTH];
					}
				}
			}
			
			if (dir < 0)
			{
				for (int v = 0; v < IMG_HEIGHT; v++)
				{
					for (int u = 0; u < IMG_WIDTH; u++)
					{
						r[u + v * IMG_WIDTH] = a[v + u * IMG_HEIGHT];
					}
				}
			}
			return r;
		}
		
		short[] shift_arr(short[] depth)
		{
			short[] r = new short[depth.Length];
			for (int i = 0; i < depth.Length; i++)
			{
				r[i] = (short)(depth[i] >> 3);
			}
			
			return r;
		}
		
		int load_gd(string fileName)
		{
			string[] lines = System.IO.File.ReadAllLines(fileName);
			m_gd[0] = Convert.ToDouble(lines[0]);
			m_gd[1] = Convert.ToDouble(lines[1]);
			m_gd[2] = Convert.ToDouble(lines[2]);
			m_ifLoadGD = true;
			return 0;
		}
		
		
		#endregion
	}
	
	class MyContainer
	{
		public double[][] m_double;
		public int[] m_int;
	}
	
	class MatrixInv
	{
		/*
        * Perform LUP decomposition on a matrix A.
        * Return L and U as a single matrix(double[][]) and P as an array of ints.
        * We implement the code to compute LU "in place" in the matrix A.
        * In order to make some of the calculations more straight forward and to 
        * match Cormen's et al. pseudocode the matrix A should have its first row and first columns
        * to be all 0.
        * */
		public static MyContainer LUPDecomposition(double[][] A)
		{
			int n = A.Length - 1;
			/*
            * pi represents the permutation matrix.  We implement it as an array
            * whose value indicates which column the 1 would appear.  We use it to avoid 
            * dividing by zero or small numbers.
            * */
			int[] pi = new int[n + 1];
			double p = 0;
			int kp = 0;
			int pik = 0;
			int pikp = 0;
			double aki = 0;
			double akpi = 0;
			
			//Initialize the permutation matrix, will be the identity matrix
			for (int j = 0; j <= n; j++)
			{
				pi[j] = j;
			}
			
			for (int k = 0; k <= n; k++)
			{
				/*
                * In finding the permutation matrix p that avoids dividing by zero
                * we take a slightly different approach.  For numerical stability
                * We find the element with the largest 
                * absolute value of those in the current first column (column k).  If all elements in
                * the current first column are zero then the matrix is singluar and throw an
                * error.
                * */
				p = 0;
				for (int i = k; i <= n; i++)
				{
					if (Math.Abs(A[i][k]) > p)
					{
						p = Math.Abs(A[i][k]);
						kp = i;
					}
				}
				if (p == 0)
				{
					throw new Exception("singular matrix");
				}
				/*
                * These lines update the pivot array (which represents the pivot matrix)
                * by exchanging pi[k] and pi[kp].
                * */
				pik = pi[k];
				pikp = pi[kp];
				pi[k] = pikp;
				pi[kp] = pik;
				
				/*
                * Exchange rows k and kpi as determined by the pivot
                * */
				for (int i = 0; i <= n; i++)
				{
					aki = A[k][i];
					akpi = A[kp][i];
					A[k][i] = akpi;
					A[kp][i] = aki;
				}
				
				/*
                    * Compute the Schur complement
                    * */
				for (int i = k + 1; i <= n; i++)
				{
					A[i][k] = A[i][k] / A[k][k];
					for (int j = k + 1; j <= n; j++)
					{
						A[i][j] = A[i][j] - (A[i][k] * A[k][j]);
					}
				}
			}
			
			MyContainer res = new MyContainer();
			res.m_double = A;
			res.m_int = pi;
			
			return res;
		}
		
		/*
        * Given L,U,P and b solve for x.
        * Input the L and U matrices as a single matrix LU.
        * Return the solution as a double[].
        * LU will be a n+1xm+1 matrix where the first row and columns are zero.
        * This is for ease of computation and consistency with Cormen et al.
        * pseudocode.
        * The pi array represents the permutation matrix.
        * */
		public static double[] LUPSolve(double[][] LU, int[] pi, double[] b)
		{
			int n = LU.Length - 1;
			double[] x = new double[n + 1];
			double[] y = new double[n + 1];
			double suml = 0;
			double sumu = 0;
			double lij = 0;
			
			/*
            * Solve for y using formward substitution
            * */
			for (int i = 0; i <= n; i++)
			{
				suml = 0;
				for (int j = 0; j <= i - 1; j++)
				{
					/*
                    * Since we've taken L and U as a singular matrix as an input
                    * the value for L at index i and j will be 1 when i equals j, not LU[i][j], since
                    * the diagonal values are all 1 for L.
                    * */
					if (i == j)
					{
						lij = 1;
					}
					else
					{
						lij = LU[i][j];
					}
					suml = suml + (lij * y[j]);
				}
				y[i] = b[pi[i]] - suml;
			}
			//Solve for x by using back substitution
			for (int i = n; i >= 0; i--)
			{
				sumu = 0;
				for (int j = i + 1; j <= n; j++)
				{
					sumu = sumu + (LU[i][j] * x[j]);
				}
				x[i] = (y[i] - sumu) / LU[i][i];
			}
			return x;
		}
		
		/*
        * Given an nXn matrix A, solve n linear equations to find the inverse of A.
        * */
		public static double[,] InvertMatrix(double[,] Ainput, int ndim)
		{
			int n = ndim;
			double[][] A = new double[n][];
			for (int i = 0; i < n; i++)
			{
				A[i] = new double[n];
				for (int j = 0; j < n; j++)
				{
					A[i][j] = Ainput[i, j];
				}
			}
			//e will represent each column in the identity matrix
			double[] e;
			//x will hold the inverse matrix to be returned
			double[][] x = new double[n][];
			double[,] res = new double[n, n];
			for (int i = 0; i < n; i++)
			{
				x[i] = new double[A[i].Length];
			}
			/*
            * solve will contain the vector solution for the LUP decomposition as we solve
            * for each vector of x.  We will combine the solutions into the double[][] array x.
            * */
			double[] solve;
			
			//Get the LU matrix and P matrix (as an array)
			MyContainer results = LUPDecomposition(A);
			
			double[][] LU = results.m_double;
			int[] P = results.m_int;
			
			/*
            * Solve AX = e for each column ei of the identity matrix using LUP decomposition
            * */
			for (int i = 0; i < n; i++)
			{
				e = new double[A[i].Length];
				e[i] = 1;
				solve = LUPSolve(LU, P, e);
				for (int j = 0; j < solve.Length; j++)
				{
					x[j][i] = solve[j];
					res[j, i] = x[j][i];
				}
			}
			return res;
		}
	}
	
	public class ConnectedComponent
	{
		int xSize = 10;
		int ySize = 10;
		int[,] I;// = new byte[ySize, xSize]; //Original Image
		int[,] Lfirst;//  = new int [ySize,xSize]; //Labels after first step
		int[,] Lmerge;//  = new int [ySize,xSize]; //Labels after merging
		int[,] Lsorted;// = new int [ySize,xSize]; //Labels after sorting
		
		bool ifeight = true;
		Int32 i, x, y, label, no_of_merge_passes;
		
		
		public int[,] connect_component(int[,] map, int yy, int xx)
		{
			xSize = xx;
			ySize = yy;
			I = map;
			Lfirst = new int[yy, xx];
			Lmerge = new int[yy, xx];
			Lsorted = new int[yy, xx];
			
			fill_Lfirst();
			fill_Lmerge();
			fill_Lsorted();
			
			//Random random = new Random();
			//for (y = 0; y < ySize; y++)
			//{
			//    for (x = 0; x < xSize; x++)
			//    {
			//        Int32 noise = random.Next() % 3 - 1;
			//        noise += I[y, x];
			//        if (noise < 0) I[y, x] = 0;
			//        else if (noise > 9) I[y, x] = 9;
			//        else I[y, x] = (int)noise;
			//    }
			//}
			
			//for (y = 0; y < ySize; y++)
			//{
			//    for (x = 0; x < xSize; x++)
			//    {
			//        Lfirst[y, x] = Lmerge[y, x] = Lsorted[y, x] = I[y, x] = 0;
			//    }
			//}
			
			//for (y = 0; y < ySize; y++)
			//{
			//    for (x = 0; x < xSize; x++)
			//    {
			//        Console.Write(Lsorted[y, x]);
			//    }
			//    Console.Write("\n");
			//}
			//Console.Write("\n");
			return Lsorted;
		}
		
		private void fill_Lfirst()
		{
			Lfirst[0, 0] = label = 1;
			for (x = 1; x < xSize; x++)
				if (I[0, x] == I[0, x - 1]) Lfirst[0, x] = Lfirst[0, x - 1]; //label from left
			else Lfirst[0, x] = ++label;
			for (y = 1; y < ySize; y++)
			{
				if (I[y, 0] == I[y - 1, 0]) Lfirst[y, 0] = Lfirst[y - 1, 0]; //label from upstairs
				else Lfirst[y, 0] = ++label;
				for (x = 1; x < xSize; x++)
					if (I[y, x] == I[y - 1, x]) Lfirst[y, x] = Lfirst[y - 1, x]; //label from upstairs
				else if (I[y, x] == I[y, x - 1]) Lfirst[y, x] = Lfirst[y, x - 1]; //label from left
				else Lfirst[y, x] = ++label;
			}
		}
		
		private void fill_Lmerge()
		{
			for (y = 0; y < ySize; y++)
				for (x = 0; x < xSize; x++)
					Lmerge[y, x] = Lfirst[y, x]; //just copy
			no_of_merge_passes = 0;
			do
			{
				i = 0; no_of_merge_passes++;
				for (y = 0; y < ySize; y++)
					for (x = 0; x < xSize; x++)
				{
					if (x < xSize - 1 && I[y, x] == I[y, x + 1]) //merge with right neighbor ?
						if (Lmerge[y, x] != Lmerge[y, x + 1])
					{
						Lmerge[y, x] = Lmerge[y, x + 1] =
							Math.Min(Lmerge[y, x], Lmerge[y, x + 1]); i++;
					}
					if (y < ySize - 1 && I[y, x] == I[y + 1, x]) //merge with lower neighbor ?
						if (Lmerge[y, x] != Lmerge[y + 1, x])
					{
						Lmerge[y, x] = Lmerge[y + 1, x] =
							Math.Min(Lmerge[y, x], Lmerge[y + 1, x]); i++;
					}
					if (x > 0 && I[y, x] == I[y, x - 1]) //merge with left neighbor ?
						if (Lmerge[y, x] != Lmerge[y, x - 1])
					{
						Lmerge[y, x] = Lmerge[y, x - 1] =
							Math.Min(Lmerge[y, x], Lmerge[y, x - 1]); i++;
					}
					if (y > 0 && I[y, x] == I[y - 1, x]) //merge with upper neighbor ?
						if (Lmerge[y, x] != Lmerge[y - 1, x])
					{
						Lmerge[y, x] = Lmerge[y - 1, x] =
							Math.Min(Lmerge[y, x], Lmerge[y - 1, x]); i++;
					}
					if (ifeight) //8-Neighborhood ?
					{
						if (x < xSize - 1 && y < ySize - 1 && I[y, x] == I[y + 1, x + 1]) //merge with right lower neighbor ?
							if (Lmerge[y, x] != Lmerge[y + 1, x + 1])
						{
							Lmerge[y, x] = Lmerge[y + 1, x + 1] =
								Math.Min(Lmerge[y, x], Lmerge[y + 1, x + 1]); i++;
						}
						if (x > 0 && y < ySize - 1 && I[y, x] == I[y + 1, x - 1]) //merge with left lower neighbor ?
							if (Lmerge[y, x] != Lmerge[y + 1, x - 1])
						{
							Lmerge[y, x] = Lmerge[y + 1, x - 1] =
								Math.Min(Lmerge[y, x], Lmerge[y + 1, x - 1]); i++;
						}
						if (x > 0 && y > 0 && I[y, x] == I[y - 1, x - 1]) //merge with left upper neighbor ?
							if (Lmerge[y, x] != Lmerge[y - 1, x - 1])
						{
							Lmerge[y, x] = Lmerge[y - 1, x - 1] =
								Math.Min(Lmerge[y, x], Lmerge[y - 1, x - 1]); i++;
						}
						if (x < xSize - 1 && y > 0 && I[y, x] == I[y - 1, x + 1]) //merge with right upper neighbor ?
							if (Lmerge[y, x] != Lmerge[y - 1, x + 1])
						{
							Lmerge[y, x] = Lmerge[y - 1, x + 1] =
								Math.Min(Lmerge[y, x], Lmerge[y - 1, x + 1]); i++;
						}
					}
				}
			} while (i > 0);
		}
		
		private void fill_Lsorted()
		{
			int[,] histogram = new int[label + 1, 2]; //histogram with 2 lines
			for (y = 0; y < ySize; y++)
				for (x = 0; x < xSize; x++)
					histogram[Lmerge[y, x], 0]++;        //first line filling
			int max, imax = 0, newlabel = 1;
			do //sorted new labels from 1 = biggest, 2 = second etc.
			{
				max = 0;
				for (i = 1; i <= label; i++)
				{
					if (histogram[i, 1] > 0) continue; //already sorted
					if (histogram[i, 0] > max) { max = histogram[i, 0]; imax = i; }
				}
				if (max > 0) histogram[imax, 1] = newlabel++;
			} while (max > 0);
			for (y = 0; y < ySize; y++) //apply new labels
				for (x = 0; x < xSize; x++)
					Lsorted[y, x] = histogram[Lmerge[y, x], 1] - 1;
		}
	}