/************************************************************************//* File: ~/sopra/RoboPackII/Cinematic.java                                *//* This file consists of the methods the robot needs to calculate his	*//* new values								*//************************************************************************/package RoboPackII;final class CinematicCalculations{	/* This class provides the 'main class' with the needed        */	/* calculation methods.					       */	public static void TO6(AngleDates[] angles, double[] linkl, 			       double[][] T)	{	   /* The 'angles'- and 'linkl'-arrays have to consist of 6    */	   /* elements, the 'T'-array is a 3x4-array.		       */	   double c12, s12; 	   double[] s = new double[6];	   double[] c = new double[6]; 	   double h1,h2; 	   int i; 	   for ( i=0 ; i<6 ; i++ )  	     {               c[i] = Math.cos( angles[i].act );               s[i] = Math.sin( angles[i].act );             } 	   c12 = c[1] * c[2] - s[1] * s[2]; 	   s12 = c[1] * s[2] + s[1] * c[2];           h1 = c[3] * c[4] * c[5] - s[3] * s[5]; 	   h2 = s[4] * c[5]; 	   T[2][0] = (-s12) * h1 - c12 * h2;            h1 = c12 * h1 - s12 * h2;	   h2 = s[3] * c[4] * c[5] + c[3] * s[5];	   T[0][0] = c[0] * h1 + s[0] * h2;	   T[1][0] = s[0] * h1 - c[0] * h2; 	   h1 = (-c[3]) * c[4] * s[5] - s[3] * c[5]; 	   h2 = s[4] * s[5]; 	   T[2][1] = (-s12) * h1 + c12 * h2; 	   h1 = c12 * h1 + s12 * h2;	   h2 = c[3] * c[5] - s[3] * c[4] * s[5]; 	   T[0][1] = c[0] * h1 + s[0] * h2; 	   T[1][1] = s[0] * h1 - c[0] * h2; 	   h1 = c[3] * s[4]; 	   T[2][2] = s12 * h1 - c12 * c[4];  	   h1 = c12 * h1 + s12 * c[4]; 	   h2 = s[3] * s[4];           T[0][2] = (-c[0]) * h1 - s[0] * h2;	   T[1][2] = (-s[0]) * h1 + c[0] * h2; 	   h1 = linkl[1] * c[1] - linkl[2] * s12; 	   T[0][3] = c[0] * h1; 	   T[1][3] = s[0] * h1; 	   T[2][3] = (-linkl[1]) * s[1] - linkl[2] * c12;	}	public static void calculatePos(AngleDates[] angles, double[] pos,					RoboProto rob)	{	   /* 'Angles has to be a 6-elements- and 'pos' a 3-elements-    */	   /* array.						         */           double[][] A = new double[3][4];	   double[] position = new double[3];	   int i,h;           TO6(angles,rob.linkLengths,A);              position[0] = A[0][3] + A[0][2] * rob.linkLengths[3];              position[1] = A[2][2] * rob.linkLengths[3] + A[2][3] 				+ rob.linkLengths[0];              position[2] = -A[1][2] * rob.linkLengths[3] - A[1][3];                for ( i=0 ; i<3 ; i++ )               {                 h = (int)(position[i] * 1000 + 0.5);                 position[i] = h / 1000.0;                   pos[i] = position[i];              }	}	public static int inverseAngle1(double[][] T, double[] angNew, 		AngleDates[] angOld, int reminder, RoboProto rob)	{	   /* 'T' has to be a 3x4-array, 'angNew' consists of 2 elements  */	   /* and 'angOld' of 6. 				 	  */	   /* Returns the new value of reminder.			  */		   double help1, help2, help3; 	   int i;	      	   if ( T[0][3] != 0.0 || T[1][3] != 0.0 )  	     {  	       /* atan2 is calculatable for these values,so */      	       /* two angle values can be determined.       */   	       help1 = Math.atan2( T[1][3] , T[0][3] );	       help2 = Math.sqrt( T[0][3]*T[0][3] + T[1][3]*T[1][3]); 	       help3 = Math.atan2( 0.0 , help2 );   	       angNew[0] = help1 - help3;  	       help3 = Math.atan2( 0.0 , -help2 );  	       angNew[1] = help1 - help3;   	       if ( angNew[0] == 0.0 && rob.angles[0].act <= 0.0 &&     		    angNew[1] >= 0.0 ) 		 angNew[1] = -Math.PI;   	       if ( angNew[1] == 0.0 && rob.angles[0].act < 0.0 &&       		    angNew[0] > 0.0 ) 		 angNew[0] = -Math.PI;  	       for ( i=0 ; i < 2 ; i++ )    		 {     		   if ( angNew[i] < (-Math.PI ))		     angNew[i] = angNew[i] + 2 * Math.PI;                   else {         		  if ( angNew[i] > Math.PI ) 			    angNew[i] = angNew[i] - 2 * Math.PI;         		}         		 }	       return 0;	     }	   else 	     {	       /* Point is on the Y-line. In this case */               /* atan2 is not calculatable.           */       	      angNew[0] = angOld[0].act;       	      if ( angOld[0].act + 1.570796327  <=  (rob.angles[0].max)  &&            	   angOld[0].act + 1.570796327  >= (rob.angles[0].min ) )                 angNew[1] = angOld[0].act  + Math.PI / 2.0;              else angNew[1] = angOld[0].act - Math.PI / 2.0;       	      return 2;    	     }	}	public static int inverseAngle3(double[][] T, double[] angNew, 			double[] linkl, int reminder, RoboProto rob)	{	   /* 'T' has to be a 3x4-array, 'angNew' consists of 2 elements  */	   /* and 'linkl' of 3. 				 	  */	   /* Returns the new value of reminder.			  */		   double help, K, help2; 	   int i;	   K = T[0][3] * T[0][3] + T[1][3] * T[1][3] + T[2][3] * T[2][3];	   K = K - linkl[1] * linkl[1] - linkl[2] * linkl[2]; 	   K = K / ( 2 * linkl[1] ); 	   if ( Math.abs(K) <= 0.000001 ) 	     K = 0.0; 	   help = linkl[2] * linkl[2] - K * K; 	   if ( help > 0 )  	     { 	       help = Math.sqrt( help );  	       help2 = Math.atan2( 0.0 , linkl[2] );   	       angNew[0] = help2 - Math.atan2( K , help );   	       angNew[1] = help2 - Math.atan2( K , (-help) );   	       for (i=0; i < 2; i++)   		 {  		   if ( angNew[i] > rob.angles[2].max ) 		     angNew[i] = angNew[i] - 2 * Math.PI;   		 }   	       return 0; 	     }	   else 	     return 1;  // Calculation not possible.	}	public static int inverseAngle2(double[][] T, double[] angNew, 			double[] linkl, double angle1, double angle3, 		        double[] s, double[] c, int reminder, RoboProto rob)	{	   /* 'T' has to be a 3x4-array, 'angNew' consists of 2 elements  */	   /* and 'linkl' of 3. 's' and 'c' consist of 6 elements.	  */	   /* Returns the new value of reminder.			  */	   double help1,help2; 	   help1 = T[2][3] * T[2][3]; 	   help2 = c[1] * T[0][3] + s[1] * T[1][3]; 	   help1 = help1 + help2 * help2; 	   if ( help1 > 0.0 ) 	     {  	       help1 = (-linkl[1] * c[3]) * T[2][3] +                       ( c[1] * T[0][3] + s[1] * T[1][3] ) *                       ( linkl[1] * s[3] - linkl[2] );   	       if ( help1 == -0.0 )		 help1 = 0.0; 	       help2 = (linkl[1] * s[3] - linkl[2]) * T[2][3] -            	       (-linkl[1] * c[3])*(c[1] * T[0][3] + s[1] * T[1][3]);  	       if ( Math.abs(help1) <= 0.000001) 		 help1 = 0.0;  	       if ( Math.abs(help2) <= 0.000001) 		 help2 = 0.0;	       if ( help1 != 0.0 || help2 != 0.0 ) 	         {   		   angNew[0] = Math.atan2( help1,help2 ) - angle3;   	           if ( angNew[0] > rob.angles[1].max )		     angNew[0] = angNew[0] - 2*Math.PI;   		   if ( Math.abs(angNew[0] - Math.PI) <= 0.000001 ) 	             angNew[0] = -Math.PI;     		   return 0;  	         }  	       else return 1; 	     } 	   else return 1;	}	public static int inverseAngle4(double[][] T, double[] angNew, 	    AngleDates[] angOld, double angle1, double angle2, double angle3,	    double[] s, double[] c, int reminder, RoboProto rob) 	{	   /* 'T' has to be a 3x4-array, 'angNew' consists of 2 elements  */	   /* and 'angOld' of 6. 's' and 'c' consist of 6 elements.	  */	   /* Returns the new value of 'reminder'.			  */	   double help1, help2, help3, help4;	   double c23, s23;           c23 = c[2] * c[3] - s[2] * s[3]; 	   s23 = c[2] * s[3] + s[2] * c[3];	   help1 = (-T[0][2]) * s[1] + T[1][2] * c[1];           help3 = (-T[0][2]) * c[1] * c23 ; 	   help4 = (-T[1][2]) * s[1] * c23 ;	   help2 = help3 + help4 + T[2][2] * s23; 	   if ( Math.abs(help1) <= 0.000001 )	     help1 = 0.0; 	   if ( Math.abs(help2) <= 0.000001 )	     help2 = 0.0; 	   if ( Math.abs(help1) <= 1.000001 && Math.abs(help2) <= 1.000001 &&     	      ( Math.abs(help1) >= 0.01 || Math.abs(help2) >= 0.01))  	     {  	       angNew[0] = Math.atan2( help1,help2 );   	       if ( Math.abs(Math.abs(angNew[0]) - Math.PI) <= 0.000001 )   	         {   	           if ( angOld[3].act < 0.0 && angNew[0] >= 0.0 ) 		     angNew[0] = angNew[0] * (-1);    		   if ( angOld[3].act > 0.0 && angNew[0] < 0.0 )		     angNew[0] = angNew[0] * ( -1);   		 }   	       return 0;  	     }	   else 	     {      	       if ( Math.abs(help1) <= 0.01 && Math.abs(help2) <= 0.01 )       	         { 		   angNew[0] = angOld[3].act;           	   return 5;        	 }      	       else return 1;     	     } 	}	public static int inverseAngle5(double[][] T, double[] angNew,			double a1, double a2, double a3, double a4, double[] s,			double[] c, int reminder, RoboProto rob)	{	   /* 'T' has to be a 3x4-array, 'angNew' consists of 6 elements.  */	   /* 's' and 'c' consist of 6 elements.	                   */	   /* Returns the new value of 'reminder'.	       		   */	   double help1; 	   double help2; 	   double c23, s23; 	   c23 = c[2] * c[3] - s[2] * s[3]; 	   s23 = c[2] * s[3] + s[2] * c[3]; 	   help1 = -(T[0][2] * ( c[1] * c23 * c[4] + s[1] * s[4] ) 		  + T[1][2] *                 ( s[1] * c23 * c[4] - c[1] * s[4] ) - T[2][2] * ( s23 * c[4])); 	   help2 = T[0][2] * ( (-c[1]) * s23 ) + T[1][2] * ( (-s[1]) * s23 ) 	           + T[2][2] * (-c23);	   if ( Math.abs(help1) <= 0.000001 )	     help1 = 0.0; 	   if ( Math.abs(help2) <= 0.000001 )	     help2 = 0.0; 	   if ( Math.abs(help1) <= 1.000001 && Math.abs(help2) <= 1.000001 &&    	        ( help1 != 0.0 || help2 != 0.0)) 	     {  	       angNew[0] = Math.atan2( help1,help2 );   	       return 0;  	     } 	   else return 1;	}	public static int inverseAngle6(double[][] T, AngleDates[] angles,	 	       double[] angNew, double a1, double a2, double a3,		       double a4, double a5, double[] s, double[] c, 		       int reminder, RoboProto rob) 	{	   /* 'T' has to be a 3x4-array, 'angles' consists of 6 elements  */	   /* and 'angNew' of 6. 's' and 'c' consist of 6 elements.	  */	   /* Returns the new value of 'reminder'.			  */	   double help1;