private float getCorrectYaw(double tX, double tZ) {
   if (locZ > tZ) return (float) (-Math.toDegrees(Math.atan((locX - tX) / (locZ - tZ))));
   if (locZ < tZ) {
     return (float) (-Math.toDegrees(Math.atan((locX - tX) / (locZ - tZ)))) + 180.0F;
   }
   return yaw;
 }
  @Override
  public void onSensorChanged(SensorEvent event) {

    switch (event.sensor.getType()) {
      case Sensor.TYPE_ACCELEROMETER:
        for (int i = 0; i < 3; i++) {
          System.out.println("event value:" + event.values[i]);
          valuesAccelerometer[i] = event.values[i];
        }
        break;
      case Sensor.TYPE_MAGNETIC_FIELD:
        for (int i = 0; i < 3; i++) {
          System.out.println("event value2:" + event.values[i]);
          valuesMagneticField[i] = event.values[i];
        }
        break;
    }

    boolean success =
        SensorManager.getRotationMatrix(matrixR, matrixI, valuesAccelerometer, valuesMagneticField);

    if (success) {
      SensorManager.getOrientation(matrixR, matrixValues);

      double azimuth = Math.toDegrees(matrixValues[0]);
      double pitch = Math.toDegrees(matrixValues[1]);
      double roll = Math.toDegrees(matrixValues[2]);

      System.out.println("orientation[0]: azimut: " + azimuth);
      System.out.println("orientation[1]: pitch : " + pitch);
      System.out.println("orientation[2]: roll: " + roll);
    }
  }
Example #3
1
 /**
  * Sets the position, bounds, and angular extents of this arc to the specified value. The starting
  * angle of the arc is tangent to the line specified by points (p1, p2), the ending angle is
  * tangent to the line specified by points (p2, p3), and the arc has the specified radius.
  *
  * @param p1 The first point that defines the arc. The starting angle of the arc is tangent to the
  *     line specified by points (p1, p2).
  * @param p2 The second point that defines the arc. The starting angle of the arc is tangent to
  *     the line specified by points (p1, p2). The ending angle of the arc is tangent to the line
  *     specified by points (p2, p3).
  * @param p3 The third point that defines the arc. The ending angle of the arc is tangent to the
  *     line specified by points (p2, p3).
  * @param radius The radius of the arc.
  * @since 1.2
  */
 public void setArcByTangent(Point2D p1, Point2D p2, Point2D p3, double radius) {
   double ang1 = Math.atan2(p1.getY() - p2.getY(), p1.getX() - p2.getX());
   double ang2 = Math.atan2(p3.getY() - p2.getY(), p3.getX() - p2.getX());
   double diff = ang2 - ang1;
   if (diff > Math.PI) {
     ang2 -= Math.PI * 2.0;
   } else if (diff < -Math.PI) {
     ang2 += Math.PI * 2.0;
   }
   double bisect = (ang1 + ang2) / 2.0;
   double theta = Math.abs(ang2 - bisect);
   double dist = radius / Math.sin(theta);
   double x = p2.getX() + dist * Math.cos(bisect);
   double y = p2.getY() + dist * Math.sin(bisect);
   // REMIND: This needs some work...
   if (ang1 < ang2) {
     ang1 -= Math.PI / 2.0;
     ang2 += Math.PI / 2.0;
   } else {
     ang1 += Math.PI / 2.0;
     ang2 -= Math.PI / 2.0;
   }
   ang1 = Math.toDegrees(-ang1);
   ang2 = Math.toDegrees(-ang2);
   diff = ang2 - ang1;
   if (diff < 0) {
     diff += 360;
   } else {
     diff -= 360;
   }
   setArcByCenter(x, y, radius, ang1, diff, type);
 }
  public Geographic2dCoordinate destination(
      final double latitude,
      final double longitude,
      final double initialBearing,
      final double distance) {

    final double b = Math.toRadians(initialBearing);
    final double d = distance / sphere.getRadius();
    final double lat1 = Math.toRadians(latitude);
    final double lon1 = Math.toRadians(longitude);

    double lat2 = lat1 + d * Math.cos(b);
    final double dLat = lat2 - lat1;
    final double dPhi =
        Math.log(Math.tan(lat2 / 2 + Math.PI / 4) / Math.tan(lat1 / 2 + Math.PI / 4));
    final double q =
        (!Double.isNaN(dLat / dPhi)) ? dLat / dPhi : Math.cos(lat1); // E-W line gives dPhi=0

    final double dLon = d * Math.sin(b) / q;

    // check for some daft bugger going past the pole, normalise latitude if
    // so
    if (Math.abs(lat2) > Math.PI / 2) {
      lat2 = lat2 > 0 ? Math.PI - lat2 : -(Math.PI - lat2);
    }

    final double lon2 = (lon1 + dLon + Math.PI) % (2 * Math.PI) - Math.PI;

    return new Geographic2dCoordinate(Math.toDegrees(lat2), Math.toDegrees(lon2));
  }
