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

Introduction

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

Prototype

public double getX() 

Source Link

Document

Get the abscissa of the vector.

Usage

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/*w  ww  .  ja  va  2 s  . c o  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
 *///  w  ww .  jav a  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 w w w  . ja  v  a 2 s . co 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.LayOnSide.java

@Override
public float getTruthValue() {
    Vector3D gyro = agent.getGyroRate("TorsoGyro").getTranslation();

    if (laying() && (gyro.getX() > 0.7 || gyro.getX() < -0.7)) {
        return 1.0f;
    }//w  w w  . j  av  a2s .  c  om

    return 0.0f;
}

---

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;
    }//from ww  w  .  j  a va2  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;
    }/*from w ww  .j a v  a2s. c om*/

    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.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);//w  ww.ja va  2 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.worldmodel.impl.MovableObject.java

public void calculateSpeed(float lastSeenTime, float time) {
    // need previousPosition, hence speed is calculated after update
    if (getPreviousPosition() != null) {
        Vector3D checkSpeed = new Vector3D();
        checkSpeed = getPosition().subtract(getPreviousPosition());
        if (time - lastSeenTime > 1)
            checkSpeed = new Vector3D(checkSpeed.getX() / (time - lastSeenTime),
                    checkSpeed.getY() / (time - lastSeenTime), checkSpeed.getZ() / (time - lastSeenTime));

        // we don't set speed for impossible values
        if (checkSpeed.getNorm() < getPossibleSpeed())
            speed = checkSpeed;/*from   w  w w  .j av a  2s .  c  o  m*/
    } else
        speed = new Vector3D(0.0, 0.0, 0.0);
}

---

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;
        }//from w  ww .ja  v  a2  s  .  c  om
    }
    return 0.0f;
}

---

• «
• 1
• 2
• 3
• »