private double[] rtll(double lon, double lat, double lonSp, double latSp) {
double[] all = new double[2];
double dtr = Math.PI / 180.;
double pole_lat = 90.0 + latSp;
double pole_lon = lonSp;
double ctph0 = Math.cos(pole_lat * dtr);
double stph0 = Math.sin(pole_lat * dtr);
double stph = Math.sin(lat * dtr);
double ctph = Math.cos(lat * dtr);
double ctlm = Math.cos(lon * dtr);
double stlm = Math.sin(lon * dtr);
// aph=asin(stph0.*ctph.*ctlm+ctph0.*stph);
double aph = Math.asin((stph0 * ctph * ctlm) + (ctph0 * stph));
// cph=cos(aph);
double cph = Math.cos(aph);
// almd=tlm0d+asin(stlm.*ctph./cph)/dtr;
all[0] = pole_lon + (Math.asin((stlm * ctph) / cph) / dtr);
// aphd=aph/dtr;
all[1] = aph / dtr;
return all;
}
@Override
public void onSensorChanged(SensorEvent e) {
// make sure all players have the same speed
long now = System.currentTimeMillis();
if ((now - lastSensorEvent) < SENSOR_REFRESH_LIMIT) {
return;
}
lastSensorEvent = now;
float x = e.values[0];
float y = e.values[1];
float z = e.values[2];
// http://www.anddev.org/code-snippets-for-android-f33/convert-android-accelerometer-values-and-get-tilt-from-accel-t6595.html
// http://www.hobbytronics.co.uk/accelerometer-info
double accX = -x / SensorManager.GRAVITY_EARTH;
double accY = -y / SensorManager.GRAVITY_EARTH;
double accZ = z / SensorManager.GRAVITY_EARTH;
double totAcc = Math.sqrt((accX * accX) + (accY * accY) + (accZ * accZ));
// tiltXYZ: returned angle is in the range -pi/2 through pi/2
double tiltX = Math.asin(accX / totAcc);
double tiltY = Math.asin(accY / totAcc);
// double tiltZ = Math.asin(accZ / totAcc);
// TODO [veny] there should be Strategy design pattern to calculate the speed of movement
// (int)(tiltX * 2 / Math.PI) - is increment (-1 to 1, 0 is no move)
double speedX = (tiltX * 2 / Math.PI);
double speedY = (-tiltY * 2 / Math.PI);
if (0 != speedX || 0 != speedY) {
Game.getInstance().moveMe(speedX, speedY);
}
}
public Point2D.Double projectInverse(double xyx, double xyy, Point2D.Double out) {
double t = 0;
double lpphi = RYC * xyy;
if (Math.abs(lpphi) > 1.) {
if (Math.abs(lpphi) > ONETOL) {
throw new ProjectionException("I");
} else if (lpphi < 0.) {
t = -1.;
lpphi = -Math.PI;
} else {
t = 1.;
lpphi = Math.PI;
}
} else {
lpphi = 2. * Math.asin(t = lpphi);
}
out.x = RXC * xyx / (1. + 2. * Math.cos(lpphi) / Math.cos(0.5 * lpphi));
lpphi = RC * (t + Math.sin(lpphi));
if (Math.abs(lpphi) > 1.) {
if (Math.abs(lpphi) > ONETOL) {
throw new ProjectionException("I");
} else {
lpphi = lpphi < 0. ? -MapMath.HALFPI : MapMath.HALFPI;
}
} else {
lpphi = Math.asin(lpphi);
}
out.y = lpphi;
return out;
}
/**
* 获取旋转某个角度之后的点
*
* @param viewCenter
* @param source
* @param degree
* @return
*/
public static PointF obtainRoationPoint(PointF center, PointF source, float degree) {
// 两者之间的距离
PointF disPoint = new PointF();
disPoint.x = source.x - center.x;
disPoint.y = source.y - center.y;
// 没旋转之前的弧度
double originRadian = 0;
// 没旋转之前的角度
double originDegree = 0;
// 旋转之后的角度
double resultDegree = 0;
// 旋转之后的弧度
double resultRadian = 0;
// 经过旋转之后点的坐标
PointF resultPoint = new PointF();
double distance = Math.sqrt(disPoint.x * disPoint.x + disPoint.y * disPoint.y);
if (disPoint.x == 0 && disPoint.y == 0) {
return center;
// 第一象限
} else if (disPoint.x >= 0 && disPoint.y >= 0) {
// 计算与x正方向的夹角
originRadian = Math.asin(disPoint.y / distance);
// 第二象限
} else if (disPoint.x < 0 && disPoint.y >= 0) {
// 计算与x正方向的夹角
originRadian = Math.asin(Math.abs(disPoint.x) / distance);
originRadian = originRadian + Math.PI / 2;
// 第三象限
} else if (disPoint.x < 0 && disPoint.y < 0) {
// 计算与x正方向的夹角
originRadian = Math.asin(Math.abs(disPoint.y) / distance);
originRadian = originRadian + Math.PI;
} else if (disPoint.x >= 0 && disPoint.y < 0) {
// 计算与x正方向的夹角
originRadian = Math.asin(disPoint.x / distance);
originRadian = originRadian + Math.PI * 3 / 2;
}
// 弧度换算成角度
originDegree = radianToDegree(originRadian);
resultDegree = originDegree + degree;
// 角度转弧度
resultRadian = degreeToRadian(resultDegree);
resultPoint.x = (int) Math.round(distance * Math.cos(resultRadian));
resultPoint.y = (int) Math.round(distance * Math.sin(resultRadian));
resultPoint.x += center.x;
resultPoint.y += center.y;
return resultPoint;
}
@Override
protected void transformInverse(int x, int y, float[] out) {
float dx = x - icentreX;
float dy = y - icentreY;
float x2 = dx * dx;
float y2 = dy * dy;
if (y2 >= (b2 - (b2 * x2) / a2)) {
out[0] = x;
out[1] = y;
} else {
float rRefraction = 1.0f / refractionIndex;
float z = (float) Math.sqrt((1.0f - x2 / a2 - y2 / b2) * (a * b));
float z2 = z * z;
float xAngle = (float) Math.acos(dx / Math.sqrt(x2 + z2));
float angle1 = ImageMath.HALF_PI - xAngle;
float angle2 = (float) Math.asin(Math.sin(angle1) * rRefraction);
angle2 = ImageMath.HALF_PI - xAngle - angle2;
out[0] = x - (float) Math.tan(angle2) * z;
float yAngle = (float) Math.acos(dy / Math.sqrt(y2 + z2));
angle1 = ImageMath.HALF_PI - yAngle;
angle2 = (float) Math.asin(Math.sin(angle1) * rRefraction);
angle2 = ImageMath.HALF_PI - yAngle - angle2;
out[1] = y - (float) Math.tan(angle2) * z;
}
}
private void getTrackPath(float x, float y, float radius) {
float halfStroke = mTrackSize / 2f;
mTrackPath.reset();
if (mTrackCap != Paint.Cap.ROUND) {
mTempRect.set(x - radius + 1f, y - radius + 1f, x + radius - 1f, y + radius - 1f);
float angle = (float) (Math.asin(halfStroke / (radius - 1f)) / Math.PI * 180);
if (x - radius > mDrawRect.left) {
mTrackPath.moveTo(mDrawRect.left, y - halfStroke);
mTrackPath.arcTo(mTempRect, 180 + angle, -angle * 2);
mTrackPath.lineTo(mDrawRect.left, y + halfStroke);
mTrackPath.close();
}
if (x + radius < mDrawRect.right) {
mTrackPath.moveTo(mDrawRect.right, y - halfStroke);
mTrackPath.arcTo(mTempRect, -angle, angle * 2);
mTrackPath.lineTo(mDrawRect.right, y + halfStroke);
mTrackPath.close();
}
} else {
float angle = (float) (Math.asin(halfStroke / (radius - 1f)) / Math.PI * 180);
if (x - radius > mDrawRect.left) {
float angle2 =
(float)
(Math.acos(Math.max(0f, (mDrawRect.left + halfStroke - x + radius) / halfStroke))
/ Math.PI
* 180);
mTempRect.set(mDrawRect.left, y - halfStroke, mDrawRect.left + mTrackSize, y + halfStroke);
mTrackPath.arcTo(mTempRect, 180 - angle2, angle2 * 2);
mTempRect.set(x - radius + 1f, y - radius + 1f, x + radius - 1f, y + radius - 1f);
mTrackPath.arcTo(mTempRect, 180 + angle, -angle * 2);
mTrackPath.close();
}
if (x + radius < mDrawRect.right) {
float angle2 =
(float)
Math.acos(Math.max(0f, (x + radius - mDrawRect.right + halfStroke) / halfStroke));
mTrackPath.moveTo(
(float) (mDrawRect.right - halfStroke + Math.cos(angle2) * halfStroke),
(float) (y + Math.sin(angle2) * halfStroke));
angle2 = (float) (angle2 / Math.PI * 180);
mTempRect.set(
mDrawRect.right - mTrackSize, y - halfStroke, mDrawRect.right, y + halfStroke);
mTrackPath.arcTo(mTempRect, angle2, -angle2 * 2);
mTempRect.set(x - radius + 1f, y - radius + 1f, x + radius - 1f, y + radius - 1f);
mTrackPath.arcTo(mTempRect, -angle, angle * 2);
mTrackPath.close();
}
}
}
private Path getOutline(float scale) {
RectF outerBB = new RectF(-mOuter * scale, -mOuter * scale, mOuter * scale, mOuter * scale);
RectF innerBB = new RectF(-mInner * scale, -mInner * scale, mInner * scale, mInner * scale);
double gamma = (mInner + mOuter) * Math.sin(Math.toRadians(mGap / 2.0f));
float alphaOuter = (float) Math.toDegrees(Math.asin(gamma / (mOuter * 2.0f)));
float alphaInner = (float) Math.toDegrees(Math.asin(gamma / (mInner * 2.0f)));
Path path = new Path();
path.arcTo(outerBB, mStart + alphaOuter, mSweep - 2 * alphaOuter, true);
path.arcTo(innerBB, mStart + mSweep - alphaInner, 2 * alphaInner - mSweep);
path.close();
return path;
}
/**
* check if the sphere is (at least partially) visible
*
* @param center sphere center
* @param radius sphere radius
*/
private void checkSphereVisible(Coords center, double radius) {
double frustumRadius = getView3D().getFrustumRadius();
Coords origin = getView3D().getCenter();
Coords v = origin.sub(center);
v.calcNorm();
double centersDistance = v.getNorm();
if (centersDistance > radius + frustumRadius) { // sphere totally
// outside the frustum
visible = Visible.TOTALLY_OUTSIDE;
} else if (centersDistance + frustumRadius < radius) { // frustum
// totally
// inside the
// sphere
visible = Visible.TOTALLY_OUTSIDE;
} else if (centersDistance + radius < frustumRadius) { // totally inside
visible = Visible.TOTALLY_INSIDE;
} else if (centersDistance < frustumRadius) { // center inside
visible = Visible.CENTER_INSIDE;
} else {
// calc angles to draw minimum longitudes
double horizontalDistance = Math.sqrt(v.getX() * v.getX() + v.getY() * v.getY());
if (horizontalDistance > frustumRadius) {
alpha = Math.asin(frustumRadius / horizontalDistance);
beta = Math.atan2(v.getY(), v.getX());
// App.debug("alpha = "+(alpha*180/Math.PI)+"degrees, beta =
// "+(beta*180/Math.PI)+"degrees");
visible = Visible.CENTER_OUTSIDE; // center outside
} else {
visible = Visible.CENTER_INSIDE; // do as if center inside
}
}
}
/**
* Computes the bounding coordinates of all points on the surface of a sphere that have a great
* circle distance to the point represented by this GeoLocation instance that is less or equal to
* the distance argument.
*
* <p>For more information about the formulae used in this method visit <a
* href="http://JanMatuschek.de/LatitudeLongitudeBoundingCoordinates">
* http://JanMatuschek.de/LatitudeLongitudeBoundingCoordinates</a>.
*
* @param distance the distance from the point represented by this GeoLocation instance. Must me
* measured in the same unit as the radius argument.
* @param radius the radius of the sphere, e.g. the average radius for a spherical approximation
* of the figure of the Earth is approximately 6371.01 kilometers.
* @return an array of two GeoLocation objects such that:
* <ul>
* <li>The latitude of any point within the specified distance is greater or equal to the
* latitude of the first array element and smaller or equal to the latitude of the
* second array element.
* <li>If the longitude of the first array element is smaller or equal to the longitude of
* the second element, then the longitude of any point within the specified distance is
* greater or equal to the longitude of the first array element and smaller or equal to
* the longitude of the second array element.
* <li>If the longitude of the first array element is greater than the longitude of the
* second element (this is the case if the 180th meridian is within the distance), then
* the longitude of any point within the specified distance is greater or equal to the
* longitude of the first array element <strong>or</strong> smaller or equal to the
* longitude of the second array element.
* </ul>
*/
public GeoLocation[] boundingCoordinates(double distance, double radius) {
if (radius < 0d || distance < 0d) throw new IllegalArgumentException();
// angular distance in radians on a great circle
double radDist = distance / radius;
double minLat = radLat - radDist;
double maxLat = radLat + radDist;
double minLon, maxLon;
if (minLat > MIN_LAT && maxLat < MAX_LAT) {
double deltaLon = Math.asin(Math.sin(radDist) / Math.cos(radLat));
minLon = radLon - deltaLon;
if (minLon < MIN_LON) minLon += 2d * Math.PI;
maxLon = radLon + deltaLon;
if (maxLon > MAX_LON) maxLon -= 2d * Math.PI;
} else {
// a pole is within the distance
minLat = Math.max(minLat, MIN_LAT);
maxLat = Math.min(maxLat, MAX_LAT);
minLon = MIN_LON;
maxLon = MAX_LON;
}
return new GeoLocation[] {fromRadians(minLat, minLon), fromRadians(maxLat, maxLon)};
}
private static void TanSinTan(
int y0,
int x0,
int sin1,
int y2,
int x2,
int cos0a,
int cos0b,
int sin0a,
int sin0b,
DenseMatrix64F R,
float euler[]) {
float val_y0 = get(R, y0);
float val_x0 = get(R, x0);
float val_sin1 = get(R, sin1);
float val_y2 = get(R, y2);
float val_x2 = get(R, x2);
if (1.0f - Math.abs(val_sin1) <= GrlConstants.F_EPS) {
float sign = (float) Math.signum(val_sin1);
float sin0 = (get(R, sin0a) + sign * get(R, sin0b)) / 2.0f;
float cos0 = (get(R, cos0a) + sign * get(R, cos0b)) / 2.0f;
euler[0] = (float) Math.atan2(sin0, cos0);
euler[1] = sign * (float) Math.PI / 2.0f;
euler[2] = 0;
} else {
euler[0] = (float) Math.atan2(val_y0, val_x0);
euler[1] = (float) Math.asin(val_sin1);
euler[2] = (float) Math.atan2(val_y2, val_x2);
}
}
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);
}
/**
* Calculates range for wheel items
*
* @return the items range
*/
private ItemsRange getItemsRange() {
if (getItemHeight() == 0) {
return null;
}
int first = currentItem;
int count = 1;
while (count * getItemHeight() < getHeight()) {
first--;
count += 2; // top + bottom items
}
if (scrollingOffset != 0) {
if (scrollingOffset > 0) {
first--;
}
count++;
// process empty items above the first or below the second
int emptyItems = scrollingOffset / getItemHeight();
first -= emptyItems;
count += Math.asin(emptyItems);
}
return new ItemsRange(first, count);
}
private double getDistance(
double submittedCountryLat,
double submittedCountryLong,
double countryLat,
double countryLong) {
double distance = 0;
double submittedCountryLatRadian = Math.toRadians(submittedCountryLat);
double submittedCountryLongRadian = Math.toRadians(submittedCountryLong);
double countryLatRadian = Math.toRadians(countryLat);
double countryLongRadian = Math.toRadians(countryLong);
double absoluteDistanceLat = submittedCountryLatRadian - countryLatRadian;
double absoluteDistanceLong = submittedCountryLongRadian - countryLongRadian;
distance =
Math.sin(absoluteDistanceLat / 2) * Math.sin(absoluteDistanceLat / 2)
+ Math.cos(submittedCountryLatRadian)
* Math.cos(countryLatRadian)
* Math.sin(absoluteDistanceLong / 2)
* Math.sin(absoluteDistanceLong / 2);
distance = 2 * Math.asin(Math.sqrt(distance));
return distance * 6371;
}
@Test
public void testAsin() {
for (double doubleValue : DOUBLE_VALUES) {
assertFunction("asin(" + doubleValue + ")", DOUBLE, Math.asin(doubleValue));
}
assertFunction("asin(NULL)", DOUBLE, null);
}
public static Point calcAngles(Point target) {
// todo checks
double d = target.distance(ATTACH_POINT);
double d2 = d / 2;
double sinb = d2 / ARM_LENGTH;
double b = Math.asin(sinb) * 2;
b *= (180 / Math.PI);
int dx = target.x - ATTACH_POINT.x;
int dy = target.y - ATTACH_POINT.y;
double a1 = Math.atan((double) dx / dy);
a1 *= (180 / Math.PI);
if (a1 < 0) {
a1 = -a1;
} else {
a1 = 180 - a1;
}
double a = a1 + (180 - b) / 2;
Point p = new Point();
p.x = (int) a;
p.y = (int) b;
System.out.println(
"Point to Angles: P(" + target.x + "," + target.y + ") -> A(" + p.x + "," + p.y + ")");
/*System.out.print("{" + target.x + "," + target.y + "},{" + p.x + "," + p.y + "},");*/
return p;
}
/**
* Draws the needle.
*
* @param g2 the graphics device.
* @param plotArea the plot area.
* @param rotate the rotation point.
* @param angle the angle.
*/
protected void drawNeedle(Graphics2D g2, Rectangle2D plotArea,
Point2D rotate, double angle) {
Arc2D shape = new Arc2D.Double(Arc2D.PIE);
double radius = plotArea.getHeight();
double halfX = plotArea.getWidth() / 2;
double diameter = 2 * radius;
shape.setFrame(plotArea.getMinX() + halfX - radius ,
plotArea.getMinY() - radius,
diameter, diameter);
radius = Math.toDegrees(Math.asin(halfX / radius));
shape.setAngleStart(270 - radius);
shape.setAngleExtent(2 * radius);
Area s = new Area(shape);
if ((rotate != null) && (angle != 0)) {
/// we have rotation houston, please spin me
getTransform().setToRotation(angle, rotate.getX(), rotate.getY());
s.transform(getTransform());
}
defaultDisplay(g2, s);
}
public double calcAngle(int lon0, int lat0, int lon1, int lat1, int lon2, int lat2) {
double dlat1 = (lat1 - lat0);
double dlon1 = (lon1 - lon0) * coslat;
double dlat2 = (lat2 - lat1);
double dlon2 = (lon2 - lon1) * coslat;
double dd = Math.sqrt((dlat1 * dlat1 + dlon1 * dlon1) * (dlat2 * dlat2 + dlon2 * dlon2));
if (dd == 0.) return 0.;
double sinp = (dlat1 * dlon2 - dlon1 * dlat2) / dd;
double cosp = (dlat1 * dlat2 + dlon1 * dlon2) / dd;
double p;
if (sinp > -0.7 && sinp < 0.7) {
p = Math.asin(sinp) * 57.3;
if (cosp < 0.) {
p = 180. - p;
}
} else {
p = Math.acos(cosp) * 57.3;
if (sinp < 0.) {
p = -p;
}
}
if (p > 180.) {
p -= 360.;
}
return p;
}
/**
* @param point point to rotate
* @param pointCoords coordinates of this point
* @param projectCoords coordinates of the projected point on bottom face
* @param o coordinates of the origin of the rotation line
* @param vs direction of the rotation line
* @param f value of the cursor used in the rotation
* @param fd direction of the bottom face
* @param dist distance between point and projected point
* @param test value (XOR)
*/
protected void rotate(
GeoPoint3D point,
Coords pointCoords,
Coords projectCoords,
Coords o,
Coords vs,
double f,
Coords fd,
double dist,
boolean test) {
Coords v2 = projectCoords.sub(o);
double d2 = pointCoords.distLine(o, vs);
double angle;
if (Kernel.isEqual(dist, d2)) {
angle = Math.PI / 2;
} else {
angle = Math.asin(dist / d2);
}
if (test ^ (v2.crossProduct(vs).dotproduct(fd) < 0)) { // top point is
// inside bottom
// face
angle = Math.PI - angle;
}
point.rotate(f * angle, o, vs);
}
@Test
public void testOperators() {
try {
assertEquals(2d, Expression.evaluate("1 + 1").toDouble(), FuzzyEquals.TOLERANCE);
assertEquals(0d, Expression.evaluate("1 - 1").toDouble(), FuzzyEquals.TOLERANCE);
assertEquals(2d, Expression.evaluate("1 * 2").toDouble(), FuzzyEquals.TOLERANCE);
assertEquals(2d, Expression.evaluate("6 / 3").toDouble(), FuzzyEquals.TOLERANCE);
assertEquals(2d, Expression.evaluate("6 % 4").toDouble(), FuzzyEquals.TOLERANCE);
assertTrue(Expression.evaluate("true && true").toBoolean());
assertFalse(Expression.evaluate("true AND false").toBoolean());
assertTrue(Expression.evaluate("true || true").toBoolean());
assertTrue(Expression.evaluate("true OR false").toBoolean());
assertFalse(Expression.evaluate("not(true)").toBoolean());
assertEquals(6d, Expression.evaluate("max(2,3,6)").toDouble(), FuzzyEquals.TOLERANCE);
assertEquals(2d, Expression.evaluate("min(2,3,6)").toDouble(), FuzzyEquals.TOLERANCE);
assertEquals(2d, Expression.evaluate("floor(2.2)").toDouble(), FuzzyEquals.TOLERANCE);
assertEquals(4d, Expression.evaluate("ceil(3.6)").toDouble(), FuzzyEquals.TOLERANCE);
assertEquals(-3d, Expression.evaluate("neg(3)").toDouble(), FuzzyEquals.TOLERANCE);
assertEquals(3d, Expression.evaluate("abs(neg(3))").toDouble(), FuzzyEquals.TOLERANCE);
assertEquals(
Math.cos(Math.toRadians(180)),
Expression.evaluate("cos(rad(180))").toDouble(),
FuzzyEquals.TOLERANCE);
assertEquals(
Math.sin(Math.toRadians(90)),
Expression.evaluate("sin(rad(90))").toDouble(),
FuzzyEquals.TOLERANCE);
assertEquals(
Math.tan(Math.toRadians(45)),
Expression.evaluate("tan(rad(45))").toDouble(),
FuzzyEquals.TOLERANCE);
assertEquals(Math.acos(0), Expression.evaluate("acos(0)").toDouble(), FuzzyEquals.TOLERANCE);
assertEquals(
Math.asin(180), Expression.evaluate("asin(180)").toDouble(), FuzzyEquals.TOLERANCE);
assertEquals(Math.atan(1), Expression.evaluate("atan(1)").toDouble(), FuzzyEquals.TOLERANCE);
assertTrue(Expression.evaluate("1 == 1").toBoolean());
assertTrue(Expression.evaluate("1 > 0").toBoolean());
assertTrue(Expression.evaluate("1 < 2").toBoolean());
assertTrue(Expression.evaluate("1 <= 2").toBoolean());
assertTrue(Expression.evaluate("1 >= 1").toBoolean());
assertEquals("b", Expression.evaluate("substr(abcd,1,2)").toString());
Expression exp = new Expression("dateDifference(01/01/2006, 01/05/2006, |DAY|)");
exp.setResolver(new SimpleResolver());
assertEquals(4d, exp.evaluate().toDouble(), FuzzyEquals.TOLERANCE);
exp = new Expression("max(1, 2, NULL)");
exp.setResolver(new SimpleResolver());
assertEquals("max(1, 2, NULL)", exp.evaluate().toString());
} catch (InvalidExpressionException e) {
e.printStackTrace();
fail();
}
}
/** {@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 angle in degrees from specified origin to specified destination. */
public static int getAngle(Point origin, Point destination) // origin != destination
{
double distance = getDistance(origin, destination);
int add = 0;
int x = destination.getx() - origin.getx();
int y = origin.gety() - destination.gety(); // Java flips things around
double angleRad = Math.asin(Math.abs(y) / distance);
double angleDeg = Math.toDegrees(angleRad);
if ((x >= 0) && (y >= 0)) // Quadrant 1
{
angleDeg = angleDeg;
}
if ((x < 0) && (y > 0)) // Quadrant 2
{
angleDeg = 180 - angleDeg;
}
if ((x <= 0) && (y <= 0)) // Quadrant 3
{
angleDeg = 180 + angleDeg;
}
if ((x > 0) && (y < 0)) // Quadrant 4
{
angleDeg = 360 - angleDeg;
}
float angleFloat = Math.round(angleDeg);
int angleInt = Math.round(angleFloat);
return (angleInt);
}
public void process() {
if (in instanceof VSDouble) {
out.setValue(Math.asin(in.getValue()));
out.setChanged(true);
element.notifyPin(0);
}
}
// 根据起始点和目标点的位置，更新方向值
private void calcDirection() {
double dx = directionX - position.x;
double dy = position.y - directionY;
double dist = Math.sqrt(dx * dx + dy * dy);
double angle = Math.asin(Math.abs(dy) / dist);
if (dx > 0) {
if (dy > 0) {
// angle = angle;
} else {
angle = Math.PI * 2 - angle;
}
} else {
if (dy > 0) {
angle = Math.PI - angle;
} else {
angle = Math.PI + angle;
}
}
int iangle = (int) ((angle / Math.PI) * 180);
if (iangle < 0) {
iangle = 0;
}
if (iangle >= 360) {
iangle = 359;
}
if (position.direction >= 1000) {
position.direction = 1000 + iangle;
} else {
position.direction = iangle;
}
}
private void fillBuffer() {
int curfreq = frequency;
if (curfreq < 1300) curfreq = 1300;
if (curfreq > 9400) curfreq = 9400;
if (curfreq != prevfrequency) {
double tempSignalValue = Math.sin(2 * Math.PI * prevfrequency * time / sampleRate);
double shift = Math.asin(tempSignalValue);
if ((short) (32767.0 * tempSignalValue) < lastSignalValue) {
shift = Math.PI - shift;
}
time = 0;
for (int i = 0; i < buffer1.length; i++) {
buffer1[i] =
(short) (32767.0 * Math.sin(2 * Math.PI * curfreq * time / sampleRate + shift));
time++;
if (time < 0) time = 0;
}
} else {
for (int i = 0; i < buffer1.length; i++) {
buffer1[i] = (short) (32767.0 * Math.sin(2 * Math.PI * curfreq * time / sampleRate));
time++;
if (time < 0) time = 0;
}
}
lastSignalValue = buffer1[buffer1.length - 1];
prevfrequency = curfreq;
}
// This method computes all principal angles using a sine based algorithm.
// Small angles are computed more accurately than larger angles
public static double[] getPrincipleAngles(DenseMatrix X, DenseMatrix Y) {
double[] s = new double[0];
if (X.numColumns() == 0 || Y.numColumns() == 0) return s;
DenseMatrix QX;
DenseMatrix QY;
// Orthonormalize X and Y
if (X.numColumns() < Y.numColumns()) { // Switch X and Y in this case
QX = Utilities.orth(Y);
QY = Utilities.orth(X);
} else {
QX = Utilities.orth(X);
QY = Utilities.orth(Y);
}
// Compute QY=QY-QX*(QX^T*QY)
DenseMatrix Z = new DenseMatrix(QX.numRows(), QY.numColumns());
DenseMatrix Temp1 = new DenseMatrix(QX.numColumns(), QX.numRows());
DenseMatrix Temp2 = new DenseMatrix(QX.numColumns(), QY.numColumns());
QX.transpose(Temp1); // Temp1=QX^T
Temp1.mult(QY, Temp2); // Temp2=Temp1*QY=QX^T*QY
QX.mult(Temp2, Z);
QY.add(-1D, Z);
// Now compute the singular values
try {
SVD svd = SVD.factorize(QY);
s = svd.getS();
} catch (NotConvergedException e) {
}
// Return angles in radians
for (int i = 0; i < s.length; ++i) s[i] = Math.asin(s[i]);
return s;
}
@Override
public float getPercentageDone(
float pSecondsElapsed, final float pDuration, final float pMinValue, final float pMaxValue) {
float s;
float p = 0.0f;
float a = 0.0f;
if (pSecondsElapsed == 0) {
return pMinValue;
}
if ((pSecondsElapsed /= pDuration) == 1) {
return pMinValue + pMaxValue;
}
if (p == 0) {
p = pDuration * 0.3f;
}
if (a == 0 || (pMaxValue > 0 && a < pMaxValue) || (pMaxValue < 0 && a < -pMaxValue)) {
a = pMaxValue;
s = p / 4;
} else {
s = (float) (p / PI_TWICE * Math.asin(pMaxValue / a));
}
return (float)
(-(a
* Math.pow(2, 10 * (pSecondsElapsed -= 1))
* FloatMath.sin((pSecondsElapsed * pDuration - s) * PI_TWICE / p))
+ pMinValue);
}
public double calculationByDistance(GeoPoint gpOrigem, GeoPoint gpDestino) {
int Radius = 6371; // radius of earth in Km
double c = 0f;
if (gpOrigem != null && gpDestino != null) {
double lat1 = gpOrigem.getLatitudeE6() / 1E6;
double lat2 = gpDestino.getLatitudeE6() / 1E6;
double lon1 = gpOrigem.getLongitudeE6() / 1E6;
double lon2 = gpDestino.getLongitudeE6() / 1E6;
double dLat = Math.toRadians(lat2 - lat1);
double dLon = Math.toRadians(lon2 - lon1);
double a =
Math.sin(dLat / 2) * Math.sin(dLat / 2)
+ Math.cos(Math.toRadians(lat1))
* Math.cos(Math.toRadians(lat2))
* Math.sin(dLon / 2)
* Math.sin(dLon / 2);
c = 2 * Math.asin(Math.sqrt(a));
double valueResult = Radius * c;
double km = valueResult / 1;
DecimalFormat newFormat = new DecimalFormat("####");
Integer kmInDec = Integer.valueOf(newFormat.format(km));
double meter = valueResult % 1000;
double meterInDec = Integer.valueOf(newFormat.format(meter));
Log.i("Radius Value", "" + valueResult + " KM " + kmInDec + " Meter " + meterInDec);
}
return Radius * c;
}
@Override
public void onOrientationData(Myo myo, long timestamp, Quaternion rotation) {
Quaternion normalized = rotation.normalized();
double roll =
Math.atan2(
2.0f * (normalized.getW() * normalized.getX() + normalized.getY() * normalized.getZ()),
1.0f
- 2.0f
* (normalized.getX() * normalized.getX()
+ normalized.getY() * normalized.getY()));
double pitch =
Math.asin(
2.0f * (normalized.getW() * normalized.getY() - normalized.getZ() * normalized.getX()));
double yaw =
Math.atan2(
2.0f * (normalized.getW() * normalized.getZ() + normalized.getX() * normalized.getY()),
1.0f
- 2.0f
* (normalized.getY() * normalized.getY()
+ normalized.getZ() * normalized.getZ()));
rollW = ((roll + Math.PI) / (Math.PI * 2.0) * SCALE);
pitchW = ((pitch + Math.PI / 2.0) / Math.PI * SCALE);
yawW = ((yaw + Math.PI) / (Math.PI * 2.0) * SCALE);
OrientationData orientation = new OrientationData();
orientation.rollW = rollW;
orientation.pitchW = pitchW;
orientation.yawW = yawW;
System.out.println(orientation.rollW);
}
/**
* Gets the angle between Vector 0,0,1 and the forward vector in the horizontal plane
*
* @return Angle in degrees
*/
public double getBearing() {
Vector forward = this.orientation.getForward();
double angle = Math.toDegrees(Math.asin(forward.x));
if (forward.z < 0 && angle < 0) return -180 - angle;
if (forward.z < 0) return 180 - angle;
return angle;
}
@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;
}
}