List of usage examples for org.apache.commons.math.geometry Vector3D getY
public double getY()
From source file:at.tuwien.ifs.commons.util.MathUtils.java
/** * Squared euclidean distance in 3D space * NOTE: apache commons math3 has this method already. But for simplicity I implemented it here. * remote it in the future/*from ww w .j av a 2 s.co m*/ * @param v from * @param u to * @return the distance^2 */ public static double euclidean2(Vector3D v, Vector3D u) { double x = (v.getX() - u.getX()); double y = (v.getY() - u.getY()); double z = (v.getZ() - u.getZ()); return (x * x + y * y + z * z); }
From source file:magma.util.geometry.Geometry.java
/** * Checks if the passed 2D point is inside the passed polygon * @param position the point to check (x,y) (it has to be in the same * coordinate system as the points of the polygon * @param polygon corner points 2D of the polygon in clockwise direction * @return true if the passed point is inside the polygon */// www . java 2 s.c o m public static boolean isInsidePolygon(Vector3D position, Vector3D[] polygon) { int index0 = polygon.length - 1; int index1 = 0; double x2 = position.getX(); double y2 = position.getY(); for (int i = 0; i < polygon.length; i++) { double x0 = polygon[index0].getX(); double y0 = polygon[index0].getY(); double x1 = polygon[index1].getX(); double y1 = polygon[index1].getY(); // if points are clockwise then the determinant has to be positive // ----1 | x0 y0 1 | // A = - | x1 y1 1 | // ----2 | x2 y2 1 | double result = 0.5 * (x1 * y2 - y1 * x2 - x0 * y2 + y0 * x2 + x0 * y1 - y0 * x1); if (result > 0) { return false; } index0 = index1; index1++; } return true; }
From source file:magma.util.FuzzyCompare.java
/** * Equal ( == )//from ww w . j ava 2s. c o m * * @param first first vector to compare * @param second second vector to compare * @param range The range within the vector values are equal * @return true if the both values are within the same range */ public static boolean eq(Vector3D first, Vector3D second, float range) { if (first == null && second == null) { return true; } if (first == null || second == null) { return false; } if (!eq((float) first.getX(), (float) second.getX(), range)) { return false; } if (!eq((float) first.getY(), (float) second.getY(), range)) { return false; } if (!eq((float) first.getZ(), (float) second.getZ(), range)) { return false; } return true; }
From source file:magma.agent.believe.impl.LayOnBack.java
@Override public float getTruthValue() { Vector3D gyro = agent.getGyroRate("TorsoGyro").getTranslation(); if (laying() && gyro.getY() > 0.7 && gyro.getX() < 0.4 && gyro.getX() > -0.4) { return 1.0f; }//w w w. j a v a 2 s . c om return 0.0f; }
From source file:magma.agent.believe.impl.LayOnFront.java
@Override public float getTruthValue() { Vector3D gyro = agent.getGyroRate("TorsoGyro").getTranslation(); if (laying() && gyro.getY() < -0.7 && gyro.getX() < 0.4 && gyro.getX() > -0.4) { return 1.0f; }/*w w w . j av a 2s . com*/ return 0.0f; }
From source file:magma.agent.behavior.basic.BeamHomeBehavior.java
@Override public void perform(float intensity) { // the position is based on if we play left to right or right to left Vector3D position = worldModel.getThisPlayer().getHomePosition(worldModel.getPlaymode()); // the z component is the rotation of the player setPos((float) position.getX(), (float) position.getY(), (float) position.getZ()); super.perform(1.0f); // after beaming we init gyro agentModel.getGyroRate(IServerConfigFilesConstants.GYRORATE_NAMES[0]).initialize(); logger.log(Level.FINER, "init gyro"); }
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 ava 2s .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 w w . j a v a2 s . co m*/ } else { scorePosition = intermediate2; } return (scorePosition); }
From source file:magma.agent.agentmodel.impl.GyroRate.java
public void setGyro(Vector3D gyro) { this.gyro = gyro; double x, y, z; x = Math.toRadians(gyro.getX() * 0.02); y = Math.toRadians(gyro.getY() * 0.02); z = Math.toRadians(gyro.getZ() * 0.02); double[] rotationValues = new double[9]; rotationValues[0] = cos(y) * cos(z) + sin(x) * sin(y) * sin(z); rotationValues[1] = sin(z) * cos(x); rotationValues[2] = -sin(y) * cos(z) + sin(x) * cos(y) * sin(z); rotationValues[3] = -cos(y) * sin(z) + sin(x) * sin(y) * cos(z); rotationValues[4] = cos(x) * cos(z); rotationValues[5] = sin(z) * sin(z) + sin(x) * cos(y) * cos(z); rotationValues[6] = cos(x) * sin(y); rotationValues[7] = -sin(x);/*ww w. j a v a2 s . c o m*/ rotationValues[8] = cos(x) * cos(y); Matrix3d M = new Matrix3d(rotationValues); M.mul(identity); // Error correction // N = M / sqrt(MT x M) // N - closest rotation result without errors // M - with errors // MT - transpose of M // sqrt of matrix - done by Cholesky Algorithm in MatrixUtil.java Matrix3d MT = new Matrix3d(M.m00, M.m10, M.m20, M.m01, M.m11, M.m21, M.m02, M.m12, M.m22); MT.mul(M); // call cholesky for sqrt(matrix) Matrix3d sqrt = MatrixUtil.cholesky(MT); sqrt.invert(); M.mul(sqrt); // setting identity as result of error correction identity = M; logger.log(Level.FINER, "gyro update: ({0}, {1}, {2})", new Object[] { identity.m02, identity.m12, identity.m22 }); }
From source file:magma.agent.believe.impl.BeamTime.java
@Override public float getTruthValue() { String playmode = worldModel.getPlaymode(); if (playmode.equalsIgnoreCase(IServerConfigFilesConstants.PLAYMODE_GOAL_RIGHT) || playmode.equalsIgnoreCase(IServerConfigFilesConstants.PLAYMODE_GOAL_LEFT) || playmode.equalsIgnoreCase(IServerConfigFilesConstants.PLAYMODE_BEFORE_KICK_OFF)) { Vector3D position = worldModel.getThisPlayer().getPosition(); Vector3D homePosition = worldModel.getThisPlayer().getHomePosition(playmode); logger.log(Level.FINE, "position: ({0}, {1}) home position: ({2}, {3})", new Object[] { position.getX(), position.getY(), homePosition.getX(), homePosition.getY() }); double distance = position.subtract(homePosition).getNorm(); if (distance > 0.3) { return 1.0f; }// ww w. j av a 2 s .co m } return 0.0f; }