aikhomog.java

一个多机器人的仿真平台

        Vec2 gp = new Vec2(goalie_x, 0.0);
        gp.sub(pos);
        Vec2 gpb = new Vec2(ball);
        gpb.sub(gp);
        double k = (isBehind(gpb.x, 0.0) ?
                    ((gpb.y < 0.0) ? -1.0 : 1.0) : Math.sin(gpb.t));
        gp = new Vec2(goalie_x, k * GOAL_WIDTH / 2);
        gp.sub(pos);
        /* check if I'm already acting goalie */
        if (gp.r < MARGIN) {
            return new Vec2(0.0, 0.0);
        }
        boolean goalie = false;
        /* check if teammates is in goalie position */
        for (int i = 0; i < teammates.length && !goalie; i++) {
            Vec2 p = new Vec2(teammates[i]);
            p.setr(p.r + abstract_robot.RADIUS);
            p.sub(gp);
            if (p.r < MARGIN) {
                goalie = true;
            }
        }
        if (!goalie) {
            gp.setr(GOALIE_G / (gp.r * gp.r));
            f.add(gp);
        }
        /* add positive force for offensive positions */
        Vec2 rf = getRoleForce(offensive_pos1, GOALIE_G, time);
        if (rf == null) {
            return new Vec2(0.0, 0.0);
        }
        f.add(rf);
        rf = getRoleForce(offensive_pos2, GOALIE_G, time);
        if (rf == null) {
            return new Vec2(0.0, 0.0);
        }
        f.add(rf);

        return f;
    }

    private Vec2 getRoleForce(Vec2 rolePos, double roleG, long time) {
        Vec2 pos = abstract_robot.getPosition(time);
        Vec2[] teammates = abstract_robot.getTeammates(time);
        Vec2 rp = new Vec2(rolePos);
        rp.sub(pos);
        /* check if I'm already acting in this role */
        if (rp.r < MARGIN) {

            return null;
        }
        boolean roleFilled = false;
        /* check if teammate is acting in this role */
        for (int i = 0; i < teammates.length && !roleFilled; i++) {
            Vec2 p = new Vec2(teammates[i]);
            p.setr(p.r + abstract_robot.RADIUS);
            p.sub(rp);
            if (p.r < MARGIN) {
                roleFilled = true;
            }
        }
        if (!roleFilled) {
            rp.setr(roleG / (rp.r * rp.r));
            return rp;
        }


        return new Vec2(0.0, 0.0);
    }

    /**
     * An obstacle, modelled as a left and right angle.
     */
    class Obstacle extends Object {
        /**
         * The left angle.
         */
        private double left;

        /**
         * The right angle.
         */
        private double right;



        /**
         * Creates an obstacle with the given boundaries.
         *
         * @param left left angle given in radians.
         * @param right right anfle given in radians.
         */
        protected Obstacle(double left, double right) {
            this.left = left;
            this.right = right;
        }

        protected Obstacle(Vec2 g, Vec2 p, double r, double ownR) {
            double cp = cross(g, p);
            double d = ownR + r + MARGIN;
            double t = g.t;
            if (cp >= 0.0) {
                if (p.r <= d) {
                    t = p.t - 1.1 * Math.PI / 2.0;
                } else {
                    t -= Math.PI / 2.0;
                }
            } else {
                if (p.r <= d) {
                    t = p.t + 1.1 * Math.PI / 2.0;
                } else {
                    t += Math.PI / 2.0;
                }
            }
            Vec2 v1 = new Vec2(d * Math.cos(t), d * Math.sin(t));
            if (p.r > d) {
                v1.add(p);
            }
            if (p.r <= d) {
                if (cp >= 0) {
                    t = p.t + 1.1 * Math.PI / 2.0;
                } else {
                    t = p.t - 1.1 * Math.PI / 2.0;
                }
            } else {
                t = g.t + Math.PI;
            }
            Vec2 v2 = new Vec2(d * Math.cos(t), d * Math.sin(t));
            if (p.r > d) {
                v2.add(p);
            }
            if (cp >= 0.0) {
                left = v2.t;
                right = v1.t;
            } else {
                left = v1.t;
                right = v2.t;
            }
        }


        /**
         * @return the left boundary, in radians, of this obstacle.
         */
        public double getLeft() {
            return left;
        }

        /**
         * @return the right boundary, in radians, of this obstacle.
         */
        public double getRight() {
            return right;
        }

        /**
         * @param alpha an angle to check given in radians.
         *
         * @return <CODE>true</CODE> if this obstacle obscures the
         * given angle; <CODE>false</CODE> otherwise.
         */
        protected boolean obscures(double alpha) {
            if (left * right < 0.0) {
                if (left > 0.0) {
                    return left > alpha && alpha > right;
                } else {
                    return left > alpha || alpha > right;
                }
            } else if (left > right) {

                return left > alpha && alpha > right;
            } else {
                return left > alpha || alpha > right;
            }
        }

        /**
         * @param o an obstacle to compare with.
         *
         * @return -1 if this obstacle is completely to the left of
         * <CODE>o</CODE>, 1 if it is completely to the right, and 0
         * otherwise.
         */
        protected int compare(Obstacle o) {
            if (obscures(o.left) || obscures(o.right)
                    || o.obscures(left) || o.obscures(right)) {
                return 0;
            } else {
                return (angle(left, o.right) < angle(right, o.left)) ? 1 : -1;
            }
        }

        /**
         * Merges this obstacle with the given obstacle.
         *
         * @param o obstacle to merge with.
         */
        protected void merge(Obstacle o) {
            if (o.obscures(left)) {
                left = o.left;
            }
            if (o.obscures(right)) {
                right = o.right;
            }
        }

        /**
         * @return a string representation of this object.
         */
        public String toString() {
            return ("[" + rad2deg(left) + "," + rad2deg(right) + "]");
        }
    }

    class ObstacleList extends Object {
        private Vector obstacles;

        protected ObstacleList() {
            obstacles = new Vector();

        }

        protected int size() {
            return obstacles.size();
        }

        protected Obstacle get(int i) {
            return (Obstacle) obstacles.elementAt(i);
        }

        protected Obstacle getBoundaries() {
            return new Obstacle(get(0).getLeft(), get(size() - 1).getRight());
        }

        protected void add(Obstacle o) {
            if (obstacles.isEmpty()) {
                obstacles.addElement(o);
            } else {
                for (int i = obstacles.size() - 1; i >= 0; i--) {
                    Obstacle tmp = (Obstacle) obstacles.elementAt(i);

                    int c = o.compare(tmp);
                    if (c < 0) {
                        obstacles.insertElementAt(tmp, i + 1);
                        if (i == 0) {
                            obstacles.setElementAt(o, 0);
                        }
                    } else if (c > 0) {
                        obstacles.insertElementAt(o, i + 1);
                        break;
                    } else {
                        tmp.merge(o);
                        if (i > 0) {
                            obstacles.removeElement(tmp);
                            o = tmp;
                        }
                    }
                }
            }
        }

        public String toString() {
            StringBuffer sb = new StringBuffer();
            sb.append('{');
            if (size() > 0) {
                sb.append(get(0));
                for (int i = 1; i < size(); i++) {
                    sb.append(' ').append(get(i));
                }
            }
            sb.append('}');
            return sb.toString();
        }
    }
}