Example usage for org.apache.commons.math.geometry Vector3D add

Introduction

In this page you can find the example usage for org.apache.commons.math.geometry Vector3D add.

Prototype

public Vector3D add(Vector3D v) 

Source Link

Document

Add a vector to the instance.

Usage

From source file:magma.agent.behavior.complex.GetToInitialPosition.java

/**
 * calculates a point on a circle from the own goal to which to go
 *//*from  w  w  w.j a v a2  s.c  om*/
private boolean getCurrentBehavior() {
    IThisPlayer thisPlayer = worldModel.getThisPlayer();
    Vector3D home = thisPlayer.getHomePosition("doesNotMatter");
    Vector3D ourPos = thisPlayer.getPosition();
    Vector3D ownGoal = worldModel.getOwnGoalPosition();
    Vector3D ball = worldModel.getBall().getPosition();
    double distanceX = IServerConfigFilesConstants.FIELD_LENGTH - Math.abs(home.getX());

    // destination position is certain distance from goal in line with ball
    Vector3D goalBallDir = new Vector3D(distanceX, ball.subtract(ownGoal).normalize());
    Vector3D goalBall = ownGoal.add(goalBallDir);

    setPosition((float) goalBall.getX(), (float) goalBall.getY(), (float) goalBall.getAlpha());
    double directionToTarget = thisPlayer.getBodyDirectionTo(position).degrees();

    rotation = (float) directionToTarget;
    float distanceToTarget = (float) thisPlayer.getDistanceToXY(position);

    if (distanceToTarget < DISTANCE_PRECISION) {
        directionToTarget = thisPlayer.getBodyDirectionTo(ball).degrees();
        rotation = (float) directionToTarget;
    }

    logger.log(Level.FINE,
            "initial pos: ({0},{1}) distanceToTarget: {2} directionToTarget: {3} " + "ourpos: ({4},{5})",
            new Object[] { goalBall.getX(), goalBall.getY(), distanceToTarget, directionToTarget, ourPos.getX(),
                    ourPos.getY() });

    if (distanceToTarget < DISTANCE_PRECISION) {
        // we are at the desired position
        // make sure that we are also in desired direction
        if (!turnedLeftBefore && directionToTarget > ANGULAR_PRECISION && directionToTarget <= 90) {
            turnLeft(directionToTarget);
            return true;
        }

        if (!turnedLeftBefore && directionToTarget < -ANGULAR_PRECISION && directionToTarget >= -90) {
            turnRight(directionToTarget - 180);
            return true;
        }

        if (!turnedLeftBefore && directionToTarget > 90 && directionToTarget < (180 - ANGULAR_PRECISION)) {
            turnRight(directionToTarget - 180);
            return true;
        }

        if (!turnedRightBefore && directionToTarget < -90 && directionToTarget > -180 + ANGULAR_PRECISION) {
            turnLeft(180 + directionToTarget);
            return true;
        }
    }

    if (directionToTarget > 50 && directionToTarget < 130) {
        // position is left
        currentBehavior = behaviors.get(IBehavior.STEP_LEFT);
        return true;
    }

    if (directionToTarget < -50 && directionToTarget > -130) {
        // position is right
        currentBehavior = behaviors.get(IBehavior.STEP_RIGHT);
        return true;
    }

    if (directionToTarget > 90 || directionToTarget < -90) {
        // or do we prefer to turn
        if (!turnedLeftBefore && directionToTarget > 0 && directionToTarget < (180 - ANGULAR_PRECISION)) {
            turnRight(directionToTarget - 180);
            return true;
        }

        if (!turnedRightBefore && directionToTarget < -ANGULAR_PRECISION) {
            turnLeft(180 + directionToTarget);
            return true;
        }
        currentBehavior = behaviors.get(IBehavior.STEP_BACKWARD);
        return true;
    }
    return false;
}

---

From source file:magma.agent.behavior.complex.GetInScorePosition.java

private Vector3D calculateAlternativePosition(Vector3D ballPosition, IVisibleObject thisPlayer,
        Vector3D scorePosition) {
    Vector3D intermediatePosition = ballPosition.subtract(thisPlayer.getPosition()).normalize();

    // find out which way is shorter
    Vector3D intermediate1 = new Vector3D(intermediatePosition.getY(), -intermediatePosition.getX(),
            intermediatePosition.getZ());
    Vector3D intermediate2 = new Vector3D(-intermediatePosition.getY(), intermediatePosition.getX(),
            intermediatePosition.getZ());

    intermediate1 = ballPosition.add(new Vector3D(BALL_KEEP_AWAY_DISTANCE, intermediate1));
    intermediate2 = ballPosition.add(new Vector3D(BALL_KEEP_AWAY_DISTANCE, intermediate2));

    float way1 = (float) (thisPlayer.getDistanceTo(intermediate1)
            + scorePosition.subtract(intermediate1).getNorm());
    float way2 = (float) (thisPlayer.getDistanceTo(intermediate2)
            + scorePosition.subtract(intermediate2).getNorm());

    if (way1 < way2) {
        scorePosition = intermediate1;//from   w  ww.ja  v  a 2  s  .  c om
    } else {
        scorePosition = intermediate2;
    }
    return (scorePosition);
}

---