Example #5
1
 private double cal_angle(float x, float y) {
   if (x >= 0 && y >= 0) return Math.toDegrees(Math.atan(y / x));
   else if (x < 0 && y >= 0) return Math.toDegrees(Math.atan(y / x)) + 180;
   else if (x < 0 && y < 0) return Math.toDegrees(Math.atan(y / x)) + 180;
   else if (x >= 0 && y < 0) return Math.toDegrees(Math.atan(y / x)) + 360;
   return 0;
 }
  public String createVTG(SignalKModel model) {
    if (model.getValue(vessels_dot_self_dot + nav_courseOverGroundMagnetic) != null
        || model.getValue(vessels_dot_self_dot + nav_courseOverGroundTrue) != null
        || model.getValue(vessels_dot_self_dot + nav_speedOverGround) != null) {
      VTGSentence sen = (VTGSentence) sf.createParser(TalkerId.II, SentenceId.VTG);
      sen.setMode(FaaMode.AUTOMATIC);
      if (model.getValue(vessels_dot_self_dot + nav_courseOverGroundMagnetic) != null)
        sen.setMagneticCourse(
            Math.toDegrees(
                (double) model.getValue(vessels_dot_self_dot + nav_courseOverGroundMagnetic)));
      if (model.getValue(vessels_dot_self_dot + nav_courseOverGroundTrue) != null)
        sen.setTrueCourse(
            Math.toDegrees(
                (double) model.getValue(vessels_dot_self_dot + nav_courseOverGroundTrue)));
      if (model.getValue(vessels_dot_self_dot + nav_speedOverGround) != null) {
        sen.setSpeedKmh(
            (double) model.getValue(vessels_dot_self_dot + nav_speedOverGround) * MS_TO_KM);
        sen.setSpeedKnots(
            (double) model.getValue(vessels_dot_self_dot + nav_speedOverGround) * MS_TO_KNOTS);
      }

      return sen.toSentence();
    }
    return null;
  }
