List of usage examples for org.apache.commons.math.geometry Vector3D Vector3D
public Vector3D(double x, double y, double z)
From source file:magma.agent.behavior.complex.MoveWithBall.java
private Vector3D calculateNewXY() { double newY = 0; double slope; float maxForwardGoalie; float maxForwardPlayer; double goalieDistanceWithBall; double playerDistanceWithBall; if (leftSide) { maxForwardGoalie = -5.0f;//from w w w. java2 s . co m maxForwardPlayer = -2.0f; goalieDistanceWithBall = -5.0; playerDistanceWithBall = -2.0; } // right side else { maxForwardGoalie = 5.0f; maxForwardPlayer = 2.0f; goalieDistanceWithBall = 5.0; playerDistanceWithBall = 2.0; } goalCentre = worldModel.getOwnGoalPosition(); // straight line equation slope = ballPosition.getY() / (ballPosition.getX() - goalCentre.getX()); newY = slope * thisPlayer.getPosition().getX() - slope * goalCentre.getX(); double newX = 0; if (thisPlayer.getID() == 1) { if (FuzzyCompare.eq((float) thisPlayer.getPosition().getX(), maxForwardGoalie, 0.1f)) // so that goalie doesn't leave the // penalty rectangle. newX = thisPlayer.getPosition().getX(); else newX = ballPosition.getX() + goalieDistanceWithBall; } else if (thisPlayer.getID() == 2) { if (FuzzyCompare.eq((float) thisPlayer.getPosition().getX(), maxForwardPlayer, 0.1f))// so that player doesn't leave our half. newX = ballPosition.getX() + playerDistanceWithBall; else newX = ballPosition.getX() + 2.0; } return new Vector3D(newX, newY, 0); }
From source file:magma.agent.worldmodel.impl.VisibleObject.java
/** * @param other the point to which to calculate * @return the distance projection to the x y plane between this object and * the passed coordinate/*from w w w . ja v a2s. co m*/ */ public double getDistanceToXY(Vector3D other) { Vector3D delta = getPosition().subtract(other); return new Vector3D(delta.getX(), delta.getY(), 0).getNorm(); }
From source file:magma.agent.agentmodel.impl.AgentModel.java
/** * updates all known ForceResistances from perception * @param perception the result from server message parsing *//*from w w w. j a v a 2 s. c om*/ protected void updateForceResistance(IPerception perception) { Vector3D force; Vector3D forceOrigin; for (int i = 0; i < IServerConfigFilesConstants.FORCERESISTANCE_NAMES.length; i++) { // Fetch new Force Values force = perception .getForceResistancePerceptor(IServerConfigFilesConstants.FORCERESISTANCE_PERCEPTORS[i]) .getForce(); forceOrigin = perception .getForceResistancePerceptor(IServerConfigFilesConstants.FORCERESISTANCE_PERCEPTORS[i]) .getForceOrigin(); // Set new Force Values getForceResistance(IServerConfigFilesConstants.FORCERESISTANCE_NAMES[i]) .setForce(new Vector3D(force.getX(), force.getY(), force.getZ())); getForceResistance(IServerConfigFilesConstants.FORCERESISTANCE_NAMES[i]) .setForceOrigin(new Vector3D(forceOrigin.getX(), forceOrigin.getY(), forceOrigin.getZ())); } }
From source file:magma.agent.worldmodel.localizer.impl.LocalizerTriangulationTest.java
@Test public void testLocalizeGoalPosts() { expect(G1R.getDistance()).andStubReturn(6.22); expect(G1R.getHorizontalDirection()).andStubReturn(toRadians(15.94)); expect(F2R.getDistance()).andStubReturn(7.35); expect(F2R.getHorizontalDirection()).andStubReturn(toRadians(-23.67)); expect(G1R.compareTo(F2R)).andStubReturn(-1); replay(G1R, F2R);//from w w w . ja va2 s . c o m flags.put("G1R", G1R); flags.put("F2R", F2R); PositionOrientation localizeInfo = testee.localize(flags, 0, 0, Vector3D.zero); assertNotNull(localizeInfo); assertTrue(FuzzyCompare.eq(new Vector3D(-0.178, -0.0187, 0), localizeInfo.getPosition(), 0.01f)); assertEquals(-0.162, localizeInfo.getOrientationX().radians(), 0.001); }
From source file:magma.agent.worldmodel.impl.VisibleObjectTest.java
/** * Test method for// w ww .j ava2 s . co m * {@link magma.agent.worldmodel.impl.VisibleObject#getDirectionTo(IVisibleObject)} * . */ @Test public void testGetDirectionTo() throws Exception { thisPlayer.setPosition(new Vector3D(1.0, 1.0, 0.0)); testee.position = new Vector3D(2.0, 2.0, 0.0); assertEquals(-135.0, testee.getDirectionTo(thisPlayer).degrees(), 0.0001); assertEquals(45.0, thisPlayer.getDirectionTo(testee).degrees(), 0.0001); }
From source file:magma.agent.worldmodel.localizer.impl.LocalizerTriangulation.java
/** * Calculates absolute position and directions from the two flags passed * using triangulation. Absolute means with respect to the game fields * coordinate system./*from ww w . j av a 2s. c o m*/ * @param flag1 first visible landmark with known position * @param flag2 second visible landmark with known position (has to be right * of first) * @param neckAngle the horizontal neck angle of the viewer * @return an array of two Vector3Ds, the first containing the absolute x,y,z * position of the viewer on the field, the second containing no real * vector, but the horizontal, latitudal and rotational absolute body * angles of the viewer */ private PositionOrientation triangulate(ILocalizationFlag flag1, ILocalizationFlag flag2, float neckAngle) { float flag1Direction; float flag2Direction; double r1, r2; // the distance of the player to the flags double dist, dist2; // the distance (square) of the two flags double a; // distance from one flag to intersection // point P3 double h; // distance from P3 to the intersection // Points P1 and P2 of the two circles double x, x1, x2, x3; double y, y1, y2, y3; double ratio; Angle horizontalAbsDirection; float beta; logger.log(Level.FINER, "Triangulation with flags 1:{0} 2:{1}", new Object[] { flag1, flag2 }); // do the calculations flag1Direction = (float) flag1.getHorizontalDirection(); flag2Direction = (float) flag2.getHorizontalDirection(); r1 = flag1.getDistance(); r2 = flag2.getDistance(); x1 = flag1.getKnownPosition().getX(); x2 = flag2.getKnownPosition().getX(); y1 = flag1.getKnownPosition().getY(); y2 = flag2.getKnownPosition().getY(); // calculate the square distance of the two flags dist2 = (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1); dist = Math.sqrt(dist2); if (dist > r1 + r2) { // the circles would not intersect dist = r1 + r2; dist2 = dist * dist; } else if ((r1 > r2) && (dist + r2 < r1)) { // the circles would not intersect dist = r1 - r2; dist2 = dist * dist; } else if ((r2 > r1) && (dist + r1 < r2)) { // the circles would not intersect dist = r2 - r1; dist2 = dist * dist; } r1 *= r1; r2 *= r2; // a = (r1^2 - r2^2 + d^2 ) / (2 d) // a = distance from flag1 to base point of height line a = (r1 - r2 + dist2) / (2.0 * dist); // h^2 = r1^2 - a^2 // h = height of the triangle flag1, flag2 and my position h = r1 - a * a; if (h < 0.0) h = 0.0; else h = Math.sqrt(h); // calculate middle of intersection line // P3 = P1 + a ( P2 - P1 ) / d x3 = x1 + a * (x2 - x1) / dist; y3 = y1 + a * (y2 - y1) / dist; // two circles intersect usually in 2 points. Find out which one to // select if (flag1Direction > flag2Direction) { // result x = x3 + h ( y2 - y1 ) / d x = x3 + h * (y2 - y1) / dist; // result y = y3 - h ( x2 - x1 ) / d y = y3 - h * (x2 - x1) / dist; } else { x = x3 - h * (y2 - y1) / dist; y = y3 + h * (x2 - x1) / dist; } // TODO: add z position calculation Vector3D position = new Vector3D(x, y, 0); // calculate the absolute direction r1 = flag1.getDistance(); ratio = (y1 - y) / r1; beta = (float) Math.asin(ratio); if (x > x1) { horizontalAbsDirection = Angle.RAD180.subtract(beta).subtract(flag1Direction); } else { horizontalAbsDirection = Angle.rad(beta).subtract(flag1Direction); } // adjust to head angle horizontalAbsDirection = horizontalAbsDirection.subtract(neckAngle); // TODO add vertical direction logger.log(Level.FINER, "Localize pos:({0}, {1}), angles:{2}, neck:{3}", new Object[] { x, y, horizontalAbsDirection, neckAngle }); return new PositionOrientation(position, horizontalAbsDirection); }
From source file:magma.agent.behavior.complex.RunToPosition.java
public void setPosition(float x, float y, float rotation) { setPosition(new Vector3D(x, y, 0), rotation); }
From source file:magma.agent.worldmodel.impl.GlobalMap.java
private void processHear(IPerception perception, Ball ball) { IHearPerceptor hear = perception.getHearPerceptor(); String name = hear.getName(); if ("self".equalsIgnoreCase(name)) { // message from ourselves return;/*from w ww .j ava 2 s . com*/ } try { String message = hear.getMessage(); int index = message.indexOf('-'); if (index < 0) { // no message from us return; } int x = new Integer(message.substring(0, index)); int y = new Integer(message.substring(index + 1)); float xlong = (x / 100.0f) - 10; float ylong = (y / 100.0f) - 5; ball.setPosition(new Vector3D(xlong, ylong, ball.getPosition().getZ())); logger.log(Level.FINER, "hear ball: ({0}, {1})", new Object[] { xlong, ylong }); } catch (Exception e) { // might happen with opponent messages } }
From source file:magma.agent.agentmodel.impl.AgentModel.java
/** * updates all known GyroRates from perception * @param perception the result from server message parsing */// w w w. ja va 2 s. com protected void updateGytoRate(IPerception perception) { Vector3D rate; for (int i = 0; i < IServerConfigFilesConstants.GYRORATE_NAMES.length; i++) { rate = perception.getGyroRatePerceptor(IServerConfigFilesConstants.GYRORATE_PERCEPTORS[i]).getGyro(); getGyroRate(IServerConfigFilesConstants.GYRORATE_NAMES[i]) .setGyro(new Vector3D(rate.getX(), rate.getY(), rate.getZ())); } }
From source file:magma.agent.worldmodel.localizer.impl.LocalizerTriangulationTest.java
@Test public void testLocalizeBug1() { expect(G2R.getDistance()).andStubReturn(6.22); expect(G2R.getHorizontalDirection()).andStubReturn(toRadians(48.779998)); expect(F2R.getDistance()).andStubReturn(7.34); expect(F2R.getHorizontalDirection()).andStubReturn(toRadians(21.93000)); expect(G2R.compareTo(F2R)).andStubReturn(-1); replay(G2R, F2R);// w ww. j a va 2 s . c o m flags.put("G2R", G2R); flags.put("F2R", F2R); PositionOrientation localizeInfo = testee.localize(flags, 0, 0, Vector3D.zero); assertNotNull(localizeInfo); assertTrue(FuzzyCompare.eq(new Vector3D(-0.185829, -0.0489, 0), localizeInfo.getPosition(), 0.01f)); assertEquals(-0.95624, localizeInfo.getOrientationX().radians(), 0.001); }