SHARE
TWEET

Untitled

a guest Jun 19th, 2019 98 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. double rotx1 = -((t[2] * t1[1] - t1[2] * t[1]) / (t1[1] * t1[1] + t1[2] * t1[2]));
  2. double rotx11 = Math.Asin(rotx1) / Math.PI * 180;
  3. double roty1 = -((t[2] * t1[0] - t1[2] * t[0]) / (t1[0] * t1[0] + t1[2] * t1[2]));
  4. double roty11 = Math.Asin(roty1) / Math.PI * 180;
  5. double rotz1 = -((t[1] * t1[0] - t1[1] * t[0]) / (t1[0] * t1[0] + t1[1] * t1[1]));
  6. double rotz11 = Math.Asin(rotz1) / Math.PI * 180;
  7. Console.WriteLine("X=" + Convert.ToString(rotx11) + " Y=" +
  8. Convert.ToString(roty11) + " Z=" + Convert.ToString(rotz11));
  9.      
  10. double[] pos = { 395, -0, 37, 90, 90, 0 };//The coordinates of the first marker.          
  11.         double[] pos1 = { 321, -0, 37, 90, 90, 0 };//The coordinates of the second marker.
  12.         double[] t = { 0, 0, 0, 0, 0, 0 };
  13.         double[] t1 = { 0, 0, 0, 0, 0, 0 };
  14.         double[] t2= { 0, 0, 0, 0, 0, 0 };
  15.  
  16.         double x = -45.5;//Known changes in the coordinates of the point of the first marker
  17.         double y = 59;
  18.         double z = 33 ;
  19.  
  20.         double x1 = -38.5;// Known changes in the coordinates of the point of the second marker
  21.         double y1 = 40.5;
  22.         double z1 = 54;
  23.  
  24.         t[0] = pos[0] + x;
  25.         t[1] = pos[1] + y;
  26.         t[2] = pos[2] -4;
  27.         t[3] = pos[3];
  28.         t[4] = pos[4];
  29.         t[5] = pos[5];
  30.  
  31.         t1[0] = pos1[0] + x1;
  32.         t1[1] = pos1[1] + y1;
  33.         t1[2] = pos1[2] + 17;
  34.         t1[3] = pos1[3];
  35.         t1[4] = pos1[4];
  36.         t1[5] = pos1[5];
  37.  
  38.  
  39.         double Ayx21 = Math.Atan2(t[1] - t1[1], t[0] - t1[0]) / Math.PI * 180;
  40.  
  41.         double rotx = Math.Atan2(t[1] - t1[1], t[2] - t1[2]);
  42.  
  43.         double roty1111 = -Math.Atan2((t[2] - t1[2]) * Math.Cos(rotx), (t[1] - t1[1]));
  44.  
  45.         double rotz = Math.Atan2(Math.Sin(roty1111), Math.Sin(rotx) * Math.Cos(rotx)) / Math.PI * 180;
  46.         double rotx11111 = (rotz) - (Ayx21);
  47.         double roty = -Math.Atan2((t[2] - t1[2]) * Math.Cos(rotx), (t[1] - t1[1])) / Math.PI * 180;
  48.         t2[0] = pos[0] + x;
  49.         t2[1] = pos[1] + y;
  50.         t2[2] = pos[2] + z ;//убрать потом -37
  51.         t2[3] = (rotx11111);
  52.         t2[4] = (90 + roty);
  53.  
  54.          t2[5] = (180 + rotz);
  55.         string position1 = Convert.ToString(t2[0]).Replace(',', '.');
  56.         string position2 = Convert.ToString(t2[1]).Replace(',', '.');
  57.         string position3 = Convert.ToString(t2[2]).Replace(',', '.');
  58.         string angle1 = Convert.ToString(t2[3]).Replace(',', '.');
  59.         string angle2 = Convert.ToString(t2[4]).Replace(',', '.');
  60.         string angle3 = Convert.ToString(t2[5]).Replace(',', '.');
  61.         Console.WriteLine("[" + Convert.ToString(position1) + "," + Convert.ToString(position2) + "," + Convert.ToString(position3) + "," + Convert.ToString(angle1) + "," + Convert.ToString(angle2) + "," + Convert.ToString(angle3) + "]");
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top