Example #7
1
  /**
   * @param tiles
   * @param distance
   * @param lat
   * @param lon
   * @return
   */
  private DataTile processTile(
      ArrayList<ArrayList<DataTile>> tiles, int distance, double lat, double lon) {
    int rankAverage;
    int rankTotal = 0;
    Integer finalLat;
    Integer finalLon;
    Double finalHeight;
    Double heightTotal = 0d;

    ArrayList<Integer> rankArray = new ArrayList<Integer>();
    ArrayList<Double> heightArray = new ArrayList<Double>();

    if (tiles.size() == 0) {
      // Theres missing data so return null :(
      return null;
    }

    for (int x = 0; x < tiles.size() - 1; x++) {

      ArrayList<DataTile> tileRow = tiles.get(x);

      for (int y = 0; y < tileRow.size() - 1; y++) {

        Double slope;
        Double opp;
        Double centre = tileRow.get(y).getHeight();
        Double right = tileRow.get(y + 1).getHeight();
        Double below = tiles.get(x).get(y).getHeight();

        heightArray.add(centre);

        // Calc right slope first
        opp = centre - right;
        slope = Math.toDegrees(Math.tan((opp / distance)));
        rankArray.add(calcSlopeRank(slope));

        // Calc below slope
        opp = centre - below;
        slope = Math.toDegrees(Math.tan((opp / distance)));
        rankArray.add(calcSlopeRank(slope));
      }
    }

    // Calculate rank
    for (Integer rank : rankArray) {
      rankTotal += rank;
    }
    rankAverage = rankTotal / rankArray.size();

    for (Double height : heightArray) {
      heightTotal += height;
    }
    finalHeight = heightTotal / heightArray.size();

    // truncate lat/lon
    finalLat = (int) lat;
    finalLon = (int) lon;

    return new DataTile(finalLat.doubleValue(), finalLon.doubleValue(), rankAverage, finalHeight);
  }
  @SuppressWarnings("MagicNumber")
  @Test
  public void checkDegreeBetweenPlayers() {
    GameProfile playerOne = new GameProfile();
    GameProfile playerTwo = new GameProfile();
    final double x1 = 100;
    playerOne.setX(x1);
    final double y1 = 100;
    playerOne.setY(y1);
    final double x2 = 150;
    playerTwo.setX(x2);
    final double y2 = 150;
    playerTwo.setY(y2);
    MoveActionStrategy actionStrategy = new MoveActionStrategy(gameServer);
    final double myDirection = 90;
    final double enemyDirection = 90;

    double playerOneDegree =
        Math.toDegrees(actionStrategy.getDegree(playerOne, myDirection, x2 - x1, y2 - y1));
    double playerTwoDegree =
        Math.toDegrees(actionStrategy.getDegree(playerTwo, enemyDirection, -x2 + x1, -y2 + y1));

    assertEquals("wrong degree for player 1", 45.0d, Math.floor(playerOneDegree), 0);
    assertEquals("wrong degree for player 2", 135.0d, Math.floor(playerTwoDegree), 0);
  }
Example #9
0
  private float angleBetweenLines(
      float fx1, float fy1, float fx2, float fy2, float sx1, float sy1, float sx2, float sy2) {
    float angle1 = (float) Math.atan2((fy1 - fy2), (fx1 - fx2));
    float angle2 = (float) Math.atan2((sy1 - sy2), (sx1 - sx2));

    return findAngleDelta((float) Math.toDegrees(angle1), (float) Math.toDegrees(angle2));
  }
    /** {@inheritDoc} */
    @Override
    Position pointOnBearing0(
        double startLatDegrees,
        double startLonDegrees,
        double distanceMeters,
        double bearingDegrees) {
      // Convert to radians
      startLatDegrees = Math.toRadians(startLatDegrees);
      startLonDegrees = Math.toRadians(startLonDegrees);
      bearingDegrees = Math.toRadians(bearingDegrees);
      // the earth's radius in meters
      final double earthRadius = EARTH_MEAN_RADIUS_KM * 1000.0;

      double endLat =
          Math.asin(
              Math.sin(startLatDegrees) * Math.cos(distanceMeters / earthRadius)
                  + Math.cos(startLatDegrees)
                      * Math.sin(distanceMeters / earthRadius)
                      * Math.cos(bearingDegrees));
      double endLon =
          startLonDegrees
              + Math.atan2(
                  Math.sin(bearingDegrees)
                      * Math.sin(distanceMeters / earthRadius)
                      * Math.cos(startLatDegrees),
                  Math.cos(distanceMeters / earthRadius)
                      - Math.sin(startLatDegrees) * Math.sin(endLat));
      return Position.create(Math.toDegrees(endLat), Math.toDegrees(endLon));
    }
