aikhomog.java

一个多机器人的仿真平台

package Domains.SoccerBots.teams;

import java.util.Vector;
import EDU.gatech.cc.is.util.Vec2;

import EDU.gatech.cc.is.abstractrobot.*;

/**
 * A homogeneous robot soccer team.
 *

 * @author Håkan L. Younes
 */
public class AIKHomoG extends ControlSystemSS {
    private static final double FIELD_WIDTH = 1.525;

    private static final double FIELD_LENGTH = 2.74;

    private static final double GOAL_WIDTH = 0.5;


    private static final double MARGIN = 0.02;

    private static final double RANGE = 0.3;

    private static final double TEAMMATE_G = 1.0;

    private static final double WALL_G = 1.0;

    private static final double GOALIE_G = 2.0;

    private static final double FORCE_LIMIT = 1.0;


    /**
     * @return the cross product of the two given vectors.
     */
    protected static double cross(Vec2 v, Vec2 u) {
        return v.x * u.y - v.y * u.x;
    }

    /**
     * @return the dot product of the two given vectors.
     */

    protected static double dot(Vec2 v, Vec2 u) {
        return v.x * u.x + v.y * u.y;
    }


    /**
     * @return the angle, in the range 0 through <I>pi</I>, between
     * the two given vectors.
     */
    protected static double angle(Vec2 v, Vec2 u) {
        return Math.acos(dot(v, u) / (v.r * u.r));
    }


    /**
     * @return the angle, in the range 0 through <I>pi</I>, between
     * the two given angles.
     */
    protected static double angle(double alpha, double beta) {
        return Math.acos(Math.cos(alpha - beta));
    }

    /**
     * @return the given angle converted to degrees.
     */
    protected static int rad2deg(double alpha) {
        return (int)(180.0 * alpha / Math.PI);
    }


    private int side;
    private double forward_angle;
    private double goalie_x;
    private Vec2 offensive_pos1, offensive_pos2;

    public void Configure() {
        Vec2 goal = abstract_robot.getOpponentsGoal(0L);
        if (goal.x > 0.0) {
            side = -1;
            forward_angle = 0.0;
            goalie_x = -FIELD_LENGTH / 2 + abstract_robot.RADIUS;
            offensive_pos1 = new Vec2(4.0 * FIELD_LENGTH / 10.0,
                                      GOAL_WIDTH / 2.0
                                      + abstract_robot.RADIUS);
            offensive_pos2 = new Vec2(4.0 * FIELD_LENGTH / 10.0,
                                      -GOAL_WIDTH / 2.0
                                      - abstract_robot.RADIUS);
        } else {
            side = 1;
            forward_angle = Math.PI;
            goalie_x = FIELD_LENGTH / 2 - abstract_robot.RADIUS;
            offensive_pos1 = new Vec2(-4.0 * FIELD_LENGTH / 10.0,
                                      GOAL_WIDTH / 2.0
                                      + abstract_robot.RADIUS);
            offensive_pos2 = new Vec2(-4.0 * FIELD_LENGTH / 10.0,
                                      -GOAL_WIDTH / 2.0
                                      - abstract_robot.RADIUS);
        }
    }

    private boolean isBehind(double x1, double x2) {
        if (side < 0) {
            return x1 < x2;
        } else {
            return x2 < x1;
        }
    }

    public int TakeStep() {
        long time = abstract_robot.getTime();
        Vec2 ball = abstract_robot.getBall(time);
        if (!isClosestToBall(ball, time)) {
            Vec2 f = getForce(time);

            if (f.r < FORCE_LIMIT) {
                abstract_robot.setSteerHeading(time, ball.t);
                abstract_robot.setSpeed(time, 0.0);
                abstract_robot.setDisplayString("tracking ball");
            } else {
                abstract_robot.setSteerHeading(time, getFreeDirection(f, RANGE,
                                                                      time));
                abstract_robot.setSpeed(time, 0.5);
                abstract_robot.setDisplayString("force: "
                                                + (int)(100.0 * f.r)
                                                + ", " + rad2deg(f.t));
            }
            return CSSTAT_OK;
        }
        Vec2 oppGoal = abstract_robot.getOpponentsGoal(time);
        Vec2 freeGoalDrive = new Vec2(oppGoal);
        freeGoalDrive.sett(getFreeDirection(oppGoal, oppGoal.r, time));
        boolean kick = false;
        if (angle(freeGoalDrive.t, forward_angle) > Math.PI / 6.0) {
            kick = (angle(abstract_robot.getSteerHeading(time), forward_angle)
                    < Math.PI / 2.0);
        } else {
            oppGoal = freeGoalDrive;
        }
        Vec2 goal = getKickPosition(ball,
                                    oppGoal,
                                    time);
        if (!abstract_robot.canKick(time)) {
            double dir = getFreeDirection(goal, RANGE, time);
            abstract_robot.setSteerHeading(time, dir);
            abstract_robot.setSpeed(time, 1.0);
        } else {
            if (angle(freeGoalDrive.t, ball.t) > Math.PI / 12) {
                abstract_robot.setSteerHeading(time, -freeGoalDrive.t);
            } else {
                Vec2 pos = getKickPosition(ball, freeGoalDrive, time);
                abstract_robot.setSteerHeading(time, pos.t);
                if (true || kick) {
                    abstract_robot.kick(time);
                }
            }
            abstract_robot.setSpeed(time, 1.0);
        }
        return CSSTAT_OK;
    }

