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);
}
}
/**
* 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));
}
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;
}
/**
* @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);
}
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));
}
/**
* 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);
}
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);
}
@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;
}
}
/**
* 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;
}
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;
}
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:
/**
* 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;
}
}
}
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);
}
/**
* @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;
}
/** 如果超出赛场范围则返回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);
}
@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;
}
/**
* 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;
}
}
/**
* 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]));
}
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);
}