movemethods.java

一个由c转成java的3D robot 仿真平台

/************************************************************************/
/* 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 (rob.robotIsReal && rob.makeRealRobotMove)
		 {
	           for (int kl = 0; kl < 3; kl++)
		     rob.realRobotInterface(kl, angle[kl].act);
		   rob.realRobotInterface(3, angle[4].act);
	         }
        
   	      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;
	}
}