// get angle from three 3D points public static double getAngle(Point3D A, Point3D B, Point3D C) { double AB = Math.Sqrt( Math.Pow(B.getX() - A.getX(), 2) + Math.Pow(B.getY() - A.getY(), 2) + Math.Pow(B.getZ() - A.getZ(), 2)); double BC = Math.Sqrt( Math.Pow(C.getX() - B.getX(), 2) + Math.Pow(C.getY() - B.getY(), 2) + Math.Pow(C.getZ() - B.getZ(), 2)); double CA = Math.Sqrt( Math.Pow(A.getX() - C.getX(), 2) + Math.Pow(A.getY() - C.getY(), 2) + Math.Pow(A.getZ() - C.getZ(), 2)); if ((-Math.Pow(CA, 2) + Math.Pow(AB, 2) + Math.Pow(BC, 2)) / (2 * AB * BC) < -1 || (-Math.Pow(CA, 2) + Math.Pow(AB, 2) + Math.Pow(BC, 2)) / (2 * AB * BC) > 1) { Console.WriteLine("!!" + (-Math.Pow(CA, 2) + Math.Pow(AB, 2) + Math.Pow(BC, 2)) / (2 * AB * BC)); } return((180 / 3.14) * Math.Acos((-Math.Pow(CA, 2) + Math.Pow(AB, 2) + Math.Pow(BC, 2)) / (2 * AB * BC))); }
public Line3D(Point3D A, Point3D B) { x0 = A.getX(); l = B.getX() - A.getX(); y0 = A.getY(); m = B.getY() - A.getY(); z0 = A.getZ(); n = B.getZ() - A.getZ(); }
// get angle from three 3D points public static double getAngle(Point3D A, Point3D B, Point3D C) { double AB = Math.Sqrt( Math.Pow(B.getX() - A.getX(),2) + Math.Pow(B.getY() - A.getY(),2) + Math.Pow(B.getZ() - A.getZ(),2)); double BC = Math.Sqrt( Math.Pow(C.getX() - B.getX(),2) + Math.Pow(C.getY() - B.getY(),2) + Math.Pow(C.getZ() - B.getZ(),2)); double CA = Math.Sqrt( Math.Pow(A.getX() - C.getX(),2) + Math.Pow(A.getY() - C.getY(),2) + Math.Pow(A.getZ() - C.getZ(),2)); if ((-Math.Pow(CA, 2) + Math.Pow(AB, 2) + Math.Pow(BC, 2)) / ( 2 * AB * BC) < -1 || (-Math.Pow(CA, 2) + Math.Pow(AB, 2) + Math.Pow(BC, 2)) / ( 2 * AB * BC) > 1) Console.WriteLine("!!" + (-Math.Pow(CA, 2) + Math.Pow(AB, 2) + Math.Pow(BC, 2)) / ( 2 * AB * BC)); return (180/3.14) * Math.Acos((-Math.Pow(CA, 2) + Math.Pow(AB, 2) + Math.Pow(BC, 2)) / ( 2 * AB * BC) ); }
public static List<Example> readRightHandTS() { // TRAIN RIGHT HAND PART... if (Constants.DEBUG) { // Attributes: A.8.9.XOY, A.8.9.YOZ, D.8y.9y Console.WriteLine("\n=====\tMANA DREAPTA ====="); Console.WriteLine("\tLanga corp"); } StreamReader sr; String strLine; List<Example> rightHandTS = new List<Example>(); double maxim1 = 0.0; double minim1 = 180.0; double average1 = 0.0; double maxim2 = 0.0; double minim2 = 180.0; double average2 = 0.0; double maxim3 = 0.0; double minim3 = 180.0; double average3 = 0.0; try { sr = new StreamReader("D:\\LICENTA\\Pose Recognition\\input_antrenare_3\\msd\\langa corp.txt"); double fileSize = 22323; for (int i=0; i<fileSize; i+=21) { strLine = sr.ReadLine(); // blank strLine = sr.ReadLine(); // 0:hipcenter strLine = sr.ReadLine(); // 1:spine strLine = sr.ReadLine(); // 2:shoulder center strLine = sr.ReadLine(); // 3:head strLine = sr.ReadLine(); // 4:shoulder left strLine = sr.ReadLine(); // 5:elbow left strLine = sr.ReadLine(); // 6:wrist left strLine = sr.ReadLine(); // 7:hand left strLine = sr.ReadLine(); // 8:shoulder right string[] words = strLine.Split(' '); Point3D P8 = new Point3D(Double.Parse(words[0]), Double.Parse(words[1]), Double.Parse(words[2])); strLine = sr.ReadLine(); // 9:elbow right words = strLine.Split(' '); Point3D P9 = new Point3D(Double.Parse(words[0]), Double.Parse(words[1]), Double.Parse(words[2])); strLine = sr.ReadLine(); // 10:wrist right strLine = sr.ReadLine(); // 11:hand right strLine = sr.ReadLine(); // 12:hip left strLine = sr.ReadLine(); // 13:knee left strLine = sr.ReadLine(); // 14:ankle left strLine = sr.ReadLine(); // 15:foot left strLine = sr.ReadLine(); // 16:hip right strLine = sr.ReadLine(); // 17:knee right strLine = sr.ReadLine(); // 18:ankle right strLine = sr.ReadLine(); // 19:foot right Example example = new Example(); double angle = GeometryUtils.getAngle(P8,P9,Constants.planeType.XOY); if (maxim1 < angle) maxim1 = angle; if (minim1 > angle) minim1 = angle; average1 += angle; example.add("A.8.9.XoY", angle); angle = GeometryUtils.getAngle(P8,P9,Constants.planeType.YOZ); if (maxim2 < angle) maxim2 = angle; if (minim2 > angle) minim2 = angle; average2 += angle; example.add("A.8.9.YoZ", angle); angle = P8.getY() - P9.getY(); if (maxim3 < angle) maxim3 = angle; if (minim3 > angle) minim3 = angle; average3 += angle; example.add("D.8y.9y", angle); example.setResult("langa corp"); rightHandTS.Add(example); } if (Constants.DEBUG) { Console.WriteLine("A.8.9.XOY:\t(" + (int)minim1 + ", " + (int)maxim1 + "), " + ((int)average1/fileSize*21)); Console.WriteLine("A.8.9.YOZ:\t(" + (int)minim2 + ", " + (int)maxim2 + "), " + ((int)average2/fileSize*21)); Console.WriteLine("D.8y.9y:\t(" + (int)minim3 + ", " + (int)maxim3 + "), " + ((int)average3/fileSize*21)); } //RIDICAT LATERAL if (Constants.DEBUG) { Console.WriteLine("\tRidicat lateral"); } sr = new StreamReader("D:\\LICENTA\\Pose Recognition\\input_antrenare_3\\msd\\ridicat lateral.txt"); fileSize = 24444; maxim1 = 0.0; minim1 = 180.0; average1 = 0.0; maxim2 = 0.0; minim2 = 180.0; average2 = 0.0; maxim3 = 0.0; minim3 = 180.0; average3 = 0.0; for (int i=0; i<fileSize; i+=21) { strLine = sr.ReadLine(); // blank strLine = sr.ReadLine(); // 0:hipcenter strLine = sr.ReadLine(); // 1:spine strLine = sr.ReadLine(); // 2:shoulder center strLine = sr.ReadLine(); // 3:head strLine = sr.ReadLine(); // 4:shoulder left strLine = sr.ReadLine(); // 5:elbow left strLine = sr.ReadLine(); // 6:wrist left strLine = sr.ReadLine(); // 7:hand left strLine = sr.ReadLine(); // 8:shoulder right string[] words = strLine.Split(' '); Point3D P8 = new Point3D(Double.Parse(words[0]), Double.Parse(words[1]), Double.Parse(words[2])); strLine = sr.ReadLine(); // 9:elbow right words = strLine.Split(' '); Point3D P9 = new Point3D(Double.Parse(words[0]), Double.Parse(words[1]), Double.Parse(words[2])); strLine = sr.ReadLine(); // 10:wrist right strLine = sr.ReadLine(); // 11:hand right strLine = sr.ReadLine(); // 12:hip left strLine = sr.ReadLine(); // 13:knee left strLine = sr.ReadLine(); // 14:ankle left strLine = sr.ReadLine(); // 15:foot left strLine = sr.ReadLine(); // 16:hip right strLine = sr.ReadLine(); // 17:knee right strLine = sr.ReadLine(); // 18:ankle right strLine = sr.ReadLine(); // 19:foot right Example example = new Example(); double angle = GeometryUtils.getAngle(P8,P9,Constants.planeType.XOY); if (maxim1 < angle) maxim1 = angle; if (minim1 > angle) minim1 = angle; average1 += angle; example.add("A.8.9.XoY", angle); angle = GeometryUtils.getAngle(P8,P9,Constants.planeType.YOZ); if (maxim2 < angle) maxim2 = angle; if (minim2 > angle) minim2 = angle; average2 += angle; example.add("A.8.9.YoZ", angle); angle = P8.getY() - P9.getY(); if (maxim3 < angle) maxim3 = angle; if (minim3 > angle) minim3 = angle; average3 += angle; example.add("D.8y.9y", angle); example.setResult("ridicat lateral"); rightHandTS.Add(example); } if (Constants.DEBUG) { Console.WriteLine("A.8.9.XOY:\t(" + (int)minim1 + ", " + (int)maxim1 + "), " + ((int)average1/fileSize*21)); Console.WriteLine("A.8.9.YOZ:\t(" + (int)minim2 + ", " + (int)maxim2 + "), " + ((int)average2/fileSize*21)); Console.WriteLine("D.8y.9y:\t(" + (int)minim3 + ", " + (int)maxim3 + "), " + ((int)average3/fileSize*21)); } // RIDICAT SUS if (Constants.DEBUG) { Console.WriteLine("\tRidicat sus"); } sr = new StreamReader("D:\\LICENTA\\Pose Recognition\\input_antrenare_3\\msd\\ridicat sus.txt"); fileSize = 14469; maxim1 = 0.0; minim1 = 180.0; average1 = 0.0; maxim2 = 0.0; minim2 = 180.0; average2 = 0.0; maxim3 = 0.0; minim3 = 180.0; average3 = 0.0; for (int i=0; i<fileSize; i+=21) { strLine = sr.ReadLine(); // blank strLine = sr.ReadLine(); // 0:hipcenter strLine = sr.ReadLine(); // 1:spine strLine = sr.ReadLine(); // 2:shoulder center strLine = sr.ReadLine(); // 3:head strLine = sr.ReadLine(); // 4:shoulder left strLine = sr.ReadLine(); // 5:elbow left strLine = sr.ReadLine(); // 6:wrist left strLine = sr.ReadLine(); // 7:hand left strLine = sr.ReadLine(); // 8:shoulder right string[] words = strLine.Split(' '); Point3D P8 = new Point3D(Double.Parse(words[0]), Double.Parse(words[1]), Double.Parse(words[2])); strLine = sr.ReadLine(); // 9:elbow right words = strLine.Split(' '); Point3D P9 = new Point3D(Double.Parse(words[0]), Double.Parse(words[1]), Double.Parse(words[2])); strLine = sr.ReadLine(); // 10:wrist right strLine = sr.ReadLine(); // 11:hand right strLine = sr.ReadLine(); // 12:hip left strLine = sr.ReadLine(); // 13:knee left strLine = sr.ReadLine(); // 14:ankle left strLine = sr.ReadLine(); // 15:foot left strLine = sr.ReadLine(); // 16:hip right strLine = sr.ReadLine(); // 17:knee right strLine = sr.ReadLine(); // 18:ankle right strLine = sr.ReadLine(); // 19:foot right Example example = new Example(); double angle = GeometryUtils.getAngle(P8,P9,Constants.planeType.XOY); if (maxim1 < angle) maxim1 = angle; if (minim1 > angle) minim1 = angle; average1 += angle; example.add("A.8.9.XoY", angle); angle = GeometryUtils.getAngle(P8,P9,Constants.planeType.YOZ); if (maxim2 < angle) maxim2 = angle; if (minim2 > angle) minim2 = angle; average2 += angle; example.add("A.8.9.YoZ", angle); angle = P8.getY() - P9.getY(); if (maxim3 < angle) maxim3 = angle; if (minim3 > angle) minim3 = angle; average3 += angle; example.add("D.8y.9y", angle); example.setResult("ridicat sus"); rightHandTS.Add(example); } if (Constants.DEBUG) { Console.WriteLine("A.8.9.XOY:\t(" + (int)minim1 + ", " + (int)maxim1 + "), " + ((int)average1/fileSize*21)); Console.WriteLine("A.8.9.YOZ:\t(" + (int)minim2 + ", " + (int)maxim2 + "), " + ((int)average2/fileSize*21)); Console.WriteLine("D.8y.9y:\t(" + (int)minim3 + ", " + (int)maxim3 + "), " + ((int)average3/fileSize*21)); } // RIDICAT FRONTAL if (Constants.DEBUG) { Console.WriteLine("\tRidicat frontal"); } sr = new StreamReader("D:\\LICENTA\\Pose Recognition\\input_antrenare_3\\msd\\ridicat frontal.txt"); fileSize = 21567; maxim1 = 0.0; minim1 = 180.0; average1 = 0.0; maxim2 = 0.0; minim2 = 180.0; average2 = 0.0; maxim3 = 0.0; minim3 = 180.0; average3 = 0.0; for (int i=0; i<fileSize; i+=21) { strLine = sr.ReadLine(); // blank strLine = sr.ReadLine(); // 0:hipcenter strLine = sr.ReadLine(); // 1:spine strLine = sr.ReadLine(); // 2:shoulder center strLine = sr.ReadLine(); // 3:head strLine = sr.ReadLine(); // 4:shoulder left strLine = sr.ReadLine(); // 5:elbow left strLine = sr.ReadLine(); // 6:wrist left strLine = sr.ReadLine(); // 7:hand left strLine = sr.ReadLine(); // 8:shoulder right string[] words = strLine.Split(' '); Point3D P8 = new Point3D(Double.Parse(words[0]), Double.Parse(words[1]), Double.Parse(words[2])); strLine = sr.ReadLine(); // 9:elbow right words = strLine.Split(' '); Point3D P9 = new Point3D(Double.Parse(words[0]), Double.Parse(words[1]), Double.Parse(words[2])); strLine = sr.ReadLine(); // 10:wrist right strLine = sr.ReadLine(); // 11:hand right strLine = sr.ReadLine(); // 12:hip left strLine = sr.ReadLine(); // 13:knee left strLine = sr.ReadLine(); // 14:ankle left strLine = sr.ReadLine(); // 15:foot left strLine = sr.ReadLine(); // 16:hip right strLine = sr.ReadLine(); // 17:knee right strLine = sr.ReadLine(); // 18:ankle right strLine = sr.ReadLine(); // 19:foot right Example example = new Example(); double angle = GeometryUtils.getAngle(P8,P9,Constants.planeType.XOY); if (maxim1 < angle) maxim1 = angle; if (minim1 > angle) minim1 = angle; average1 += angle; example.add("A.8.9.XoY", angle); angle = GeometryUtils.getAngle(P8,P9,Constants.planeType.YOZ); if (maxim2 < angle) maxim2 = angle; if (minim2 > angle) minim2 = angle; average2 += angle; example.add("A.8.9.YoZ", angle); angle = P8.getY() - P9.getY(); if (maxim3 < angle) maxim3 = angle; if (minim3 > angle) minim3 = angle; average3 += angle; example.add("D.8y.9y", angle); example.setResult("ridicat frontal"); rightHandTS.Add(example); } if (Constants.DEBUG) { Console.WriteLine("A.8.9.XOY:\t(" + (int)minim1 + ", " + (int)maxim1 + "), " + ((int)average1/fileSize*21)); Console.WriteLine("A.8.9.YOZ:\t(" + (int)minim2 + ", " + (int)maxim2 + "), " + ((int)average2/fileSize*21)); Console.WriteLine("D.8y.9y:\t(" + (int)minim3 + ", " + (int)maxim3 + "), " + ((int)average3/fileSize*21)); } } catch (Exception e) {Console.WriteLine(e);} return rightHandTS; }