Example #11
0
  /**
   * Returns the angles, in degrees, about each axis of this quaternion stored in a Vector3 <br>
   * <br>
   * vect.X = Rotation about the X axis (Pitch) <br>
   * vect.Y = Rotation about the Y axis (Yaw) <br>
   * vect.Z = Rotation about the Z axis (Roll) <br>
   *
   * @param a
   * @return axis angles
   */
  public static Vector3 getAxisAngles(Quaternion a) {
    // Map to Euler angles
    final float q0 = a.w;
    final float q1 = a.z; // roll
    final float q2 = a.x; // pitch
    final float q3 = a.y; // yaw

    final double r1, r2, r3, test;
    test = q0 * q2 - q3 * q1;

    if (Math.abs(test) < 0.4999) {
      r1 = Math.atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2));
      r2 = Math.asin(2 * test);
      r3 = Math.atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3));
    } else { // pitch is at north or south pole
      int sign = (test < 0) ? -1 : 1;
      r1 = 0;
      r2 = sign * Math.PI / 2;
      r3 = -sign * 2 * Math.atan2(q1, q0);
    }

    // ...and back to Tait-Bryan
    final float roll = (float) Math.toDegrees(r1);
    final float pitch = (float) Math.toDegrees(r2);
    float yaw = (float) Math.toDegrees(r3);
    if (yaw > 180) // keep -180 < yaw < 180
    yaw -= 360;
    else if (yaw < -180) yaw += 360;
    return new Vector3(pitch, yaw, roll);
  }
Example #12
0
  private Location calculateNextLocation(Location loc, double distance) {
    Random rand = new Random();
    int minAngle = 0;
    int maxAngle = 360;
    int randomAngle = rand.nextInt((maxAngle - minAngle) + 1) + minAngle;
    double bearing = Math.toRadians(randomAngle);

    double lat1 = Math.toRadians(loc.Latitude);
    double lon1 = Math.toRadians(loc.Longtitude);

    double lat2 =
        Math.asin(
            Math.sin(lat1) * Math.cos(distance / EarthRadius)
                + Math.cos(lat1) * Math.sin(distance / EarthRadius) * Math.cos(bearing));

    double lon2 =
        lon1
            + Math.atan2(
                Math.sin(bearing) * Math.sin(distance / EarthRadius) * Math.cos(lat1),
                Math.cos(distance / EarthRadius) - Math.sin(lat1) * Math.sin(lat2));

    lat2 = Math.toDegrees(lat2);
    lon2 = Math.toDegrees(lon2);

    return LatLongToXYZ(lat2, lon2);
  }
Example #13
0
 @Override
 public void run() {
   if (path != null) {
     Node n = path.getNextNode();
     Block b = null;
     float angle = yaw;
     float look = pitch;
     if (n != null) {
       if (last == null || path.checkPath(n, last, true)) {
         b = n.b;
         if (last != null) {
           angle =
               ((float)
                   Math.toDegrees(
                       Math.atan2(last.b.getX() - b.getX(), last.b.getZ() - b.getZ())));
           look = (float) (Math.toDegrees(Math.asin(last.b.getY() - b.getY())) / 2);
         }
         setPositionRotation(b.getX() + 0.5, b.getY(), b.getZ() + 0.5, angle, look);
         timer.schedule(new movePath(), 300);
       } else {
         pathFindTo(end, maxIter);
       }
     } else if (last != null) {
       setPositionRotation(end.getX(), end.getY(), end.getZ(), end.getYaw(), end.getPitch());
     }
     last = n;
   }
 }
