bilinear2d.java

利用JAVA编写的群体机器人局部通讯完成一定得队形控制

                        max_x_limit = in.nval;
                        flags &= ~0x02;
                    }
                    else {
                        throw new FunctionApproximatorException(
                                      "Bilinear2D: " +
                                      "bad file format at " +
                                      "field max_x_limit in " +
                                      filename );
                    }
                }
                else if ( in.sval.equals("min_y_limit") ) {
                    if ( (in.nextToken()==':') &&
                         (in.nextToken()==StreamTokenizer.TT_NUMBER) ) {
                        min_y_limit = in.nval;
                        flags &= ~0x04;
                    }
                    else {
                        throw new FunctionApproximatorException(
                                      "Bilinear2D: " +
                                      "bad file format at " +
                                      "field min_y_limit in " +
                                      filename );
                    }
                }
                else if ( in.sval.equals("max_y_limit") ) {
                    if ( (in.nextToken()==':') &&
                         (in.nextToken()==StreamTokenizer.TT_NUMBER) ) {
                        max_y_limit = in.nval;
                        flags &= ~0x08;
                    }
                    else {
                        throw new FunctionApproximatorException(
                                      "Bilinear2D: " +
                                      "bad file format at " +
                                      "field max_y_limit in " +
                                      filename );
                    }
                }
                else if ( in.sval.equals("npoints_x") ) {
                    if ( (in.nextToken()==':') &&
                         (in.nextToken()==StreamTokenizer.TT_NUMBER) ) {
                        npoints_x = (int)in.nval;
                        flags &= ~0x10;
                    }
                    else {
                        throw new FunctionApproximatorException(
                                      "Bilinear2D: " +
                                      "bad file format at " +
                                      "field npoints_x in " +
                                      filename );
                    }
                }
                else if ( in.sval.equals("npoints_y") ) {
                    if ( (in.nextToken()==':') &&
                         (in.nextToken()==StreamTokenizer.TT_NUMBER) ) {
                        npoints_y = (int)in.nval;
                        flags &= ~0x20;
                    }
                    else {
                        throw new FunctionApproximatorException(
                                      "Bilinear2D: " +
                                      "bad file format at " +
                                      "field npoints_y in " +
                                      filename );
                    }
                }   
            }
            else {
                throw new FunctionApproximatorException(
                              "Bilinear2D: " +
                              "bad file format in "+filename );
            }
        }

        // read table

        double[] tmp[][] = new double[npoints_x][npoints_y][2];

        for( int j=0 ; j<npoints_y ; j++ ) 
            for( int i=0 ; i<npoints_x ; i++ ) {
                if ( in.nextToken()==StreamTokenizer.TT_NUMBER ) 
                    tmp[i][j][0] = in.nval;
                else  
                    throw new FunctionApproximatorException(
                                      "Bilinear2D: " +
                                      "bad file format at point " +
                                      "("+i+","+j+") in "+
                                      filename );
                if ( in.nextToken()!=',' ) 
                    throw new FunctionApproximatorException(
                                      "Bilinear2D: " +
                                      "bad file format at point " +
                                      "("+i+","+j+") in "+
                                      filename );
                if ( in.nextToken()==StreamTokenizer.TT_NUMBER ) 
                    tmp[i][j][1] = in.nval;
                else  
                    throw new FunctionApproximatorException(
                                      "Bilinear2D: " +
                                      "bad file format at point " +
                                      "("+i+","+j+") in "+
                                      filename );
            }

        // store the definition of variable x
    
        this.min_x_limit  = min_x_limit;
        this.max_x_limit  = max_x_limit;
        this.range_x      = max_x_limit - min_x_limit;
        this.npoints_x    = npoints_x;
        this.resolution_x = range_x/(npoints_x-1);

        // store the definition of variable y
    
        this.min_y_limit  = min_y_limit;
        this.max_y_limit  = max_y_limit;
        this.range_y      = max_y_limit - min_y_limit;
        this.npoints_y    = npoints_y;
        this.resolution_y = range_y/(npoints_y-1);

        // store the grid
        
        this.table        = tmp;
    }

    /**
     * Save a definition of this instance in a file.
     *
     * @param      filename   the file name.
     * @exception  IOException                    if an I/O error occurs.
     * @exception  FunctionApproximatorException  if something wrong occurs.
     */
    public void saveDefinition( String filename )
    throws FunctionApproximatorException, IOException {
        FileWriter  file = new FileWriter(filename);
        PrintWriter out  = new PrintWriter( file, true );

        // write the definition of variable x
        
        out.println( "min_x_limit: " + min_x_limit );
        out.println( "max_x_limit: " + max_x_limit );
        out.println( "npoints_x:   " + npoints_x );
        
        // write the definition of variable x
        
        out.println( "min_y_limit: " + min_y_limit );
        out.println( "max_y_limit: " + max_y_limit );
        out.println( "npoints_y:   " + npoints_y );

        // write the definition of the grid

        for( int j=0 ; j<npoints_y ; j++ ) {
            out.print("   ");
            for( int i=0 ; i<npoints_x ; i++ ) {
                out.print(table[i][j][0]+","+table[i][j][1]+"   ");
            }
            out.println();
        }
    }
    
    /**
     * Computes and returns the bilinear interpolation associated with the
     * point. The point is clipped to the domain rectangle.
     *
     * @param     point  the input point (two-dimensional array of doubles).
     * @return    the output point (two-dimensional array of doubles).
     */
    public double[] query( double[] point ) {

        double[] p_ll, p_lr, p_ul, p_ur;
        int      Ix1, Ix2, Iy1, Iy2;
        double   val_x, val_y;
        double   v_x, v_y;

        // compute index for variable x
    
        val_x = Math.min( max_x_limit, Math.max( point[0], min_x_limit ) );
        Ix1   = (int)Math.floor( (val_x - min_x_limit) / resolution_x );
        v_x   = Math.IEEEremainder( val_x - min_x_limit, resolution_x ) /
                resolution_x;
        if ( v_x<0 )
            v_x = 1+v_x;
        Ix2   = Ix1 + 1;
        if ( Ix2==npoints_x )
            Ix2--;
     
        // compute index for variable y
    
        val_y = Math.min( max_y_limit, Math.max( point[1], min_y_limit ) );
        Iy1   = (int)Math.floor( (val_y - min_y_limit) / resolution_y );
        v_y   = Math.IEEEremainder( val_y - min_y_limit, resolution_y ) /
                resolution_y;
        if ( v_y<0 )
            v_y = 1+v_y;
        Iy2   = Iy1 + 1;
        if ( Iy2==npoints_y )
            Iy2--;

        // retrieve the four neighbors

        p_ll = table[Ix1][Iy1];
        p_lr = table[Ix2][Iy1];
        p_ul = table[Ix1][Iy2];
        p_ur = table[Ix2][Iy2];

        // compute the result

        double[] result = new double[2];
        
        result[0] = p_ll[0] + 
                   (p_lr[0]-p_ll[0])*v_x + 
                   (p_ul[0]-p_ll[0])*v_y + 
                   (p_ur[0]-p_ul[0]-p_lr[0]+p_ll[0])*v_x*v_y;

        result[1] = p_ll[1] + 
                   (p_lr[1]-p_ll[1])*v_x + 
                   (p_ul[1]-p_ll[1])*v_y + 
                   (p_ur[1]-p_ul[1]-p_lr[1]+p_ll[1])*v_x*v_y;

        return result;
    }

    /**
     * Computes and returns the bilinear interpolation associated with the
     * point. The point is clipped to the domain rectangle.
     *
     * @param     x   the x ordinate of the input point.
     * @param     y   the y ordinate of the input point.
     * @return    the output point (two-dimensional array of doubles).
     */
    public double[] query( int x, int y ) {
        double[] point = new double[2];
        point[0] = x;
        point[1] = y;
        return query( point );
    }

    /**
     * Not implemented.
     *
     * @exception FunctionApproximatorException  always.
     */
    public void update( double[] q, double[] p )
    throws FunctionApproximatorException {
        throw new FunctionApproximatorException( "update: not implemented" );
    }
}