I'm working in another iPhone App that uses AR, and I'm creating my own framework, but I'm having trouble trying to get the angle of a second coordinate relative to the current device position, anyone know a good resource that could help me with this?
Thanks in advance!
If the two points are close enough together, and well away from the poles, you can use some simple trig:
float dy = lat2 - lat1;
float dx = cosf(M_PI/180*lat1)*(long2 - long1);
float angle = atan2f(dy, dx);
EDIT: I forgot to mention that latN and longN — and therefore dx and dy — can be in degrees or radians, so long as you don't mix units. angle, however, will always come back in radians. Of course, you can get it back to degrees if you multiply by 180/M_PI.
Here is the android version of this code
import com.google.android.maps.GeoPoint;
public double calculateAngle(GeoPoint startPoint, GeoPoint endPoint) {
double lat1 = startPoint.getLatitudeE6() / 1E6;
double lat2 = endPoint.getLatitudeE6() / 1E6;
double long2 = startPoint.getLongitudeE6() / 1E6;
double long1 = endPoint.getLongitudeE6() / 1E6;
double dy = lat2 - lat1;
double dx = Math.cos(Math.PI / 180 * lat1) * (long2 - long1);
double angle = Math.atan2(dy, dx);
return angle;
}
Related
So I was trying to follow the code in this question to get a turret that can fire ballistic projectiles with a fixed starting velocity and no drag to a given point on a 3D surface.
Find an angle to launch the projectile at to reach a specific point
But It's not quite working. The turret ends up aiming too high when the target is close, and too low when the target is further away. There is of course a specific distance at which it does hit the target but that distance is arbitrary, so that's not at all helpful to me.
The way the error scales makes me think I have a multiplication mistake, or am missing some multiplication or division, but I can't for the life of me figure out where I am going wrong. Can anyone point me in the right direction?
Code Below:
float CalculateAngle(float velocity)
{
float gravity = -Physics.gravity.y;
Vector3 modPos = target.position;
if (modPos.x < 0) modPos.x -= 2 * modPos.x;
if (modPos.y < 0) modPos.y -= 2 * modPos.y;
if (modPos.z < 0) modPos.z -= 2 * modPos.z;
modPos.x /= 10;
modPos.y /= 10;
modPos.z /= 10;
float deltaX = modPos.x - FirePoint.position.x;
float deltaZ = modPos.z - FirePoint.position.z;
float deltaY = modPos.y - FirePoint.position.y;
float horzDelta = Mathf.Sqrt(deltaX * deltaX + deltaZ * deltaZ);
float RHSFirstPart = (velocity * velocity) / (gravity * horzDelta);
float RHSSecondPart = Mathf.Sqrt(((velocity * velocity) * ((velocity * velocity) - (2 * gravity * deltaY))/ (gravity * gravity * horzDelta * horzDelta)) - 1);
float tanθ = RHSFirstPart - RHSSecondPart;
float angle = Mathf.Atan2(tanθ, 1) * Mathf.Rad2Deg;
if (angle < 0) return angle;
return -angle;
}
Edit 1:
Still struggling heavily with this. I just can't get the math to work. I went back to the original root of the knowledge here https://physics.stackexchange.com/questions/56265/how-to-get-the-angle-needed-for-a-projectile-to-pass-through-a-given-point-for-t then wrote a function that did the exact equation given in the answers, copying the input values and everything. Except when I run it it fails, as one of the values that needs to be squared is negative which throws a NaN. I assume I am going wrong somewhere in my equation but I've gone over it a hundred times and I am not spotting the error. My code:
float CalculateAngle3(float velocity)
{
float deltaX = 500;
float deltaY = 20;
float v = 100;
float vSqr = v * v;
float g = 9.81f * 9.81f;
float a = vSqr * (vSqr - 2 * g * deltaY);
float b = (g * g) * (deltaX * deltaX);
float c = a / b - 1;
float d = Mathf.Sqrt(c); //c is negitive causing an NaN
float e = vSqr / g * deltaX;
float tanθ = e - d;
return tanθ;
}
Edit 2:
Gave up. This guy solved it so I am just going to use his logic instead
: P
https://www.forrestthewoods.com/blog/solving_ballistic_trajectories/
Using it like such:
Vector3 s0;
Vector3 s1;
if (fts.solve_ballistic_arc(FirePoint.position, bomb.StartingVelocity.z, target.position, -Physics.gravity.y, out s0, out s1) > 0)
{
targetPosition = transform.position + s1;
SafetyEnabled = false;
}
else
{
//Don't fire if we don't have a solution
SafetyEnabled = true;
}
I'm going to leave the question open for now since it's still technically not answered. I still don't know why the original implementation wasn't working.
It is possible your quadratic formula is incorrect (I do not know why you did not code a separate small function that solves the quadratic equation for any three given coefficients, to make your code more readable and less prone to errors)
float RHSFirstPart = velocity / (gravity * horzDelta);
float RHSSecondPart = Mathf.Sqrt(RHSFirstPart*RHSFirstPart - 2*RHSFirstPart*deltaY/horzDelta - 1);
float tanθ = RHSFirstPart - RHSSecondPart;
A comment: In most applications we do not really need the actual angle but the values of cos(angle) and sin(angle) because these are the components of the unit vector which usually is sought (just like in your case). So no need to use inverse trigonometry to find an actual number, which slows down calculations and is may introduce unnecessary round-off errors.
I found a piece of code on web. It calculates the Minimum bounding rectangle by a given lat/lon point and a distance.
private static void GetlatLon(double LAT, double LON, double distance, double angle, out double newLon, out double newLat)
{
double dx = distance * 1000 * Math.Sin(angle * Math.PI / 180.0);
double dy = distance * 1000 * Math.Cos(angle * Math.PI / 180.0);
double ec = 6356725 + 21412 * (90.0 - LAT) / 90.0;
double ed = ec * Math.Cos(LAT * Math.PI / 180);
newLon = (dx / ed + LON * Math.PI / 180.0) * 180.0 / Math.PI;
newLat = (dy / ec + LAT * Math.PI / 180.0) * 180.0 / Math.PI;
}
public static void GetRectRange(double centorlatitude, double centorLogitude, double distance,
out double maxLatitude, out double minLatitude, out double maxLongitude, out double minLongitude)
{
GetlatLon(centorlatitude, centorLogitude, distance, 0, out temp, out maxLatitude);
GetlatLon(centorlatitude, centorLogitude, distance, 180, out temp, out minLatitude);
GetlatLon(centorlatitude, centorLogitude, distance, 90, out minLongitude, out temp);
GetlatLon(centorlatitude, centorLogitude, distance, 270, out maxLongitude, out temp);
}
double ec = 6356725 + 21412 * (90.0 - LAT) / 90.0; //why?
double ed = ec * Math.Cos(LAT * Math.PI / 180); // why?
dx / ed //why?
dy / ec //why?
6378137 is the equator radius, 6356725 is polar radius, 21412 =6378137 -6356725.
from the link, I know a little of the meanings. But these four lines, I don't know why. Could you please help to give more information? Could you please help to let me know the derivation of the formula?
From the link, in the section "Destination point given distance and bearing from start point", it gives another formula to get the result. What is the derivation of the formula?
From this link , I know the derivation of the Haversine Formula, it's very informative. I don't think the formula in the section of "Destination point given distance and bearing from start point" is just a simple reversion of Haversine.
Thanks a lot!
This is a prime example of why commenting your code makes it more readable and maintainable. Mathematically you are looking at the following:
double ec = 6356725 + 21412 * (90.0 - LAT) / 90.0; //why?
This is a measure of eccentricity to account for the equatorial bulge in some fashion. 21412 is, as you know, the difference in earth radius between the equator and pole. 6356725 is the polar radius. (90.0 - LAT) / 90.0 is 1 at the equator, and 0 at the pole. The formula simply estimates how much bulge is present at any given latitude.
double ed = ec * Math.Cos(LAT * Math.PI / 180); // why?
(LAT * Math.PI / 180) is a conversion of latitude from degrees to radians. cos (0) = 1 and cos(1) = 0, so at the equator, you are applying the full amount of the eccentricity while at the pole you are applying none. Similar to the preceding line.
dx / ed //why?
dy / ec //why?
The above seems to be the fractional additions to distance in both the x and y directions attributable to the bulge at any given lat/lon used in the newLon newLat computation to arrive at the new location.
I haven't done any research into the code snippet you found, but mathematically, this is what is taking place. Hopefully that will steer you in the right direction.
Haversine Example in C
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
double m2ft (double l) { /* convert meters to feet */
return l/(1200.0/3937.0);
}
double ft2smi (double l) { /* convert feet to statute miles*/
return l/5280.0;
}
double km2smi (double l) { /* convert km to statute mi. */
return ft2smi(m2ft( l * 1000.0 ));
}
static const double deg2rad = 0.017453292519943295769236907684886;
static const double earth_rad_m = 6372797.560856;
typedef struct pointd {
double lat;
double lon;
} pointd;
/* Computes the arc, in radian, between two WGS-84 positions.
The result is equal to Distance(from,to)/earth_rad_m
= 2*asin(sqrt(h(d/earth_rad_m )))
where:
d is the distance in meters between 'from' and 'to' positions.
h is the haversine function: h(x)=sin²(x/2)
The haversine formula gives:
h(d/R) = h(from.lat-to.lat)+h(from.lon-to.lon)+cos(from.lat)*cos(to.lat)
http://en.wikipedia.org/wiki/Law_of_haversines
*/
double arcradians (const pointd *from, const pointd *to)
{
double latitudeArc = (from-> lat - to-> lat) * deg2rad;
double longitudeArc = (from-> lon - to-> lon) * deg2rad;
double latitudeH = sin (latitudeArc * 0.5);
latitudeH *= latitudeH;
double lontitudeH = sin (longitudeArc * 0.5);
lontitudeH *= lontitudeH;
double tmp = cos (from-> lat * deg2rad) * cos (to-> lat * deg2rad);
return 2.0 * asin (sqrt (latitudeH + tmp*lontitudeH));
}
/* Computes the distance, in meters, between two WGS-84 positions.
The result is equal to earth_rad_m*ArcInRadians(from,to)
*/
double dist_m (const pointd *from, const pointd *to) {
return earth_rad_m * arcradians (from, to);
}
int main (int argc, char **argv) {
if (argc < 5 ) {
fprintf (stderr, "Error: insufficient input, usage: %s (lat,lon) (lat,lon)\n", argv[0]);
return 1;
}
pointd points[2];
points[0].lat = strtod (argv[1], NULL);
points[0].lon = strtod (argv[2], NULL);
points[1].lat = strtod (argv[3], NULL);
points[1].lon = strtod (argv[4], NULL);
printf ("\nThe distance in meters from 1 to 2 (smi): %lf\n\n", km2smi (dist_m (&points[0], &points[1])/1000.0) );
return 0;
}
/* Results/Example.
./bin/gce 31.77 -94.61 31.44 -94.698
The distance in miles from Nacogdoches to Lufkin, Texas (smi): 23.387997 miles
*/
I assume 6356725 has something to do with the radius of the earth. Check out this answer, and also take a look at the Haversine Formula.
I want to calculate the distance from one point to another point, when I start capturing accelerometer data, and till I stop that, it should gather all samples and should give me distance based on velocity, roll, pitch, yaw and displacement.
I have tried following code based on this link1 and link2 for double integration.
for (int i = 1; i < [arrXData count]; i++)
{
//========================================================================
//Old Velocity
double v0 = [[arrVelocity objectAtIndex:(i-1)] doubleValue];
//Time Data
double t0 = [[arrTimeData objectAtIndex:(i-1)] doubleValue];
double t1 = [[arrTimeData objectAtIndex:(i)] doubleValue];
//Acceleration Data
double x0 = [[arrXData objectAtIndex:(i-1)] doubleValue];
double x1 = [[arrXData objectAtIndex:(i)] doubleValue];
//NewVelocity
double v1 = v0 + (t1 - t0) * (x0 + x1)/2;
//========================================================================
//Old Distance
double d0 = [[arrDistance objectAtIndex:(i-1)] doubleValue];
//New Distance
double d1 = d0 + (x1 - x0) * (v0 + v1)/2;
//========================================================================
//Store Distance & Velocity
[arrVelocity addObject:[NSNumber numberWithDouble:v1]];
[arrDistance addObject:[NSNumber numberWithDouble:d1]];
//========================================================================
}
But the above code gives negative values sometimes, and not sure, whether this gives 100% correct result based on x acclearation values. Here we used x acceleration values after applying filter on it. We used high pass filter for reducing noise on x acceleration.
Does anyone know any algorithm on which we can rely?
Any help will be appreciated.
I have tried several different solutions but no luck so far.
- (CGPoint)contractLineTemp:(CGPoint)point :(CGPoint)circle :(float)circleRadius {
CGFloat x,y;
x = point.x - circle.x;
y = point.y - circle.y;
CGFloat theta = atan2(x, y);
CGPoint newPoint;
newPoint.x = circle.x + circleRadius * sin(theta);
newPoint.y = circle.y + circleRadius * cos(theta);
return newPoint;
}
- (CGPoint)contractLineTemp:(CGPoint)startPoint :(CGPoint)endPoint :(float)scaleBy {
float dx = endPoint.x - startPoint.x;
float dy = endPoint.y - startPoint.y;
float scale = scaleBy * Q_rsqrt(dx * dx + dy * dy);
return CGPointMake (endPoint.x - dx * scale, endPoint.y - dy * scale);
}
Both of these solutions kind of work. If I draw the line to the center of the circle you can see that it intersects the circle exactly where it should.
http://www.freeimagehosting.net/le5pi
If I use either of the solutions above and draw to the circumference of the circle depending on the angle it is no longer going towards the center of the circle. In the second image the line should be in the middle of the right edge of the circle and going straight right.
http://www.freeimagehosting.net/53ovs
http://www.freeimagehosting.net/sb3b2
Sorry for the links. I am to new to currently post images.
Thanks for you help.
It's easier to treat this as a vector problem. Your second approach is close, but you don't correctly scale the vector between the two points. It's easier to work with a normalized vector in this case, although you have to assume that the distance between the two points on the line is non-zero.
Given:
double x0 = CIRC_X0; /* x-coord of center of circle */
double y0 = CIRC_Y0; /* y-coord of center of circle */
double x1 = LINE_X1; /* x-coord of other point on the line */
double y1 = LINE_Y1; /* y-coord of other point on the line */
Then the vector between the two points is (vx,vy):
double vx = x1 - x0;
double vy = y1 - y0;
It's easier to work with a unit vector, which we can get by normalizing (vx,vy):
double vmag = sqrt(vx*vx + vy*vy);
vx /= vmag; /* Assumption is vmag > 0 */
vy /= vmag;
Now, any point along the line can be described as:
x0 + dist * vx
y0 + dist * vy
where dist is the distance from the center. The intersection of the circle and the line must be a distance of CIRC_RADIUS from the center, so:
double x_intersect = x0 + CIRC_RADIUS * vx;
double y_intersect = y0 + CIRC_RADIUS * vy;
I think that there may be a convention conflict on what theta, x and y are. The atan2 function yields values in the range -pi..pi, by taking the convention of theta as the angle growing from the X axis towards Y. However you are considering theta as the angle from Y to X.
Try changing the code:
CGFloat theta = atan2(y, x);
CGPoint newPoint;
newPoint.x = circle.x + circleRadius * cos(theta);
newPoint.y = circle.y + circleRadius * sin(theta);
Although your formulae are consistent within a coordinate system, it may have conflict with the screen/display device coordinate system.
How do I calculate the angle in degrees between the coordinates of two POIs (points of interest) on an iPhone map application?
I'm guessing you try to calculate the degrees between the coordinates of two points of interest (POI).
Calculating the arc of a great circle:
+(float) greatCircleFrom:(CLLocation*)first
to:(CLLocation*)second {
int radius = 6371; // 6371km is the radius of the earth
float dLat = second.coordinate.latitude-first.coordinate.latitude;
float dLon = second.coordinate.longitude-first.coordinate.longitude;
float a = pow(sin(dLat/2),2) + cos(first.coordinate.latitude)*cos(second.coordinate.latitude) * pow(sin(dLon/2),2);
float c = 2 * atan2(sqrt(a),sqrt(1-a));
float d = radius * c;
return d;
}
Another option is to pretend you are on cartesian coordinates (faster but not without error on long distances):
+(float)angleFromCoordinate:(CLLocationCoordinate2D)first
toCoordinate:(CLLocationCoordinate2D)second {
float deltaLongitude = second.longitude - first.longitude;
float deltaLatitude = second.latitude - first.latitude;
float angle = (M_PI * .5f) - atan(deltaLatitude / deltaLongitude);
if (deltaLongitude > 0) return angle;
else if (deltaLongitude < 0) return angle + M_PI;
else if (deltaLatitude < 0) return M_PI;
return 0.0f;
}
If you want the result in degrees instead radians, you have to apply the following conversion:
#define RADIANS_TO_DEGREES(radians) ((radians) * 180.0 / M_PI)
You are calculating the 'Bearing' from one point to another here. There's a whole bunch of formula for that, and lots of other geographic quantities like distance and cross-track error, on this web page:
http://www.movable-type.co.uk/scripts/latlong.html
the formulae are in several formats so you can easily convert to whatever language you need for your iPhone. There's also javascript calculators so you can test your code gets the same answers as theirs.
If the other solutions dont work for you try this:
- (int)getInitialBearingFrom:(CLLocation *)first
to:(CLLocation *)second
{
float lat1 = [self degreesToRad:first.coordinate.latitude];
float lat2 = [self degreesToRad:second.coordinate.latitude];
float lon1 = [self degreesToRad:first.coordinate.longitude];
float lon2 = [self degreesToRad:second.coordinate.longitude];
float dLon = lon2 - lon1;
float y = sin (dLon) * cos (lat2);
float x1 = cos (lat1) * sin (lat2);
float x2 = sin (lat1) * cos (lat2) * cos (dLon);
float x = x1 - x2;
float bearingRadRaw = atan2f (y, x);
float bearingDegRaw = bearingRadRaw * 180 / M_PI;
int bearing = ((int) bearingDegRaw + 360) % 360; // +- 180 deg to 360 deg
return bearing;
}
For final bearing, simply take the initial bearing from the end point to the start point and reverse it (using θ = (θ+180) % 360).
You need these 2 helpers:
-(float)radToDegrees:(float)radians
{
return radians * 180 / M_PI;
}
-(float)degreesToRad:(float)degrees
{
return degrees * M_PI /180;
}