Example #14
0
  /**
   * Inverse project x,y coordinates into a LatLonPoint.
   *
   * <p>
   *
   * @param x integer x coordinate
   * @param y integer y coordinate
   * @param ret_val LatLonPoint
   * @return LatLonPoint ret_val
   * @see Proj#inverse(Point2D)
   */
  @SuppressWarnings("unchecked")
  public <T extends Point2D> T inverse(double x, double y, T ret_val) {
    // Debug.output("CADRG.inverse");

    if (ret_val == null) {
      ret_val = (T) new LatLonPoint.Double();
    }

    /* offset back into pixel space from Drawable space */
    double px = x + ul.x /* - ox */;
    double py = -y + ul.y + oy;

    // Check bounds on the call (P Space). Mutate if needed.
    if (px > ProjMath.roundAdjust(world.x / 2.0)) {
      px = ProjMath.roundAdjust(world.x / 2.0);
    } else if (px < ProjMath.roundAdjust(-world.x / 2.0)) {
      px = ProjMath.roundAdjust(-world.x / 2.0);
    }
    if (py > ProjMath.roundAdjust(world.y / 2.0)) {
      py = ProjMath.roundAdjust(world.y / 2.0);
    } else if (py < ProjMath.roundAdjust(-world.y / 2.0)) {
      py = ProjMath.roundAdjust(-world.y / 2.0);
    }

    // normalize_latitude on the way out.
    double lat_ = normalizeLatitude(py / spps_y);
    double lon_ = wrapLongitude((px / spps_x) + centerX);
    ret_val.setLocation(Math.toDegrees(lon_), Math.toDegrees(lat_));

    return ret_val;
  }
 // RMC,GLL, GGA = position
 public String createRMC(SignalKModel model) {
   if (model.get(vessels_dot_self_dot + nav_position_latitude) != null
       && model.get(vessels_dot_self_dot + nav_position_longitude) != null) {
     RMCSentence sen = (RMCSentence) sf.createParser(TalkerId.GP, SentenceId.RMC);
     sen.setDate(new Date());
     sen.setStatus(DataStatus.ACTIVE);
     sen.setMode(FaaMode.AUTOMATIC);
     sen.setPosition(
         new Position(
             (double) model.get(vessels_dot_self_dot + nav_position_latitude),
             (double) model.get(vessels_dot_self_dot + nav_position_longitude)));
     if (model.getValue(vessels_dot_self_dot + nav_speedOverGround) != null) {
       sen.setSpeed(
           (double) model.getValue(vessels_dot_self_dot + nav_speedOverGround) * MS_TO_KNOTS);
     }
     if (model.get(vessels_dot_self_dot + nav_courseOverGroundTrue) != null) {
       sen.setCourse(
           Math.toDegrees(
               (double) model.getValue(vessels_dot_self_dot + nav_courseOverGroundTrue)));
     }
     if (model.get(vessels_dot_self_dot + nav_magneticVariation) != null) {
       Double variation = (Double) model.getValue(vessels_dot_self_dot + nav_magneticVariation);
       if (variation != null) {
         sen.setVariation(Math.toDegrees(Math.abs(variation)));
         sen.setDirectionOfVariation((variation < 0) ? CompassPoint.WEST : CompassPoint.EAST);
       }
     }
     return sen.toSentence();
   }
   return null;
 }
Example #16
0
  public static GeckoEvent createSensorEvent(SensorEvent s) {
    int sensor_type = s.sensor.getType();
    GeckoEvent event = null;

    switch (sensor_type) {
      case Sensor.TYPE_ACCELEROMETER:
        event = new GeckoEvent(SENSOR_EVENT);
        event.mFlags = GeckoHalDefines.SENSOR_ACCELERATION;
        event.mMetaState = HalSensorAccuracyFor(s.accuracy);
        event.mX = s.values[0];
        event.mY = s.values[1];
        event.mZ = s.values[2];
        break;

      case 10 /* Requires API Level 9, so just use the raw value - Sensor.TYPE_LINEAR_ACCELEROMETER*/:
        event = new GeckoEvent(SENSOR_EVENT);
        event.mFlags = GeckoHalDefines.SENSOR_LINEAR_ACCELERATION;
        event.mMetaState = HalSensorAccuracyFor(s.accuracy);
        event.mX = s.values[0];
        event.mY = s.values[1];
        event.mZ = s.values[2];
        break;

      case Sensor.TYPE_ORIENTATION:
        event = new GeckoEvent(SENSOR_EVENT);
        event.mFlags = GeckoHalDefines.SENSOR_ORIENTATION;
        event.mMetaState = HalSensorAccuracyFor(s.accuracy);
        event.mX = s.values[0];
        event.mY = s.values[1];
        event.mZ = s.values[2];
        break;

      case Sensor.TYPE_GYROSCOPE:
        event = new GeckoEvent(SENSOR_EVENT);
        event.mFlags = GeckoHalDefines.SENSOR_GYROSCOPE;
        event.mMetaState = HalSensorAccuracyFor(s.accuracy);
        event.mX = Math.toDegrees(s.values[0]);
        event.mY = Math.toDegrees(s.values[1]);
        event.mZ = Math.toDegrees(s.values[2]);
        break;

      case Sensor.TYPE_PROXIMITY:
        event = new GeckoEvent(SENSOR_EVENT);
        event.mFlags = GeckoHalDefines.SENSOR_PROXIMITY;
        event.mMetaState = HalSensorAccuracyFor(s.accuracy);
        event.mX = s.values[0];
        event.mY = 0;
        event.mZ = s.sensor.getMaximumRange();
        break;

      case Sensor.TYPE_LIGHT:
        event = new GeckoEvent(SENSOR_EVENT);
        event.mFlags = GeckoHalDefines.SENSOR_LIGHT;
        event.mMetaState = HalSensorAccuracyFor(s.accuracy);
        event.mX = s.values[0];
        break;
    }
    return event;
  }
