/************************************************************************//* File: ~/sopra/RoboPackII/MoveMethods.java                              *//* This file consists of the methods which make the robot move around.  *//************************************************************************/package RoboPackII;final class AllAboutMoving{	/* This class provides the 'main class' with the needed        */	/* move and move-supporting methods.		               */	public static void calculateAngle(double[] input, double[] aB,		double[] aE, RefInt stepsB, RefInt stepsE, RoboProto rob)	   /* stepsB and -E should be call-by-reference arguments, so it   */	   /* is sensible to declare them as RefInt (see SmallClasses.     */	   /* java). That means that a reference to the object is passed.  */	{	   double angleMax, angleDiffB, angleDiffE;	   double a, b, v;	   int iMax, i;	   int X;           for (i = 0; i < 6; i++)             {               aE[i] = 0.0;               aB[i] = 0.0;             }                              /* Determination of the largest angle */           iMax = 0;           angleMax = Math.abs(input[0]);           for (i = 1; i < 6; i++)             {               if ( angleMax < Math.abs(input[i]) )		 {                   iMax = i;                   angleMax = Math.abs( input[i]);                 }              }              if ( angleMax != 0.0 )             {               /* Determination of angleDiffB. */               angleDiffB = (( rob.power[iMax] * ( rob.power[iMax] + 1 )) 				/ 2.0 ) * ( Math.PI / 180.0 );                /* Case decision */               if ( (angleDiffB * 2.0) >= Math.abs(input[iMax]) )                 {		   /* It doesn't exist a final speed. */                   aE[iMax] = 0.0;                   stepsE.in = 0;                   X = (int)( Math.abs(input[iMax]) / ( Math.PI/180.0));                   stepsB.in = (int)((1.0 + Math.sqrt( 1.0 + 4.0 * X)) / 2.0);                   if (stepsB.in == 0)		     stepsB.in = 1;                    aB[iMax] = input[iMax] /			 ((stepsB.in) * ((stepsB.in) + 1.0 ) );                   for ( i=0; i<6; i++ )                     {                       if ( i != iMax )                         {                           aE[i] = 0.0;                           aB[i] = input[i] / ((stepsB.in) * 					    ( (stepsB.in) + 1.0 ));                          }                     }                    }                else                  { 	            /*It exists a final speed. */                      stepsB.in = rob.power[iMax];                    angleDiffE = Math.abs(input[iMax]) - ( angleDiffB * 2.0 );                    v = angleDiffB / angleDiffE;                    for (i = 0; i < 6; i++)                      {                        if ( i != iMax )                          {                            a = input[i] / ( 2.0 + ( 2.0 / v ));                            b = input[i] - 2.0 * a;                            aB[i] = a / ((rob.power[iMax] * 					 (rob.power[iMax] + 1.0)) / 2.0);                            aE[i] = b;                          }                        else                          {                            if ( input[iMax] >= 0.0 ) 			      {                                aB[iMax] = Math.PI/180.0;                                aE[iMax] = angleDiffE;                              }                            else 			      {                                aB[iMax] = -Math.PI/180.0;                                aE[iMax] = -angleDiffE;                              }                             stepsE.in = (int)( angleDiffE /					(rob.power[iMax] * Math.PI/180.0));                                              } // else...                      } // for...                    } // else...                } // if (angleMax != 0)...	}	public static void MoveRobo (boolean showTheMove, Points[] pl, 		Points[] line, double[] moveAngle,AngleDates[] angle,		int steps, RefInt errorcode, RefDouble angleY, 		RefDouble angleX, RefDouble d, int show, RefInt indx, 		double[] location, RoboProto rob)	   /* line consists of 2 elements, angle of 6 and location of 3     */	   /* errorcode, angleY, angleX, d and indx should be of my special */	   /* types, since these variables simulate call-by-reference-      */	   /* arguments							    */	{ 	  int i,j; 	  double[] offset = new double[6]; 	  int reminder; 	  double[] input = new double[9]; 	  String[] estring = new String[2];	  reminder = 0;   	  if ( steps == 0 )	    steps = 1;	  for ( i=0 ; i<6 ; i++ ) 	    { 	      offset[i] = moveAngle[i] / steps; 	    } 	  errorcode.in = 0; 	  for (i = 0; i < steps; i++ )  	    {   	  if (errorcode.in == 0 )   	    {  	      if ( offset[0] != 0.0 )	        {   	          RotationY ( pl,offset[0],44,110, rob );                  reminder = 23;                }    	      if ( offset[1] != 0.0 ) 		{                   RotationK ( pl,offset[1],rob.el[86],66,88, rob );                  if ( reminder == 0 ) 		    reminder = 43;                }   	      if ( offset[2] != 0.0 ) 		{                  RotationK ( pl,offset[2],rob.el[128],96,58, rob );                  if ( reminder == 0 ) 		    reminder = 59;                 }   	      if ( offset[3] != 0.0 ) 		{                  RotationK ( pl,offset[3],rob.el[153],114,40, rob );                  if ( reminder == 0 ) 		    reminder = 70;                }   	      if ( offset[4] != 0.0 ) 		{                  RotationK ( pl,offset[4],rob.el[154],116,38, rob );                  if ( reminder == 0 )		     reminder = 70;                }  	      if ( offset[5] != 0.0 )		{                  RotationK ( pl,offset[5],rob.el[180],134,20, rob );                  if ( reminder == 0 )		    reminder = 84;                }     	      if ( reminder != 0 )		 Nvector (pl,rob.al,rob.apl,reminder, rob);   	      for (j=0; j < 6; j++)    	        angle[j].act = angle[j].act + offset[j];           	      if (Crash(pl,rob.rpl, rob))		{ 		  for ( j=0 ; j<6 ; j++ )		    offset[j] = -offset[j];                  PosRobo(pl,offset,angle, rob);                  errorcode.in = 1;                  }	      if ( show == 1 )    		{   		  if ( rob.fFile == 2 && rob.f_Z_B == 0 && rob.f_Z_R == 0)   		    {     		      if ( !((rob.akt) < indx.in))     			{      			  // file_handle (&j,input,estring,rob.BListe,indx,1,			  //	location,angleY,angleX,d);not available   		        }     		      if ( rob.f_Z_B == 0 && rob.f_Z_R == 0)     			{      			  angleY.in = rob.BListe[rob.akt].y;      			  angleX.in = rob.BListe[rob.akt].x;      			  d.in = rob.BListe[rob.akt].z;   			  rob.akt++;     			}   		    }    		  if (showTheMove)		    rob.repaint();		} // if (show == 1)...   	    } // if (errorcode == 0)... 	    } 	}	public static void RotationY(Points[] pl, double angle, int start,				     int number, RoboProto rob)	{	  int i;	  double x, z;	  double cosinus, sinus;		  cosinus = Math.cos(angle);	  sinus = Math.sin(angle);		  for (i = start; i < start + number; i++)	    {	      x = cosinus * pl[i].x + sinus * pl[i].z;	      z = cosinus * pl[i].z - sinus * pl[i].x;	      pl[i].x = x;	      pl[i].z = z;	      rob.animation.setVert(i, pl[i]);// the new coordinates are					      // set in the drawing class	    }	}	public static void RotationK (Points[] pl, double angle, Edges axis,			int start, int number, RoboProto rob )	{	  int i;	  double cosinus,sinus,v0; 	  double r00,r01,r02,r03,   	         r10,r11,r12,r13,     	         r20,r21,r22,r23; 	  double kx,ky,kz,px,py,pz;	  double kxx,kxy,kxz,kyy,kyz,kzz;	  cosinus = Math.cos(angle);	  sinus = Math.sin(angle); 	  v0 = 1 - cosinus;	  px = pl[axis.p1].x;	  py = pl[axis.p1].y;	  pz = pl[axis.p1].z; 	  kx = pl[axis.p2].x - px; 	  ky = pl[axis.p2].y - py; 	  kz = pl[axis.p2].z - pz; 	  kxx = kx * kx * v0; 	  kxy = kx * ky * v0; 	  kxz = kx * kz * v0; 	  kyz = ky * kz * v0;	  kyy = ky * ky * v0; 	  kzz = kz * kz * v0;  	  r00 = kxx + cosinus;	  r10 = kxy + kz * sinus; 	  r20 = kxz - ky *sinus;	  r01 = kxy - kz * sinus; 	  r11 = kyy + cosinus;	  r21 = kyz + kx * sinus;	  r02 = kxy + ky * sinus;	  r12 = kyz - kx * sinus;	  r22 = kzz + cosinus; 	  r03 = r00 * (-px) - r01 * py - r02 * pz + px;	  r13 = r10 * (-px) - r11 * py - r12 * pz + py;	  r23 = r20 * (-px) - r21 * py - r22 * pz + pz; 	  for ( i = start; i < start + number; i++ ) 	    {  	      kx = r00 * pl[i].x + r01 * pl[i].y + r02 * pl[i].z + r03; 	      ky = r10 * pl[i].x + r11 * pl[i].y + r12 * pl[i].z + r13;  	      kz = r20 * pl[i].x + r21 * pl[i].y + r22 * pl[i].z + r23; 	      pl[i].x = kx;  	      pl[i].y = ky;  	      pl[i].z = kz;	      rob.animation.setVert(i, pl[i]);// the new coordinates are					      // set in the drawing class  	    }	}	public static void Nvector (Points[] pl, Areas[] al,		AreasPointsList[] apl, int start, RoboProto rob )	{	  double hilf;	  int i; 	  double hx,hy,hz;	     /* Calculation of the areas parameters */	  for ( i=start; i < rob.numberOfAreas; i++ ) 	    {  	      hx = pl[al[i].p1].y * ( pl[al[i].p2].z - pl[al[i].p3].z )    		  +pl[al[i].p2].y * ( pl[al[i].p3].z - pl[al[i].p1].z )    		  +pl[al[i].p3].y * ( pl[al[i].p1].z - pl[al[i].p2].z );   	      hy = pl[al[i].p1].z * ( pl[al[i].p2].x - pl[al[i].p3].x )    	   	  +pl[al[i].p2].z * ( pl[al[i].p3].x - pl[al[i].p1].x )    		  +pl[al[i].p3].z * ( pl[al[i].p1].x - pl[al[i].p2].x );  	      hz = pl[al[i].p1].x * ( pl[al[i].p2].y - pl[al[i].p3].y )   		   +pl[al[i].p2].x * ( pl[al[i].p3].y - pl[al[i].p1].y )   		   +pl[al[i].p3].x * ( pl[al[i].p1].y - pl[al[i].p2].y );	 	      hilf = Math.sqrt( (hx) * (hx) + (hy) * (hy) + (hz) * (hz) );  	      apl[i].nx = hx / hilf;  	      apl[i].ny = hy / hilf;	      apl[i].nz = hz / hilf;	    }	}	public static boolean Crash(Points[] pl, int[] rpl, RoboProto rob)   	{ 	  int i; 	  /* --------------------------------    	     Tests, if crash with socket and   	     area happens    	    -------------------------------- */ 	  for (i=0; i < 14 ;i++)  	    {    	      if (( Math.abs(pl[rpl[i]].x) < 4.5 &&                    Math.abs(pl[rpl[i]].y) < 3.0 &&                    Math.abs(pl[rpl[i]].z) < 4.5 ) ||                  pl[rpl[i]].y < -0.05   ) 	      return true;   	    } 	  /* ---------------------------------   	   Tests, if robot crashes against link1.   	     --------------------------------- */  	  for ( i=2 ; i<14 ; i++ ) 	    {  	      if ( Math.abs(pl[rpl[i]].x) <= 1.3 &&    	           Math.abs(pl[rpl[i]].z) <= 1.3 &&      		   pl[rpl[i]].y <= rob.linkLengths[0] + 0.5 )	      return true; 	    }	  return false;	}	public static void PosRobo (Points[] pl,double[] turnangle,			AngleDates[] angle, RoboProto rob)	{ 	  int j;  	  if ( turnangle[0] != 0.0 ) 	    RotationY ( pl,turnangle[0],44,110, rob );  	  if ( turnangle[1] != 0.0 ) 	    RotationK ( pl,turnangle[1],rob.el[86],66,88, rob );   	  if ( turnangle[2] != 0.0 )	    RotationK ( pl,turnangle[2],rob.el[128],96,58, rob );   	  if ( turnangle[3] != 0.0 ) 	    RotationK ( pl,turnangle[3],rob.el[153],114,40, rob );  	  if ( turnangle[4] != 0.0 ) 	    RotationK ( pl,turnangle[4],rob.el[154],116,38, rob);  	  if ( turnangle[5] != 0.0 ) 	    RotationK ( pl,turnangle[5],rob.el[180],134,20, rob );    	  for ( j=0 ; j<6 ; j++ )   	   angle[j].act = angle[j].act + turnangle[j];      	}	public static void angleCalculation(double x, double y, double z,			RefDouble angleY, RefDouble angleX, RefDouble d )	    // x:      new projection center	    // angleY: Rotation angle around y-axis	    // angleX: Rotation angle around x-axis	    // d:      Position on the z-axis	{ 	  double amount;	  double hx,hy,hz; 	  amount = Math.sqrt ( x*x + z*z ); 	  /* Calculation of the angle amount of  angleY */ 	  if ( amount == 0 ) 	    {              angleY.in = 0.0;            }	  else             {              angleY.in = Math.acos( z / amount );            } 	  /* Calculation of the sign of angleY */	  if (  x > 0 )	    angleY.in = -angleY.in; 	  /* Rotation around the y-axis */	  hx = Math.cos(angleY.in)*x + Math.sin(angleY.in)*z; 	  hy = y; 	  hz = Math.cos(angleY.in)*z - Math.sin(angleY.in)*x; 	  amount = Math.sqrt ( hx*hx + hy*hy + hz*hz ); 	  /* Calculation of the angle amount of angleX */ 	  angleX.in = Math.acos( hz/amount ); 	  /* Calculation of the sign of angleX */ 	  if ( hy < 0 ) 	    angleX.in = -angleX.in; 	  /* Rotation around the x-axis */ 	  x = hx; 	  y = Math.cos(angleX.in)*hy - Math.sin(angleX.in)*hz; 	  z = Math.sin(angleX.in)*hy + Math.cos(angleX.in)*hz; 	  /* Test, if the new projection center is close enough 	     to the z-axis					*/ 	  if ((Math.abs(x)<0.01)&&(Math.abs(y)<0.01))               d.in = z; 	  else                       {              System.exit(1);            }      	}	public static void copyPointList (Points[] PL, Points[] pl, 								RoboProto rob )	{	  int i; 	  for (i=0; i < rob.numberOfPoints; i++ ) 	    {  	      pl[i].x = PL[i].x;  	      pl[i].y = PL[i].y;  	      pl[i].z = PL[i].z; 	    }	}	public static void calculateLocation(double[] location,			double angleY, double angleX )	{	  double x,y,z; 	  x = Math.cos(angleY) * location[0] + 	      Math.sin(angleY) * Math.sin(angleX) * location[1] +	      Math.sin(angleY) * Math.cos(angleX) * location[2]; 	  y = Math.cos(angleX) * location[1] - Math.sin(angleX) * location[2]; 	  z = -Math.sin(angleY) * location[0] + 	      Math.cos(angleY) * Math.sin(angleX) * location[1] + 	      Math.cos(angleY) * Math.cos(angleX) * location[2]; 	  location[0] = x; 	  location[1] = y; 	  location[2] = z;	}}