double 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][0])*( c[1]*c23*s[4] - s[1]*c[4]); 	   help3 = T[1][0]*( s[1]*c23*s[4] + c[1]*c[4]); 	   help4 = T[2][0]*( s23*s[4]); 	   help1 = help1 - help3 + help4; 	   help2 = T[0][0]*((c[1]*c23*c[4] + s[1]*s[4])*c[5] - c[1]*s23*s[5]) +                   T[1][0]*((s[1]*c23*c[4]-c[1]*s[4])*c[5] - s[1]*s23*s[5]) -                   T[2][0]*(s23*c[4]*c[5] + c23*s[5]);           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 );	       /* **************************************************** */   	       if ( Math.abs(Math.abs(angNew[0]) - Math.PI ) < 0.000001 )      		 {      	 	   if ( angles[5].act < 0.0 && angNew[0] >= 0.0 ) 		     angNew[0] = -angNew[0];    		   if ( angles[5].act > 0.0 && angNew[0] < 0.0 )		     angNew[0] = -angNew[0];     		  }	       /* **************************************************** */           	       if ( angNew[0] < (-Math.PI ))	         angNew[0] = angNew[0] + 2 * Math.PI;    	       else 		 {	           if ( angNew[0] > Math.PI )		     angNew[0] = angNew[0] - 2 * Math.PI;         	 }     	       return 0; 	     } 	   else return 1;	}	public static int inverseCinematic(double[][] T, double[] angNew,			AngleDates[] angOld, double[] linkl, int result,			int f_a_g, RoboProto rob)	{	   /* 'T' has to be a 3x4-array and is frame 6 relatively to     */	   /* frame 0. 'angNew' consists of 6 elements as 'angOld' does. */	   /* 'linkl' has got 3 elements.				 */	   /* 'angOld' are the actual angles and 'linkl' the actual link */	   /* lengths. If result is 0 then a valid combination of angles */	   /* has been found.						 */	   /* Returns the new value of 'result'.             		 */	   double[][] A = new double[3][4]; 	   double angMax,angHelp; 	   int number; 	   AngleDates[] w = { new AngleDates(0.0, 0.0, 0.0),			      new AngleDates(0.0, 0.0, 0.0),  			      new AngleDates(0.0, 0.0, 0.0),			      new AngleDates(0.0, 0.0, 0.0),  			      new AngleDates(0.0, 0.0, 0.0),			      new AngleDates(0.0, 0.0, 0.0) };  	   double[] s = new double[6], c = new double[6]; 	   double zw,help;	   double[] angle = new double[2]; 	   int i,j,k, reminder;      		      /* reminder = 0 ... result is OK.                         reminder = 1 ... no result.                         reminder = 2 ... the result has to be checked.                         reminder = 5 ... set angle 5  to 0.        */      	   double[][] w_tab = new double[8][7]; 		      // w_tab[i][0] = 0 ... Angle combination is OK.                       // w_tab[i][0] = 1 ... Angle combination is not OK.                      // w_tab[i][0] = 2 ... Combination has to be checked. 	   result = 1;	   reminder = 300; // stupid, only for the syntax needed default value 	   reminder = inverseAngle1 ( T,angle,angOld,reminder, rob ); 	   for ( i=0 ; i < 2 ; i++ )	     {	       w_tab[i][0] = reminder;               w_tab[i][1] = angle[0];             } 	   for ( i=2 ; i < 4 ; i++ )	     {	       w_tab[i][0] = reminder;               w_tab[i][1] = angle[1];             }              	   reminder = inverseAngle3 ( T,angle,linkl,reminder, rob ); 	   if ( reminder == 0 ) 	     {   	       /* It exists a solution for angle 3 */   	       for ( i=0 ; i < 2 ; i++ ) 	         {		   w_tab[i*2][3] = angle[0];                   w_tab[i*2+1][3] = angle[1];                 }  	       for ( i=0 ; i < 4 ; i++ ) 	         {	           s[1] = Math.sin ( w_tab[i][1] ); 	           c[1] = Math.cos ( w_tab[i][1] );   	  	   s[3] = Math.sin ( w_tab[i][3] );   		   c[3] = Math.cos ( w_tab[i][3] );    		   reminder = inverseAngle2 (T,angle,linkl,w_tab[i][1],					 w_tab[i][3], s,c,reminder, rob );   		   if ( reminder == 1 ) 	             w_tab[i][0] = 1.0 ;      		   else      		     {  		       /* A solution for angle 2 exists, that means angle 4         	          can now be calculated.                           */        		       w_tab[i][2] = angle[0];     		       s[2] = Math.sin ( angle[0] );     		       c[2] = Math.cos ( angle[0] );     		       reminder = inverseAngle4 ( T,angle,angOld,w_tab[i][1],				   w_tab[i][2], w_tab[i][3], s,c,reminder,rob);    		       if ( reminder == 1 ) 			 w_tab[i][0] = 1.0;       		       else  		         { 			    /* A solution for angle 4 exists, that means 			       angle 5 can now be calculated.		 */        		    s[4] = Math.sin ( angle[0] );     			    c[4] = Math.cos ( angle[0] );      			    w_tab[i][4] = angle[0];      			    if ( reminder == 5 )          		      {           			w_tab[i][0] = 2.0;         		        w_tab[i][5] = 0.0;       			        s[5] = 0.0;       			        c[5] = 1.0;         		      }  /* if .. reminder == 5 */       			    else       			      { 			        /* Calculation of angle 5 */       			        reminder = inverseAngle5(T,angle,w_tab[i][1],					    w_tab[i][2],w_tab[i][3], 					    w_tab[i][4],s,c,reminder, rob );           			if ( reminder == 1 ) 				  w_tab[i][0] = 1.0;            			else 				  { 				    w_tab[i][5] = angle[0];                		    s[5] = Math.sin ( w_tab[i][5] );                		    c[5] = Math.cos ( w_tab[i][5] );              			  }         		      }  /* else .. reminder == 5 */       			    if ( w_tab[i][0] != 1.0 )         		      {  				/* Calculation of angle 6 */          		        reminder = inverseAngle6(T,rob.angles,angle,				    w_tab[i][1], w_tab[i][2],w_tab[i][3],                                    w_tab[i][4],w_tab[i][5],s,c,reminder, rob);    	                        if ( reminder == 1 )			          w_tab[i][0] = 1.0 ;          		        else      			          {      			            w_tab[i][6] = angle[0];                      		            /* Copying of the angle values: */         		            w_tab[i+4][0] = w_tab[i][0];       		                    w_tab[i+4][1] = w_tab[i][1];        		            w_tab[i+4][2] = w_tab[i][2];          		            w_tab[i+4][3] = w_tab[i][3];              			    if ( Math.abs(w_tab[i][4]) == 0.0 )               			      {              			        if ( angOld[3].act >= 0.0 ) 					  w_tab[i+4][4] = w_tab[i][4] +Math.PI;               				else w_tab[i+4][4] = w_tab[i][4] 							     - Math.PI;             			      }               			    else              		              { 			 	 	if (w_tab[i][4] > 0.0) 					  w_tab[i+4][4] = w_tab[i][4] -Math.PI;                			else w_tab[i+4][4] = w_tab[i][4] 							     + Math.PI;             			      }             			    w_tab[i+4][5] = -w_tab[i][5];                            			    if ( Math.abs(w_tab[i][6]) == 0.0 )              			      {             			        if ( angOld[5].act >= 0.0 ) 					  w_tab[i+4][6] = w_tab[i][6] +Math.PI;              				else w_tab[i+4][6] = w_tab[i][6] 							     - Math.PI;             			      }              			    else            			      { 				        if (w_tab[i][6] > 0.0) 					  w_tab[i+4][6] = w_tab[i][6] -Math.PI;              			        else w_tab[i+4][6] = w_tab[i][6] 							     + Math.PI;            			      }             		          } /* else .. reminder W6 */           		      } /* if ... w_tab[i][0] == 0 */       		          }  /* else .. reminder Angle 4 */      		      }    /* else .. reminder Angle 2 */	          } /* endfor  */  		/* Determination of the best angle solution */                      	        result = 1;	        zw = 10000;		for ( i=0 ; i < 8 ; i++ ) 		  {  		    if ( w_tab[i][0] != 1.0)   		      {   		        reminder = 0;   		        for ( j=0 ; j < 6 ; j++ )    			  {   			    w[j].act = w_tab[i][j+1];    			    if ( w_tab[i][j+1] < rob.angles[j].min ||    		                 w_tab[i][j+1] > rob.angles[j].max )                               {                                w_tab[i][0] = 1.0;                                reminder = 1;                                break;                              }   		 	  } /* endfor */   		        if ( reminder == 0 )     			  {     			    TO6 ( w,linkl,A );     			    for ( j=0 ; j < 3 ; j++ )     			      {      			        if ( Math.abs(T[j][0] - A[j][0]) > 0.01 ||       			             Math.abs(T[j][1] - A[j][1]) > 0.01 ||         		             Math.abs(T[j][2] - A[j][2]) > 0.01 ||          		             Math.abs(T[j][3] - A[j][3]) > 0.1     )				  {                                     w_tab[i][0] = 1.0;                                      reminder = 1;                                    break;                                  }      			      }      		 	    if ( reminder == 0 )       			      {              		        // This combination of angles is allowed				// and correct.           			// Calculation of the target function.       			        help = 0;         		        for ( j=0 ; j < 6 ; j++ )         			  help = help + rob.angleWeights[j] * 				     Math.abs( w_tab[i][j+1] - angOld[j].act );         			if ( zw > help ) 				  {                          	    zw = help;                          	    for ( j=0 ; j < 6 ; j++ )                             	      angNew[j] = w_tab[i][j+1];                            	  }           		        result = 0;      			      }     			  }                      } /* end if */                  }  /* end for */                /* If it is the case that f_auto_gerade is 0 ,      	 	   it will be tested, if there exists a better solution. */ 	        if ( f_a_g == 0 )  		  {   		    angMax = Math.abs( angOld[0].act  - angNew[0] );   		    for ( i=1; i < 6; i++ )    		      {   			if (angMax < (Math.abs( angOld[i].act - angNew[i] ) )) 			  angMax = Math.abs( angOld[i].act - angNew[i] );  		      }   		    for ( i=0; i < 8; i++ )   		      {    		        if ( w_tab[i][0] != 1 )       		          {       			    angHelp = Math.abs( angOld[0].act - w_tab[i][1] );    		            for ( j=2; j < 7; j++ )     			      { if ((i > 0) && (i < 7))// meine Einschraenkung     			        if (angHelp < 				  (Math.abs(angOld[i-1].act - w_tab[i][j]))) 			          angHelp = Math.abs(					       angOld[i-1].act - w_tab[i][j] );    		              }           		            if ( angMax > angHelp )     			      {      			        angMax = angHelp;      			        for ( j=0; j < 6; j++ ) 			          angNew[j] = w_tab[i][j+1];   		              }    		          }    		      }                      }                } /* if reminder == 0) ... */ 	   return result;	} /* end of method: 'inverseCinematic' */	}