Example #17
0
 private void updateOrientation() {
   if (SensorManager.getRotationMatrix(R, null, accelerometerValues, magneticFieldValues)) {
     SensorManager.getOrientation(R, orientation);
     azimuth = (float) Math.toDegrees(orientation[0]);
     pitch = (float) Math.toDegrees(orientation[1]);
     roll = (float) Math.toDegrees(orientation[2]);
   }
 }
 // ---- obie funkcje przyjmuja wartoœci dodatnie, zarówno dla x jak i y --//
 // zwraca wartoœæ o jak¹ ma obróciæ siê robot aby staæ przodem do docelowych wspó³rzênych x,y
 public static double setDirection(Pose actualPose, double destX, double destY) {
   double newAngle =
       (Math.atan2(
           actualPose.getLocation().getY() - destY, actualPose.getLocation().getX() + destX));
   System.out.println(
       -(Math.toDegrees(angleDiff(Math.toRadians(actualPose.getHeading()), newAngle))));
   return -(Math.toDegrees(angleDiff(Math.toRadians(actualPose.getHeading()), newAngle)));
 } // u¿ycie:
Example #19
0
  /**
   * Return the indices where the corners occur in the given stroke. Return null if no corners exist
   * in the stroke. Iterate over the points in the stroke, for every three points (p1, p2, p3) we
   * form two vectors, (p1,p2) and (p2, p3). If the dot product of these two vectors is less than
   * threshDot, then there is definitely an angle at p2. If the dot product is greater than
   * threshDot but less than threshRelaxedDot, it is likely that there is an angle at p2, but we're
   * not sure. In this case, the magnitudes of both vectors are calculated, m1 and m2. Let 'plen' be
   * the stroke path length, we calculate r1 = m1/plen and r2 = m2/plen. If either r1 or r2 is
   * greater than threshMagRatio, then we say that there is a corner. If both vectors are really
   * short (r1<threshMagRatio && r2<threshMagRatio), then they are not significant enough to be used
   * to determine a corner. They are more likely to be noise due to the pen/tablet hardware.
   */
  public static final int[] cornerIndices(
      TimedStroke s, double threshMagRatio, double threshDot, double threshRelaxedDot) {
    if (s == null) {
      return null;
    } else if (s.getVertexCount() < 3) {
      return null;
    } else {
      int numPoints = s.getVertexCount();
      ArrayList cornerIndices = new ArrayList();
      double pathLength = PathLengthFE.pathLength(s);
      double x1, y1, x2, y2, x3, y3;
      for (int i = 0; i < numPoints - 2; i++) {
        x1 = s.getX(i);
        y1 = s.getY(i);
        x2 = s.getX(i + 1);
        y2 = s.getY(i + 1);
        x3 = s.getX(i + 2);
        y3 = s.getY(i + 2);
        double dot = FEUtilities.dotProduct(x1, y1, x2, y2, x3, y3);

        if (dot < threshDot) { // definitely an angle
          double angle = Math.acos(dot);
          angle = Math.toDegrees(angle);
          int j = i + 1;
          cornerIndices.add(new Integer(j));
          //                        numCorners++;
        } else if (dot < threshRelaxedDot) {
          // likely to be an angle
          double l1 = FEUtilities.distance(x1, y1, x2, y2);
          double l2 = FEUtilities.distance(x2, y2, x3, y3);
          l1 = l1 / pathLength;
          l2 = l2 / pathLength;
          if ((l1 > threshMagRatio) || (l2 > threshMagRatio)) {
            double angle = Math.acos(dot);
            angle = Math.toDegrees(angle);
            //                            numCorners++;
            int j = i + 1;
            cornerIndices.add(new Integer(j));
          }
        }
      }
      if (cornerIndices.size() == 0) {
        return null;
      } else {
        int arr[] = new int[cornerIndices.size()];
        // System.out.println("Corner indices:-----");
        int k = 0;
        for (Iterator i = cornerIndices.iterator(); i.hasNext(); ) {
          Integer index = (Integer) i.next();
          int val = index.intValue();
          arr[k++] = val;
          // System.out.println(val+":("+s.getX(val)+", "+s.getY(val)+")");
        }
        // System.out.println("--------------------");
        return arr;
      }
    }
  }