    private boolean isClosestToBall(Vec2 ball, long time) {
        Vec2[] mates = abstract_robot.getTeammates(time);
        for (int i = 0; i < mates.length; i++) {
            mates[i] = new Vec2(mates[i]);
            mates[i].setr(mates[i].r + abstract_robot.RADIUS);
            Vec2 diff = new Vec2(ball);
            diff.sub(mates[i]);
            if (isBehind(mates[i].x, ball.x) && isBehind(ball.x, 0.0)
                    && diff.r < RANGE) {
                return false;
            }
            if (ball.r >= RANGE && diff.r + MARGIN < ball.r) {
                return false;
            }
        }
        return true;
    }

    private Vec2 getKickPosition(Vec2 ball, Vec2 target, long time) {
        Vec2 v = new Vec2(target);
        v.sub(ball);
        double alpha = v.t + Math.PI;
        v = new Vec2((1.0/Math.sqrt(2)) * abstract_robot.RADIUS
                     * Math.cos(alpha),
                     (1.0/Math.sqrt(2)) * abstract_robot.RADIUS
                     * Math.sin(alpha));
        alpha = angle(ball, v);
        if (alpha < Math.PI / 2.0) {
            if (cross(ball, v) > 0.0) {
                v.sett(v.t + (Math.PI / 2.0 - alpha));
            } else {
                v.sett(v.t - (Math.PI / 2.0 - alpha));
            }
        }
        v.add(ball);
        return v;
    }

    // Should also consider walls!!!
    private double getFreeDirection(Vec2 goal, double range, long time) {
        ObstacleList obstacles = new ObstacleList();
        for (int k = 0; k < 2; k++) {
            Vec2[] ps = (k == 0) ? abstract_robot.getOpponents(time) :
                                    abstract_robot.getTeammates(time);
            Vec2[] players = new Vec2[ps.length];
            for (int i = 0; i < players.length; i++) {
                players[i] = new Vec2(ps[i]);
                players[i].setr(players[i].r + abstract_robot.RADIUS);
                Vec2 diff = new Vec2(goal);
                diff.sub(players[i]);
                if ((players[i].r < 2 * abstract_robot.RADIUS + MARGIN)
                        || (players[i].r < range
                            && players[i].r < goal.r + abstract_robot.RADIUS
                            && diff.r < goal.r)) {
                    obstacles.add(new Obstacle(goal, players[i],
                                               abstract_robot.RADIUS,
                                               abstract_robot.RADIUS));
                }
            }

        }
        double dir = goal.t;
        if (obstacles.size() > 0) {
            Obstacle bound = obstacles.getBoundaries();
            if (bound.obscures(dir)) {
                for (int i = 0; i < obstacles.size(); i++) {
                    Obstacle o = obstacles.get(0);
                    if (o.obscures(dir)) {
                        if (angle(dir, o.getLeft())
                                < angle(o.getRight(), dir)) {
                            dir = o.getLeft();
                        } else {
                            dir = o.getRight();
                        }
                        break;
                    }
                }
            }
        }
        StringBuffer sb = new StringBuffer();
        sb.append(rad2deg(goal.t));
        sb.append(' ').append(obstacles);
        sb.append(" -> ").append(rad2deg(dir));
        abstract_robot.setDisplayString(sb.toString());
        return dir;
    }

    private Vec2 getForce(long time) {
        Vec2 pos = abstract_robot.getPosition(time);
        Vec2 ball = abstract_robot.getBall(time);
        Vec2 f = new Vec2(0, 0);
        /* add negative force for teammates */
        Vec2[] teammates = abstract_robot.getTeammates(time);
        for (int i = 0; i < teammates.length; i++) {
            Vec2 p = new Vec2(teammates[i]);
            p.setr(p.r + abstract_robot.RADIUS);
            p.rotate(Math.PI);
            p.setr(TEAMMATE_G / (p.r * p.r));
            f.add(p);
        }
        /* add negative force for walls on the long sides */
        double r1 = FIELD_WIDTH / 2 - pos.y;
        double r2 = FIELD_WIDTH / 2 + pos.y;
        Vec2 w = new Vec2(0.0, WALL_G / (r2 * r2) - WALL_G / (r1 * r1));
        f.add(w);
        /* add negative force for walls on the short sides */
        r1 = FIELD_LENGTH / 2 - pos.x;
        r2 = FIELD_LENGTH / 2 + pos.x;
        w = new Vec2(WALL_G / (r2 * r2) - WALL_G / (r1 * r1), 0.0);
        f.add(w);
        /* add positive force for goalie position */