Example #20
0
 public Velocity redirectLeft(Velocity nullp) {
   Velocity v = new Velocity(x - nullp.x, x - nullp.z);
   float ax = (float) Math.toDegrees(Math.acos(x));
   float ay = (float) Math.toDegrees(Math.asin(z));
   if (ay <= 0) ax = 360f - ax;
   ax = ax + 90;
   v = new Velocity((float) Math.cos(Math.toRadians(ax)), (float) Math.sin(Math.toRadians(ax)));
   return new Velocity(v.x, v.z);
 }
Example #21
0
 /**
  * @param latitude the latitude, in radians.
  * @param longitude the longitude, in radians.
  */
 public static GeoLocation fromRadians(double latitude, double longitude) {
   GeoLocation result = new GeoLocation();
   result.radLat = latitude;
   result.radLon = longitude;
   result.degLat = Math.toDegrees(latitude);
   result.degLon = Math.toDegrees(longitude);
   result.checkBounds();
   return result;
 }
Example #22
0
File: CNN.java Project: em-z/Docs
 /** 如果超出赛场范围则返回true */
 public boolean goPosition(int degree) throws Exception { // 记得自定义Excetion
   float x, y;
   x = this.position[0] + (float) (robotMoveRate * Math.cos(Math.toDegrees(degree)));
   y = this.position[1] + (float) (robotMoveRate * Math.sin(Math.toDegrees(degree)));
   if (x < robotRadius || x > rangeX - robotRadius) return true;
   if (y < robotRadius || y > rangeY - robotRadius) return true;
   this.position[0] = x;
   this.position[1] = y;
   return false;
 }
 private static RealMatrix T5(double t) {
   double latangle = Math.toDegrees(magnetic_latitude(t)) - 90.0;
   double lonangle = Math.toDegrees(magnetic_longitude(t));
   RealMatrix A =
       RotationMatrix(
           -latangle, new Vector3D(0, 1, 0)); // latangle in original. Change due to java.
   RealMatrix B = RotationMatrix(lonangle, new Vector3D(0, 0, 1));
   RealMatrix T5 = A.preMultiply(B);
   return T5;
 }
 /** Normalize x & y (put in lon-lat ranges) & ensure geohash round-trip for given precision. */
 private Point normPointXY(double x, double y) {
   // put x,y as degrees into double[] as radians
   double[] latLon = {Math.toRadians(y), Math.toRadians(x)};
   DistanceUtils.normLatRAD(latLon);
   DistanceUtils.normLatRAD(latLon);
   double x2 = Math.toDegrees(latLon[1]);
   double y2 = Math.toDegrees(latLon[0]);
   // overwrite latLon, units is now degrees
   return GeohashUtils.decode(GeohashUtils.encodeLatLon(y2, x2, PRECISION), ctx);
 }
Example #25
0
 @Subscribe
 public void consume(SimulatedState simState) {
   LocationType loc =
       new LocationType(Math.toDegrees(simState.getLat()), Math.toDegrees(simState.getLon()));
   loc.setHeight(simState.getHeight());
   loc.translatePosition(simState.getX(), simState.getY(), simState.getZ());
   simulatedState =
       new SystemPositionAndAttitude(
           loc, simState.getPhi(), simState.getTheta(), simState.getPsi());
   lastStateMillis = System.currentTimeMillis();
 }
 @Override
 public double[] getDown() {
   double[] toRet =
       new double[] {
         Math.toDegrees(Math.atan2(acc.getZ(), acc.getX())),
         Math.toDegrees(Math.atan2(acc.getZ(), acc.getY()))
       };
   toRet[0] -= cal.getValues()[0];
   toRet[1] -= cal.getValues()[1];
   return toRet;
 }
Example #27
0
  /**
   * Use this function to get the angle between you and another object. For example: You can use
   * this function to check if you approaching another object from the left or right. Direction 0
   * points up, directions go clockwise, so 90 is right, etc.
   *
   * @param object an instance of another object to calculate the angle for.
   * @return the angle of the object or 0 if the object is null.
   */
  public final double getAngle(GameObject object) {

    double deltaX = object.getFullX() - getFullX();
    double deltaY = object.getFullY() - getFullY();

    if (deltaX >= 0 || deltaY >= 0) {
      return Math.toDegrees(Math.atan2(deltaX, deltaX)) + 90;
    } else {
      return Math.toDegrees((Math.atan2(deltaY, deltaX))) + 450;
    }
  }
Example #28
0
  /**
   * Calculate the angle between two points and a given vertex
   *
   * @param one
   * @param vertex
   * @param two
   * @return
   */
  public static double calcAngle(
      final Coordinate one, final Coordinate vertex, final Coordinate two) {

    final double p1x = one.x - vertex.x;
    final double p1y = one.y - vertex.y;
    final double p2x = two.x - vertex.x;
    final double p2y = two.y - vertex.y;

    final double angle1 = Math.toDegrees(Math.atan2(p1y, p1x));
    final double angle2 = Math.toDegrees(Math.atan2(p2y, p2x));
    return angle2 - angle1;
  }
  private void determineOrientation(float[] rotationMatrix) {
    float[] orientationValues = new float[3];
    SensorManager.getOrientation(rotationMatrix, orientationValues);

    mResultData[0] = (int) ((Math.toDegrees(orientationValues[0]) + 360) % 360);
    mResultData[1] = (int) Math.toDegrees(orientationValues[1]);
    mResultData[2] = (int) Math.toDegrees(orientationValues[2]);

    mResultViews[0].setText("azimuth: " + String.valueOf((int) mResultData[0]));
    mResultViews[1].setText("pitch: " + String.valueOf((int) mResultData[1]));
    mResultViews[2].setText("roll: " + String.valueOf((int) mResultData[2]));
  }
Example #30
0
 public static LatLonPoint getLatLonForPixel(
     double startLat, double startLon, double differenceX, double differenceY, float scale) {
   double startY = 20925.5249D * Math.toRadians(startLat);
   double startX = 20925.5249D * Math.cos(Math.toRadians(startLat)) * Math.toRadians(startLon);
   differenceX /= scale;
   differenceY /= scale;
   double endX = differenceX / 1000D + startX;
   double endY = (differenceY / 1000D - startY) * -1D;
   double endLat = Math.toDegrees(endY / 20925.5249D);
   double endLon = Math.toDegrees(endX / (20925.5249D * Math.cos(Math.toRadians(startLat))));
   return new LatLonPoint(endLat, endLon);
 }