Related
I'm currently trying to filter the depth information using OpenCV. For that reason I need to transform Project Tango's depth information XYZij into a image like depthmap. (Like the output of Microsoft Kinect) Unfortunately the official APIs lacking the ij part of XYZij. That's why I'm trying to project the XYZ part using the camera intrinsics projection, wich is explained in the official C API Dokumentation. My current approach looks like this:
float fx = static_cast<float>(ccIntrinsics.fx);
float fy = static_cast<float>(ccIntrinsics.fy);
float cx = static_cast<float>(ccIntrinsics.cx);
float cy = static_cast<float>(ccIntrinsics.cy);
float k1 = static_cast<float>(ccIntrinsics.distortion[0]);
float k2 = static_cast<float>(ccIntrinsics.distortion[1]);
float k3 = static_cast<float>(ccIntrinsics.distortion[2]);
for (int k = 0; k < xyz_ij->xyz_count; ++k) {
float X = xyz_ij->xyz[k][0];
float Y = xyz_ij->xyz[k][1];
float Z = xyz_ij->xyz[k][2];
float ru = sqrt((pow(X, 2) + pow(Y, 2)) / pow(Z, 2));
float rd = ru + k1 * pow(ru, 3) + k2 * pow(ru, 5) + k3 * pow(ru, 7);
int x = X / Z * fx * rd / ru + cx;
int y = X / Z * fy * rd / ru + cy;
// drawing into OpenCV Mat in red
depth.at<cv::Vec3b>(x, y)[0] = 240;
}
The resulting depthmap can be seen in the lower right corner. But it seems that this calculation result in a linear representation ... Does anyone has already done something similar? Are the XYZ points already correct positioned for this projection?
I have actually found a solution ... Just skipped the distortion calculation like they do in the rgb-depth-sync-example. My code now looks like this:
float fx = static_cast<float>(ccIntrinsics.fx);
float fy = static_cast<float>(ccIntrinsics.fy);
float cx = static_cast<float>(ccIntrinsics.cx);
float cy = static_cast<float>(ccIntrinsics.cy);
int width = static_cast<int>(ccIntrinsics.width);
int height = static_cast<int>(ccIntrinsics.height);
for (int k = 0; k < xyz_ij->xyz_count; ++k) {
float X = xyz_ij->xyz[k * 3][0];
float Y = xyz_ij->xyz[k * 3][1];
float Z = xyz_ij->xyz[k * 3][2];
int x = static_cast<int>(fx * (X / Z) + cx);
int y = static_cast<int>(fy * (Y / Z) + cy);
uint8_t depth_value = UCHAR_MAX - ((Z * 1000) * UCHAR_MAX / 4500);
cv::Point point(y % height, x % width);
line(depth, point, point, cv::Scalar(depth_value, depth_value, depth_value), 4.5);
}
And the working OpenCV result looks like this:
I have the following code to calculate points between four control points to generate a catmull-rom curve:
CGPoint interpolatedPosition(CGPoint p0, CGPoint p1, CGPoint p2, CGPoint p3, float t)
{
float t3 = t * t * t;
float t2 = t * t;
float f1 = -0.5 * t3 + t2 - 0.5 * t;
float f2 = 1.5 * t3 - 2.5 * t2 + 1.0;
float f3 = -1.5 * t3 + 2.0 * t2 + 0.5 * t;
float f4 = 0.5 * t3 - 0.5 * t2;
float x = p0.x * f1 + p1.x * f2 + p2.x * f3 + p3.x * f4;
float y = p0.y * f1 + p1.y * f2 + p2.y * f3 + p3.y * f4;
return CGPointMake(x, y);
}
This works fine, but I want to create something I think is called centripetal parameterization. This means that the curve will have no cusps and no self-intersections. If I move one control point really close to another one, the curve should become "smaller". I have Googled my eyes off trying to find a way to do this. Anyone know how to do this?
I needed to implement this for work as well. The fundamental concept you need to start with is that the main difference between the regular Catmull-Rom implementation and the modified versions is how they treat time.
In the unparameterized version from your original Catmull-Rom implementation, t starts at 0 and ends with 1 and calculates the curve from P1 to P2. In the parameterized time implementation, t starts with 0 at P0, and keeps increasing across all four points. So in the uniform case, it would be 1 at P1 and 2 at P2, and you would pass in values ranging from 1 to 2 for your interpolation.
The chordal case shows |Pi+1 - P| as the time span change. This just means that you can use the straight line distance between the points of each segment to calculate the actual length to use. The centripetal case just uses a slightly different method for calculating the optimal length of time to use for each segment.
So now we just need to know how to come up with equations that will let us plug in our new time values. The typical Catmull-Rom equation only has one t in it, the time you are trying to calculate a value for. I found the best article for describing how those parameters are calculated here: http://www.cemyuksel.com/research/catmullrom_param/catmullrom.pdf. They were focusing on a mathematical evaluation of the curves, but in it lies the crucial formula from Barry and Goldman.(1)
In the diagram above, the arrows mean "multiplied by" the ratio given in the arrow.
This then gives us what we need to actually perform a calculation to get the desired result. X and Y are calculated independently, although I used the "Distance" factor for modifying time based on the 2D distance, and not the 1D distance.
Test results:
(1) P. J. Barry and R. N. Goldman. A recursive evaluation algorithm for a class of catmull-rom splines. SIGGRAPH Computer Graphics, 22(4):199{204, 1988.
The source code for my final implementation in Java looks as follows:
/**
* This method will calculate the Catmull-Rom interpolation curve, returning
* it as a list of Coord coordinate objects. This method in particular
* adds the first and last control points which are not visible, but required
* for calculating the spline.
*
* #param coordinates The list of original straight line points to calculate
* an interpolation from.
* #param pointsPerSegment The integer number of equally spaced points to
* return along each curve. The actual distance between each
* point will depend on the spacing between the control points.
* #return The list of interpolated coordinates.
* #param curveType Chordal (stiff), Uniform(floppy), or Centripetal(medium)
* #throws gov.ca.water.shapelite.analysis.CatmullRomException if
* pointsPerSegment is less than 2.
*/
public static List<Coord> interpolate(List<Coord> coordinates, int pointsPerSegment, CatmullRomType curveType)
throws CatmullRomException {
List<Coord> vertices = new ArrayList<>();
for (Coord c : coordinates) {
vertices.add(c.copy());
}
if (pointsPerSegment < 2) {
throw new CatmullRomException("The pointsPerSegment parameter must be greater than 2, since 2 points is just the linear segment.");
}
// Cannot interpolate curves given only two points. Two points
// is best represented as a simple line segment.
if (vertices.size() < 3) {
return vertices;
}
// Test whether the shape is open or closed by checking to see if
// the first point intersects with the last point. M and Z are ignored.
boolean isClosed = vertices.get(0).intersects2D(vertices.get(vertices.size() - 1));
if (isClosed) {
// Use the second and second from last points as control points.
// get the second point.
Coord p2 = vertices.get(1).copy();
// get the point before the last point
Coord pn1 = vertices.get(vertices.size() - 2).copy();
// insert the second from the last point as the first point in the list
// because when the shape is closed it keeps wrapping around to
// the second point.
vertices.add(0, pn1);
// add the second point to the end.
vertices.add(p2);
} else {
// The shape is open, so use control points that simply extend
// the first and last segments
// Get the change in x and y between the first and second coordinates.
double dx = vertices.get(1).X - vertices.get(0).X;
double dy = vertices.get(1).Y - vertices.get(0).Y;
// Then using the change, extrapolate backwards to find a control point.
double x1 = vertices.get(0).X - dx;
double y1 = vertices.get(0).Y - dy;
// Actaully create the start point from the extrapolated values.
Coord start = new Coord(x1, y1, vertices.get(0).Z);
// Repeat for the end control point.
int n = vertices.size() - 1;
dx = vertices.get(n).X - vertices.get(n - 1).X;
dy = vertices.get(n).Y - vertices.get(n - 1).Y;
double xn = vertices.get(n).X + dx;
double yn = vertices.get(n).Y + dy;
Coord end = new Coord(xn, yn, vertices.get(n).Z);
// insert the start control point at the start of the vertices list.
vertices.add(0, start);
// append the end control ponit to the end of the vertices list.
vertices.add(end);
}
// Dimension a result list of coordinates.
List<Coord> result = new ArrayList<>();
// When looping, remember that each cycle requires 4 points, starting
// with i and ending with i+3. So we don't loop through all the points.
for (int i = 0; i < vertices.size() - 3; i++) {
// Actually calculate the Catmull-Rom curve for one segment.
List<Coord> points = interpolate(vertices, i, pointsPerSegment, curveType);
// Since the middle points are added twice, once for each bordering
// segment, we only add the 0 index result point for the first
// segment. Otherwise we will have duplicate points.
if (result.size() > 0) {
points.remove(0);
}
// Add the coordinates for the segment to the result list.
result.addAll(points);
}
return result;
}
/**
* Given a list of control points, this will create a list of pointsPerSegment
* points spaced uniformly along the resulting Catmull-Rom curve.
*
* #param points The list of control points, leading and ending with a
* coordinate that is only used for controling the spline and is not visualized.
* #param index The index of control point p0, where p0, p1, p2, and p3 are
* used in order to create a curve between p1 and p2.
* #param pointsPerSegment The total number of uniformly spaced interpolated
* points to calculate for each segment. The larger this number, the
* smoother the resulting curve.
* #param curveType Clarifies whether the curve should use uniform, chordal
* or centripetal curve types. Uniform can produce loops, chordal can
* produce large distortions from the original lines, and centripetal is an
* optimal balance without spaces.
* #return the list of coordinates that define the CatmullRom curve
* between the points defined by index+1 and index+2.
*/
public static List<Coord> interpolate(List<Coord> points, int index, int pointsPerSegment, CatmullRomType curveType) {
List<Coord> result = new ArrayList<>();
double[] x = new double[4];
double[] y = new double[4];
double[] time = new double[4];
for (int i = 0; i < 4; i++) {
x[i] = points.get(index + i).X;
y[i] = points.get(index + i).Y;
time[i] = i;
}
double tstart = 1;
double tend = 2;
if (!curveType.equals(CatmullRomType.Uniform)) {
double total = 0;
for (int i = 1; i < 4; i++) {
double dx = x[i] - x[i - 1];
double dy = y[i] - y[i - 1];
if (curveType.equals(CatmullRomType.Centripetal)) {
total += Math.pow(dx * dx + dy * dy, .25);
} else {
total += Math.pow(dx * dx + dy * dy, .5);
}
time[i] = total;
}
tstart = time[1];
tend = time[2];
}
double z1 = 0.0;
double z2 = 0.0;
if (!Double.isNaN(points.get(index + 1).Z)) {
z1 = points.get(index + 1).Z;
}
if (!Double.isNaN(points.get(index + 2).Z)) {
z2 = points.get(index + 2).Z;
}
double dz = z2 - z1;
int segments = pointsPerSegment - 1;
result.add(points.get(index + 1));
for (int i = 1; i < segments; i++) {
double xi = interpolate(x, time, tstart + (i * (tend - tstart)) / segments);
double yi = interpolate(y, time, tstart + (i * (tend - tstart)) / segments);
double zi = z1 + (dz * i) / segments;
result.add(new Coord(xi, yi, zi));
}
result.add(points.get(index + 2));
return result;
}
/**
* Unlike the other implementation here, which uses the default "uniform"
* treatment of t, this computation is used to calculate the same values but
* introduces the ability to "parameterize" the t values used in the
* calculation. This is based on Figure 3 from
* http://www.cemyuksel.com/research/catmullrom_param/catmullrom.pdf
*
* #param p An array of double values of length 4, where interpolation
* occurs from p1 to p2.
* #param time An array of time measures of length 4, corresponding to each
* p value.
* #param t the actual interpolation ratio from 0 to 1 representing the
* position between p1 and p2 to interpolate the value.
* #return
*/
public static double interpolate(double[] p, double[] time, double t) {
double L01 = p[0] * (time[1] - t) / (time[1] - time[0]) + p[1] * (t - time[0]) / (time[1] - time[0]);
double L12 = p[1] * (time[2] - t) / (time[2] - time[1]) + p[2] * (t - time[1]) / (time[2] - time[1]);
double L23 = p[2] * (time[3] - t) / (time[3] - time[2]) + p[3] * (t - time[2]) / (time[3] - time[2]);
double L012 = L01 * (time[2] - t) / (time[2] - time[0]) + L12 * (t - time[0]) / (time[2] - time[0]);
double L123 = L12 * (time[3] - t) / (time[3] - time[1]) + L23 * (t - time[1]) / (time[3] - time[1]);
double C12 = L012 * (time[2] - t) / (time[2] - time[1]) + L123 * (t - time[1]) / (time[2] - time[1]);
return C12;
}
There is a much easier and more efficient way to implement this which only requires you to compute your tangents using a different formula, without the need to implement the recursive evaluation algorithm of Barry and Goldman.
If you take the Barry-Goldman parametrization (referenced in Ted's answer) C(t) for the knots (t0,t1,t2,t3) and the control points (P0,P1,P2,P3), its closed form is pretty complicated, but in the end it's still a cubic polynomial in t when you constrain it to the interval (t1,t2). So all we need to describe it fully are the values and tangents at the two end points t1 and t2. If we work out these values (I did this in Mathematica), we find
C(t1) = P1
C(t2) = P2
C'(t1) = (P1 - P0) / (t1 - t0) - (P2 - P0) / (t2 - t0) + (P2 - P1) / (t2 - t1)
C'(t2) = (P2 - P1) / (t2 - t1) - (P3 - P1) / (t3 - t1) + (P3 - P2) / (t3 - t2)
We can simply plug this into the standard formula for computing a cubic spline with given values and tangents at the end points and we have our nonuniform Catmull-Rom spline. One caveat is that the above tangents are computed for the interval (t1,t2), so if you want to evaluate the curve in the standard interval (0,1), simply rescale the tangents by multiplying them with the factor (t2-t1).
I put a working C++ example on Ideone: http://ideone.com/NoEbVM
I'll also paste the code below.
#include <iostream>
#include <cmath>
using namespace std;
struct CubicPoly
{
float c0, c1, c2, c3;
float eval(float t)
{
float t2 = t*t;
float t3 = t2 * t;
return c0 + c1*t + c2*t2 + c3*t3;
}
};
/*
* Compute coefficients for a cubic polynomial
* p(s) = c0 + c1*s + c2*s^2 + c3*s^3
* such that
* p(0) = x0, p(1) = x1
* and
* p'(0) = t0, p'(1) = t1.
*/
void InitCubicPoly(float x0, float x1, float t0, float t1, CubicPoly &p)
{
p.c0 = x0;
p.c1 = t0;
p.c2 = -3*x0 + 3*x1 - 2*t0 - t1;
p.c3 = 2*x0 - 2*x1 + t0 + t1;
}
// standard Catmull-Rom spline: interpolate between x1 and x2 with previous/following points x0/x3
// (we don't need this here, but it's for illustration)
void InitCatmullRom(float x0, float x1, float x2, float x3, CubicPoly &p)
{
// Catmull-Rom with tension 0.5
InitCubicPoly(x1, x2, 0.5f*(x2-x0), 0.5f*(x3-x1), p);
}
// compute coefficients for a nonuniform Catmull-Rom spline
void InitNonuniformCatmullRom(float x0, float x1, float x2, float x3, float dt0, float dt1, float dt2, CubicPoly &p)
{
// compute tangents when parameterized in [t1,t2]
float t1 = (x1 - x0) / dt0 - (x2 - x0) / (dt0 + dt1) + (x2 - x1) / dt1;
float t2 = (x2 - x1) / dt1 - (x3 - x1) / (dt1 + dt2) + (x3 - x2) / dt2;
// rescale tangents for parametrization in [0,1]
t1 *= dt1;
t2 *= dt1;
InitCubicPoly(x1, x2, t1, t2, p);
}
struct Vec2D
{
Vec2D(float _x, float _y) : x(_x), y(_y) {}
float x, y;
};
float VecDistSquared(const Vec2D& p, const Vec2D& q)
{
float dx = q.x - p.x;
float dy = q.y - p.y;
return dx*dx + dy*dy;
}
void InitCentripetalCR(const Vec2D& p0, const Vec2D& p1, const Vec2D& p2, const Vec2D& p3,
CubicPoly &px, CubicPoly &py)
{
float dt0 = powf(VecDistSquared(p0, p1), 0.25f);
float dt1 = powf(VecDistSquared(p1, p2), 0.25f);
float dt2 = powf(VecDistSquared(p2, p3), 0.25f);
// safety check for repeated points
if (dt1 < 1e-4f) dt1 = 1.0f;
if (dt0 < 1e-4f) dt0 = dt1;
if (dt2 < 1e-4f) dt2 = dt1;
InitNonuniformCatmullRom(p0.x, p1.x, p2.x, p3.x, dt0, dt1, dt2, px);
InitNonuniformCatmullRom(p0.y, p1.y, p2.y, p3.y, dt0, dt1, dt2, py);
}
int main()
{
Vec2D p0(0,0), p1(1,1), p2(1.1,1), p3(2,0);
CubicPoly px, py;
InitCentripetalCR(p0, p1, p2, p3, px, py);
for (int i = 0; i <= 10; ++i)
cout << px.eval(0.1f*i) << " " << py.eval(0.1f*i) << endl;
}
Here is an iOS version of Ted's code. I excluded the 'z' parts.
.h
typedef enum {
CatmullRomTypeUniform,
CatmullRomTypeChordal,
CatmullRomTypeCentripetal
} CatmullRomType ;
.m
-(NSMutableArray *)interpolate:(NSArray *)coordinates withPointsPerSegment:(NSInteger)pointsPerSegment andType:(CatmullRomType)curveType {
NSMutableArray *vertices = [[NSMutableArray alloc] initWithArray:coordinates copyItems:YES];
if (pointsPerSegment < 3)
return vertices;
//start point
CGPoint pt1 = [vertices[0] CGPointValue];
CGPoint pt2 = [vertices[1] CGPointValue];
double dx = pt2.x - pt1.x;
double dy = pt2.y - pt1.y;
double x1 = pt1.x - dx;
double y1 = pt1.y - dy;
CGPoint start = CGPointMake(x1*.5, y1);
//end point
pt2 = [vertices[vertices.count-1] CGPointValue];
pt1 = [vertices[vertices.count-2] CGPointValue];
dx = pt2.x - pt1.x;
dy = pt2.y - pt1.y;
x1 = pt2.x + dx;
y1 = pt2.y + dy;
CGPoint end = CGPointMake(x1, y1);
[vertices insertObject:[NSValue valueWithCGPoint:start] atIndex:0];
[vertices addObject:[NSValue valueWithCGPoint:end]];
NSMutableArray *result = [[NSMutableArray alloc] init];
for (int i = 0; i < vertices.count - 3; i++) {
NSMutableArray *points = [self interpolate:vertices forIndex:i withPointsPerSegment:pointsPerSegment andType:curveType];
if ([points count] > 0)
[points removeObjectAtIndex:0];
[result addObjectsFromArray:points];
}
return result;
}
-(double)interpolate:(double*)p time:(double*)time t:(double) t {
double L01 = p[0] * (time[1] - t) / (time[1] - time[0]) + p[1] * (t - time[0]) / (time[1] - time[0]);
double L12 = p[1] * (time[2] - t) / (time[2] - time[1]) + p[2] * (t - time[1]) / (time[2] - time[1]);
double L23 = p[2] * (time[3] - t) / (time[3] - time[2]) + p[3] * (t - time[2]) / (time[3] - time[2]);
double L012 = L01 * (time[2] - t) / (time[2] - time[0]) + L12 * (t - time[0]) / (time[2] - time[0]);
double L123 = L12 * (time[3] - t) / (time[3] - time[1]) + L23 * (t - time[1]) / (time[3] - time[1]);
double C12 = L012 * (time[2] - t) / (time[2] - time[1]) + L123 * (t - time[1]) / (time[2] - time[1]);
return C12;
}
-(NSMutableArray*)interpolate:(NSArray *)points forIndex:(NSInteger)index withPointsPerSegment:(NSInteger)pointsPerSegment andType:(CatmullRomType)curveType {
NSMutableArray *result = [[NSMutableArray alloc] init];
double x[4];
double y[4];
double time[4];
for (int i=0; i < 4; i++) {
x[i] = [points[index+i] CGPointValue].x;
y[i] = [points[index+i] CGPointValue].y;
time[i] = i;
}
double tstart = 1;
double tend = 2;
if (curveType != CatmullRomTypeUniform) {
double total = 0;
for (int i=1; i < 4; i++) {
double dx = x[i] - x[i-1];
double dy = y[i] - y[i-1];
if (curveType == CatmullRomTypeCentripetal) {
total += pow(dx * dx + dy * dy, 0.25);
}
else {
total += pow(dx * dx + dy * dy, 0.5); //sqrt
}
time[i] = total;
}
tstart = time[1];
tend = time[2];
}
int segments = pointsPerSegment - 1;
[result addObject:points[index+1]];
for (int i =1; i < segments; i++) {
double xi = [self interpolate:x time:time t:tstart + (i * (tend - tstart)) / segments];
double yi = [self interpolate:y time:time t:tstart + (i * (tend - tstart)) / segments];
NSLog(#"(%f,%f)",xi,yi);
[result addObject:[NSValue valueWithCGPoint:CGPointMake(xi, yi)]];
}
[result addObject:points[index+2]];
return result;
}
Also, here is a method for turning an array of points into a Bezier path for drawing, using the above
-(UIBezierPath*)bezierPathFromPoints:(NSArray *)points withGranulaity:(NSInteger)granularity
{
UIBezierPath __block *path = [[UIBezierPath alloc] init];
NSMutableArray *curve = [self interpolate:points withPointsPerSegment:granularity andType:CatmullRomTypeCentripetal];
CGPoint __block p0 = [curve[0] CGPointValue];
[path moveToPoint:p0];
//use this loop to draw lines between all points
for (int idx=1; idx < [curve count]; idx+=1) {
CGPoint c1 = [curve[idx] CGPointValue];
[path addLineToPoint:c1];
};
//or use this loop to use actual control points (less smooth but probably faster)
// for (int idx=0; idx < [curve count]-3; idx+=3) {
// CGPoint c1 = [curve[idx+1] CGPointValue];
// CGPoint c2 = [curve[idx+2] CGPointValue];
// CGPoint p1 = [curve[idx+3] CGPointValue];
//
// [path addCurveToPoint:p1 controlPoint1:c1 controlPoint2:c2];
// };
return path;
}
Thanks for the reply of Ted and cfh.
Sorry to my poor English and I am not very sure if my understanding is right.
It confused me before that what is the relation between τ in http://graphics.cs.cmu.edu/nsp/course/15-462/Fall04/assts/catmullRom.pdf with α in
"Parameterization of Catmull-Rom Curves"[Yuksel et al. 2009].
It finally seems that τ has little relation to α.
In Ted's reply we can find that as long as t(i+1)-t(i)=1, we call it "uniform" Catmull-Rom curve.
So, all curves in http://graphics.cs.cmu.edu/nsp/course/15-462/Fall04/assts/catmullRom.pdf are "uniform" curves, while parameterized Catmull-Rom can only produce one "uniform" curve when α=0.τ just affects how sharply the curve bends at the (interpolated) control points and can produce a series of "uniform" curves.
Here, if we take the equation from cfh into consideration, i.e.:
C(t1) = P1
C(t2) = P2
C'(t1) = (P1 - P0) / (t1 - t0) - (P2 - P0) / (t2 - t0) + (P2 - P1) / (t2 - t1)
C'(t2) = (P2 - P1) / (t2 - t1) - (P3 - P1) / (t3 - t1) + (P3 - P2) / (t3 - t2)
When α=0, we know that t(i+1)-t(i)=1. Substituting t(i+1)-t(i)=1 to C'(t1) and C'(t2), we can get:
C'(t1)=1/2(P2-P0)
C'(t2)=1/2(P3-P1).
That is to say, the only one "uniform" curve generated by parameterized Catmull-Rom curve meets the special case τ=1/2 http://graphics.cs.cmu.edu/nsp/course/15-462/Fall04/assts/catmullRom.pdf. More different "uniform" curves can be generated by the changes of τ, while α pays more attention to something else.
I coded something in Python (adapted form Catmull-Rom Wikipedia page) that compares uniform, centripedal, and chordial CR Splines (though you can set alpha to whatever you'd like) using random data (you can use your own data and the fucntions work fine). Note that for the endpoints I just stuck in a quick 'hack' that maintains the slope from the first and last 2 points, although the distance between this point and the first/lost known point is arbitrary (I set it to 1% of the domain... for no reason at all. So keep that in mind before applying to something important):
# coding: utf-8
# In[1]:
import numpy
import matplotlib.pyplot as plt
get_ipython().magic(u'pylab inline')
# In[2]:
def CatmullRomSpline(P0, P1, P2, P3, a, nPoints=100):
"""
P0, P1, P2, and P3 should be (x,y) point pairs that define the Catmull-Rom spline.
nPoints is the number of points to include in this curve segment.
"""
# Convert the points to numpy so that we can do array multiplication
P0, P1, P2, P3 = map(numpy.array, [P0, P1, P2, P3])
# Calculate t0 to t4
alpha = a
def tj(ti, Pi, Pj):
xi, yi = Pi
xj, yj = Pj
return ( ( (xj-xi)**2 + (yj-yi)**2 )**0.5 )**alpha + ti
t0 = 0
t1 = tj(t0, P0, P1)
t2 = tj(t1, P1, P2)
t3 = tj(t2, P2, P3)
# Only calculate points between P1 and P2
t = numpy.linspace(t1,t2,nPoints)
# Reshape so that we can multiply by the points P0 to P3
# and get a point for each value of t.
t = t.reshape(len(t),1)
A1 = (t1-t)/(t1-t0)*P0 + (t-t0)/(t1-t0)*P1
A2 = (t2-t)/(t2-t1)*P1 + (t-t1)/(t2-t1)*P2
A3 = (t3-t)/(t3-t2)*P2 + (t-t2)/(t3-t2)*P3
B1 = (t2-t)/(t2-t0)*A1 + (t-t0)/(t2-t0)*A2
B2 = (t3-t)/(t3-t1)*A2 + (t-t1)/(t3-t1)*A3
C = (t2-t)/(t2-t1)*B1 + (t-t1)/(t2-t1)*B2
return C
def CatmullRomChain(P,alpha):
"""
Calculate Catmull Rom for a chain of points and return the combined curve.
"""
sz = len(P)
# The curve C will contain an array of (x,y) points.
C = []
for i in range(sz-3):
c = CatmullRomSpline(P[i], P[i+1], P[i+2], P[i+3],alpha)
C.extend(c)
return C
# In[8]:
# Define a set of points for curve to go through
Points = numpy.random.rand(12,2)
x1=Points[0][0]
x2=Points[1][0]
y1=Points[0][1]
y2=Points[1][1]
x3=Points[-2][0]
x4=Points[-1][0]
y3=Points[-2][1]
y4=Points[-1][1]
dom=max(Points[:,0])-min(Points[:,0])
rng=max(Points[:,1])-min(Points[:,1])
prex=x1+sign(x1-x2)*dom*0.01
prey=(y1-y2)/(x1-x2)*dom*0.01+y1
endx=x4+sign(x4-x3)*dom*0.01
endy=(y4-y3)/(x4-x3)*dom*0.01+y4
print len(Points)
Points=list(Points)
Points.insert(0,array([prex,prey]))
Points.append(array([endx,endy]))
print len(Points)
# In[9]:
#Define alpha
a=0.
# Calculate the Catmull-Rom splines through the points
c = CatmullRomChain(Points,a)
# Convert the Catmull-Rom curve points into x and y arrays and plot
x,y = zip(*c)
plt.plot(x,y,c='green',zorder=10)
# Plot the control points
px, py = zip(*Points)
plt.plot(px,py,'or')
a=0.5
c = CatmullRomChain(Points,a)
x,y = zip(*c)
plt.plot(x,y,c='blue')
a=1.
c = CatmullRomChain(Points,a)
x,y = zip(*c)
plt.plot(x,y,c='red')
plt.grid(b=True)
plt.show()
# In[10]:
Points
# In[ ]:
original code: https://en.wikipedia.org/wiki/Centripetal_Catmull%E2%80%93Rom_spline
I'm trying to find the best way to calculate the biggest (in area) rectangle which can be contained inside a rotated rectangle.
Some pictures should help (I hope) in visualizing what I mean:
The width and height of the input rectangle is given and so is the angle to rotate it. The output rectangle is not rotated or skewed.
I'm going down the longwinded route which I'm not even sure if it will handle the corner cases (no pun intended). I'm certain there is an elegant solution to this. Any tips?
EDIT: The output rectangle points don't necessarily have to touch the input rectangles edges. (Thanks to Mr E)
I just came here looking for the same answer. After shuddering at the thought of so much math involved, I thought I would resort to a semi-educated guess. Doodling a bit I came to the (intuitive and probably not entirely exact) conclusion that the largest rectangle is proportional to the outer resulting rectangle, and its two opposing corners lie at the intersection of the diagonals of the outer rectangle with the longest side of the rotated rectangle. For squares, any of the diagonals and sides would do... I guess I am happy enough with this and will now start brushing the cobwebs off my rusty trig skills (pathetic, I know).
Minor update... Managed to do some trig calculations. This is for the case when the Height of the image is larger than the Width.
Update. Got the whole thing working. Here is some js code. It is connected to a larger program, and most variables are outside the scope of the functions, and are modified directly from within the functions. I know this is not good, but I am using this in an isolated situation, where there will be no confusion with other scripts: redacted
I took the liberty of cleaning the code and extracting it to a function:
function getCropCoordinates(angleInRadians, imageDimensions) {
var ang = angleInRadians;
var img = imageDimensions;
var quadrant = Math.floor(ang / (Math.PI / 2)) & 3;
var sign_alpha = (quadrant & 1) === 0 ? ang : Math.PI - ang;
var alpha = (sign_alpha % Math.PI + Math.PI) % Math.PI;
var bb = {
w: img.w * Math.cos(alpha) + img.h * Math.sin(alpha),
h: img.w * Math.sin(alpha) + img.h * Math.cos(alpha)
};
var gamma = img.w < img.h ? Math.atan2(bb.w, bb.h) : Math.atan2(bb.h, bb.w);
var delta = Math.PI - alpha - gamma;
var length = img.w < img.h ? img.h : img.w;
var d = length * Math.cos(alpha);
var a = d * Math.sin(alpha) / Math.sin(delta);
var y = a * Math.cos(gamma);
var x = y * Math.tan(gamma);
return {
x: x,
y: y,
w: bb.w - 2 * x,
h: bb.h - 2 * y
};
}
I encountered some problems with the gamma-calculation, and modified it to take into account in which direction the original box is the longest.
-- Magnus Hoff
Trying not to break tradition putting the solution of the problem as a picture:)
Edit:
Third equations is wrong. The correct one is:
3.w * cos(α) * X + w * sin(α) * Y - w * w * sin(α) * cos(α) - w * h = 0
To solve the system of linear equations you can use Cramer rule, or Gauss method.
First, we take care of the trivial case where the angle is zero or a multiple of pi/2. Then the largest rectangle is the same as the original rectangle.
In general, the inner rectangle will have 3 points on the boundaries of the outer rectangle. If it does not, then it can be moved so that one vertex will be on the bottom, and one vertex will be on the left. You can then enlarge the inner rectangle until one of the two remaining vertices hits a boundary.
We call the sides of the outer rectangle R1 and R2. Without loss of generality, we can assume that R1 <= R2. If we call the sides of the inner rectangle H and W, then we have that
H cos a + W sin a <= R1
H sin a + W cos a <= R2
Since we have at least 3 points on the boundaries, at least one of these inequality must actually be an equality. Let's use the first one. It is easy to see that:
W = (R1 - H cos a) / sin a
and so the area is
A = H W = H (R1 - H cos a) / sin a
We can take the derivative wrt. H and require it to equal 0:
dA/dH = ((R1 - H cos a) - H cos a) / sin a
Solving for H and using the expression for W above, we find that:
H = R1 / (2 cos a)
W = R1 / (2 sin a)
Substituting this in the second inequality becomes, after some manipulation,
R1 (tan a + 1/tan a) / 2 <= R2
The factor on the left-hand side is always at least 1. If the inequality is satisfied, then we have the solution. If it isn't satisfied, then the solution is the one that satisfies both inequalities as equalities. In other words: it is the rectangle which touches all four sides of the outer rectangle. This is a linear system with 2 unknowns which is readily solved:
H = (R2 cos a - R1 sin a) / cos 2a
W = (R1 cos a - R2 sin a) / cos 2a
In terms of the original coordinates, we get:
x1 = x4 = W sin a cos a
y1 = y2 = R2 sin a - W sin^2 a
x2 = x3 = x1 + H
y3 = y4 = y2 + W
Edit: My Mathematica answer below is wrong - I was solving a slightly different problem than what I think you are really asking.
To solve the problem you are really asking, I would use the following algorithm(s):
On the Maximum Empty Rectangle Problem
Using this algorithm, denote a finite amount of points that form the boundary of the rotated rectangle (perhaps a 100 or so, and make sure to include the corners) - these would be the set S decribed in the paper.
.
.
.
.
.
For posterity's sake I have left my original post below:
The inside rectangle with the largest area will always be the rectangle where the lower mid corner of the rectangle (the corner near the alpha on your diagram) is equal to half of the width of the outer rectangle.
I kind of cheated and used Mathematica to solve the algebra for me:
From this you can see that the maximum area of the inner rectangle is equal to 1/4 width^2 * cosecant of the angle times the secant of the angle.
Now I need to figure out what is the x value of the bottom corner for this optimal condition. Using the Solve function in mathematica on my area formula, I get the following:
Which shows that the x coordinate of the bottom corner equals half of the width.
Now just to make sure, I'll going to test our answer empirically. With the results below you can see that indeed the highest area of all of my tests (definately not exhaustive but you get the point) is when the bottom corner's x value = half of the outer rectangle's width.
#Andri is not working correctly for image where width > height as I tested.
So, I fixed and optimized his code by such way (with only two trigonometric functions):
calculateLargestRect = function(angle, origWidth, origHeight) {
var w0, h0;
if (origWidth <= origHeight) {
w0 = origWidth;
h0 = origHeight;
}
else {
w0 = origHeight;
h0 = origWidth;
}
// Angle normalization in range [-PI..PI)
var ang = angle - Math.floor((angle + Math.PI) / (2*Math.PI)) * 2*Math.PI;
ang = Math.abs(ang);
if (ang > Math.PI / 2)
ang = Math.PI - ang;
var sina = Math.sin(ang);
var cosa = Math.cos(ang);
var sinAcosA = sina * cosa;
var w1 = w0 * cosa + h0 * sina;
var h1 = w0 * sina + h0 * cosa;
var c = h0 * sinAcosA / (2 * h0 * sinAcosA + w0);
var x = w1 * c;
var y = h1 * c;
var w, h;
if (origWidth <= origHeight) {
w = w1 - 2 * x;
h = h1 - 2 * y;
}
else {
w = h1 - 2 * y;
h = w1 - 2 * x;
}
return {
w: w,
h: h
}
}
UPDATE
Also I decided to post the following function for proportional rectange calculating:
calculateLargestProportionalRect = function(angle, origWidth, origHeight) {
var w0, h0;
if (origWidth <= origHeight) {
w0 = origWidth;
h0 = origHeight;
}
else {
w0 = origHeight;
h0 = origWidth;
}
// Angle normalization in range [-PI..PI)
var ang = angle - Math.floor((angle + Math.PI) / (2*Math.PI)) * 2*Math.PI;
ang = Math.abs(ang);
if (ang > Math.PI / 2)
ang = Math.PI - ang;
var c = w0 / (h0 * Math.sin(ang) + w0 * Math.cos(ang));
var w, h;
if (origWidth <= origHeight) {
w = w0 * c;
h = h0 * c;
}
else {
w = h0 * c;
h = w0 * c;
}
return {
w: w,
h: h
}
}
Coproc solved this problem on another thread (https://stackoverflow.com/a/16778797) in a simple and efficient way. Also, he gave a very good explanation and python code there.
Below there is my Matlab implementation of his solution:
function [ CI, T ] = rotateAndCrop( I, ang )
%ROTATEANDCROP Rotate an image 'I' by 'ang' degrees, and crop its biggest
% inner rectangle.
[h,w,~] = size(I);
ang = deg2rad(ang);
% Affine rotation
R = [cos(ang) -sin(ang) 0; sin(ang) cos(ang) 0; 0 0 1];
T = affine2d(R);
B = imwarp(I,T);
% Largest rectangle
% solution from https://stackoverflow.com/a/16778797
wb = w >= h;
sl = w*wb + h*~wb;
ss = h*wb + w*~wb;
cosa = abs(cos(ang));
sina = abs(sin(ang));
if ss <= 2*sina*cosa*sl
x = .5*min([w h]);
wh = wb*[x/sina x/cosa] + ~wb*[x/cosa x/sina];
else
cos2a = (cosa^2) - (sina^2);
wh = [(w*cosa - h*sina)/cos2a (h*cosa - w*sina)/cos2a];
end
hw = flip(wh);
% Top-left corner
tl = round(max(size(B)/2 - hw/2,1));
% Bottom-right corner
br = tl + round(hw);
% Cropped image
CI = B(tl(1):br(1),tl(2):br(2),:);
sorry for not giving a derivation here, but I solved this problem in Mathematica a few days ago and came up with the following procedure, which non-Mathematica folks should be able to read. If in doubt, please consult http://reference.wolfram.com/mathematica/guide/Mathematica.html
The procedure below returns the width and height for a rectangle with maximum area that fits into another rectangle of width w and height h that has been rotated by alpha.
CropRotatedDimensionsForMaxArea[{w_, h_}, alpha_] :=
With[
{phi = Abs#Mod[alpha, Pi, -Pi/2]},
Which[
w == h, {w,h} Csc[phi + Pi/4]/Sqrt[2],
w > h,
If[ Cos[2 phi]^2 < 1 - (h/w)^2,
h/2 {Csc[phi], Sec[phi]},
Sec[2 phi] {w Cos[phi] - h Sin[phi], h Cos[phi] - w Sin[phi]}],
w < h,
If[ Cos[2 phi]^2 < 1 - (w/h)^2,
w/2 {Sec[phi], Csc[phi]},
Sec[2 phi] {w Cos[phi] - h Sin[phi], h Cos[phi] - w Sin[phi]}]
]
]
Here is the easiest way to do this... :)
Step 1
//Before Rotation
int originalWidth = 640;
int originalHeight = 480;
Step 2
//After Rotation
int newWidth = 701; //int newWidth = 654; //int newWidth = 513;
int newHeight = 564; //int newHeight = 757; //int newHeight = 664;
Step 3
//Difference in height and width
int widthDiff ;
int heightDiff;
int ASPECT_RATIO = originalWidth/originalHeight; //Double check the Aspect Ratio
if (newHeight > newWidth) {
int ratioDiff = newHeight - newWidth;
if (newWidth < Constant.camWidth) {
widthDiff = (int) Math.floor(newWidth / ASPECT_RATIO);
heightDiff = (int) Math.floor((originalHeight - (newHeight - originalHeight)) / ASPECT_RATIO);
}
else {
widthDiff = (int) Math.floor((originalWidth - (newWidth - originalWidth) - ratioDiff) / ASPECT_RATIO);
heightDiff = originalHeight - (newHeight - originalHeight);
}
} else {
widthDiff = originalWidth - (originalWidth);
heightDiff = originalHeight - (newHeight - originalHeight);
}
Step 4
//Calculation
int targetRectanleWidth = originalWidth - widthDiff;
int targetRectanleHeight = originalHeight - heightDiff;
Step 5
int centerPointX = newWidth/2;
int centerPointY = newHeight/2;
Step 6
int x1 = centerPointX - (targetRectanleWidth / 2);
int y1 = centerPointY - (targetRectanleHeight / 2);
int x2 = centerPointX + (targetRectanleWidth / 2);
int y2 = centerPointY + (targetRectanleHeight / 2);
Step 7
x1 = (x1 < 0 ? 0 : x1);
y1 = (y1 < 0 ? 0 : y1);
This is just an illustration of Jeffrey Sax's solution above, for my future reference.
With reference to the diagram above, the solution is:
(I used the identity tan(t) + cot(t) = 2/sin(2t))
Does anyone have an algorithm for drawing an arrow in the middle of a given line. I have searched for google but haven't found any good implementation.
P.S. I really don't mind the language, but it would be great if it was Java, since it is the language I am using for this.
Thanks in advance.
Here's a function to draw an arrow with its head at a point p. You would set this to the midpoint of your line. dx and dy are the line direction, which is given by (x1 - x0, y1 - y0). This will give an arrow that is scaled to the line length. Normalize this direction if you want the arrow to always be the same size.
private static void DrawArrow(Graphics g, Pen pen, Point p, float dx, float dy)
{
const double cos = 0.866;
const double sin = 0.500;
PointF end1 = new PointF(
(float)(p.X + (dx * cos + dy * -sin)),
(float)(p.Y + (dx * sin + dy * cos)));
PointF end2 = new PointF(
(float)(p.X + (dx * cos + dy * sin)),
(float)(p.Y + (dx * -sin + dy * cos)));
g.DrawLine(pen, p, end1);
g.DrawLine(pen, p, end2);
}
Here's a method to add an arrow head to a line.
You just have to give it the coordinates of your arrow tip and tail.
private static void drawArrow(int tipX, int tailX, int tipY, int tailY, Graphics2D g)
{
int arrowLength = 7; //can be adjusted
int dx = tipX - tailX;
int dy = tipY - tailY;
double theta = Math.atan2(dy, dx);
double rad = Math.toRadians(35); //35 angle, can be adjusted
double x = tipX - arrowLength * Math.cos(theta + rad);
double y = tipY - arrowLength * Math.sin(theta + rad);
double phi2 = Math.toRadians(-35);//-35 angle, can be adjusted
double x2 = tipX - arrowLength * Math.cos(theta + phi2);
double y2 = tipY - arrowLength * Math.sin(theta + phi2);
int[] arrowYs = new int[3];
arrowYs[0] = tipY;
arrowYs[1] = (int) y;
arrowYs[2] = (int) y2;
int[] arrowXs = new int[3];
arrowXs[0] = tipX;
arrowXs[1] = (int) x;
arrowXs[2] = (int) x2;
g.fillPolygon(arrowXs, arrowYs, 3);
}
I have a set of latitudes and longitudes of locations.
How to find distance from one location in the set to another?
Is there a formula ?
The Haversine formula assumes a spherical earth. However, the shape of the earh is more complex. An oblate spheroid model will give better results.
If such accuracy is needed, you should better use Vincenty inverse formula.
See http://en.wikipedia.org/wiki/Vincenty's_formulae for details. Using it, you can get a 0.5mm accuracy for the spheroid model.
There is no perfect formula, since the real shape of the earth is too complex to be expressed by a formula. Moreover, the shape of earth changes due to climate events (see http://www.nasa.gov/centers/goddard/earthandsun/earthshape.html), and also changes over time due to the rotation of the earth.
You should also note that the method above does not take altitudes into account, and assumes a sea-level oblate spheroid.
Edit 10-Jul-2010: I found out that there are rare situations for which Vincenty inverse formula does not converge to the declared accuracy. A better idea is to use GeographicLib (see http://sourceforge.net/projects/geographiclib/) which is also more accurate.
Here's one: http://www.movable-type.co.uk/scripts/latlong.html
Using Haversine formula:
R = earth’s radius (mean radius = 6,371km)
Δlat = lat2− lat1
Δlong = long2− long1
a = sin²(Δlat/2) + cos(lat1).cos(lat2).sin²(Δlong/2)
c = 2.atan2(√a, √(1−a))
d = R.c
Apply the Haversine formula to find the distance. See the C# code below to find the distance between 2 coordinates. Better still if you want to say find a list of stores within a certain radius, you could apply a WHERE clause in SQL or a LINQ filter in C# to it.
The formula here is in kilometres, you will have to change the relevant numbers and it will work for miles.
E.g: Convert 6371.392896 to miles.
DECLARE #radiusInKm AS FLOAT
DECLARE #lat2Compare AS FLOAT
DECLARE #long2Compare AS FLOAT
SET #radiusInKm = 5.000
SET #lat2Compare = insert_your_lat_to_compare_here
SET #long2Compare = insert_you_long_to_compare_here
SELECT * FROM insert_your_table_here WITH(NOLOCK)
WHERE (6371.392896*2*ATN2(SQRT((sin((radians(GeoLatitude - #lat2Compare)) / 2) * sin((radians(GeoLatitude - #lat2Compare)) / 2)) + (cos(radians(GeoLatitude)) * cos(radians(#lat2Compare)) * sin(radians(GeoLongitude - #long2Compare)/2) * sin(radians(GeoLongitude - #long2Compare)/2)))
, SQRT(1-((sin((radians(GeoLatitude - #lat2Compare)) / 2) * sin((radians(GeoLatitude - #lat2Compare)) / 2)) + (cos(radians(GeoLatitude)) * cos(radians(#lat2Compare)) * sin(radians(GeoLongitude - #long2Compare)/2) * sin(radians(GeoLongitude - #long2Compare)/2)))
))) <= #radiusInKm
If you would like to perform the Haversine formula in C#,
double resultDistance = 0.0;
double avgRadiusOfEarth = 6371.392896; //Radius of the earth differ, I'm taking the average.
//Haversine formula
//distance = R * 2 * aTan2 ( square root of A, square root of 1 - A )
// where A = sinus squared (difference in latitude / 2) + (cosine of latitude 1 * cosine of latitude 2 * sinus squared (difference in longitude / 2))
// and R = the circumference of the earth
double differenceInLat = DegreeToRadian(currentLatitude - latitudeToCompare);
double differenceInLong = DegreeToRadian(currentLongitude - longtitudeToCompare);
double aInnerFormula = Math.Cos(DegreeToRadian(currentLatitude)) * Math.Cos(DegreeToRadian(latitudeToCompare)) * Math.Sin(differenceInLong / 2) * Math.Sin(differenceInLong / 2);
double aFormula = (Math.Sin((differenceInLat) / 2) * Math.Sin((differenceInLat) / 2)) + (aInnerFormula);
resultDistance = avgRadiusOfEarth * 2 * Math.Atan2(Math.Sqrt(aFormula), Math.Sqrt(1 - aFormula));
DegreesToRadian is a function I custom created, its is a simple 1 liner of"Math.PI * angle / 180.0
My blog entry - SQL Haversine
Are you looking for
Haversine formula
The haversine formula is an equation
important in navigation, giving
great-circle distances between two
points on a sphere from their
longitudes and latitudes. It is a
special case of a more general formula
in spherical trigonometry, the law of
haversines, relating the sides and
angles of spherical "triangles".
Have a look at this.. has a javascript example as well.
Find Distance
Use the Great Circle Distance Formula.
here is a fiddle with finding locations / near locations to long/lat by given IP:
http://jsfiddle.net/bassta/zrgd9qc3/2/
And here is the function I use to calculate the distance in straight line:
function distance(lat1, lng1, lat2, lng2) {
var radlat1 = Math.PI * lat1 / 180;
var radlat2 = Math.PI * lat2 / 180;
var radlon1 = Math.PI * lng1 / 180;
var radlon2 = Math.PI * lng2 / 180;
var theta = lng1 - lng2;
var radtheta = Math.PI * theta / 180;
var dist = Math.sin(radlat1) * Math.sin(radlat2) + Math.cos(radlat1) * Math.cos(radlat2) * Math.cos(radtheta);
dist = Math.acos(dist);
dist = dist * 180 / Math.PI;
dist = dist * 60 * 1.1515;
//Get in in kilometers
dist = dist * 1.609344;
return dist;
}
It returns the distance in Kilometers
If you are measuring distances less than (perhaps) 1 degree lat/long change, are looking for a very high performance approximation, and are willing to accept more inaccuracy than Haversine formula, consider these two alternatives:
(1) "Polar Coordinate Flat-Earth Formula" from Computing Distances:
a = pi/2 - lat1
b = pi/2 - lat2
c = sqrt( a^2 + b^2 - 2 * a * b * cos(lon2 - lon1) )
d = R * c
(2) Pythagorean theorem adjusted for latitude, as seen in Ewan Todd's SO post:
d_ew = (long1 - long0) * cos(average(lat0, lat1))
d_ns = (lat1 - lat0)
d = sqrt(d_ew * d_ew + d_ns * d_ns)
NOTES:
Compared to Ewan's post, I've substituted average(lat0, lat1) for lat0 inside of cos( lat0 ).
#2 is vague on whether values are degrees, radians, or kilometers; you will need some conversion code as well. See my complete code at bottom of this post.
#1 is designed to work well even near the poles, though if you are measuring a distance whose endpoints are on "opposite" sides of the pole (longitudes differ by more than 90 degrees?), Haversine is recommended instead, even for small distances.
I haven't thoroughly measured errors of these approaches, so you should take representative points for your application, and compare results to some high-quality library, to decide if the accuracies are acceptable. For distances less than a few kilometers my gut sense is that these are within 1% of correct measurement.
An alternative way to gain high performance (when applicable):
If you have a large set of static points, within one or two degrees of longitude/latitude, that you will then be calculating distances from a small number of dynamic (moving) points, consider converting your static points ONCE to the containing UTM zone (or to any other local Cartesian coordinate system), and then doing all your math in that Cartesian coordinate system.
Cartesian = flat earth = Pythagorean theorem applies, so distance = sqrt(dx^2 + dy^2).
Then the cost of accurately converting the few moving points to UTM is easily afforded.
CAVEAT for #1 (Polar): May be very wrong for distances less than 0.1 (?) meter. Even with double precision math, the following coordinates, whose true distance is about 0.005 meters, was given as "zero" by my implementation of Polar algorithm:
inputs:
lon1Xdeg 16.6564465477996 double
lat1Ydeg 57.7760262271983 double
lon2Xdeg 16.6564466358281 double
lat2Ydeg 57.776026248554 double
results:
Oblate spheroid formula:
0.00575254911118364 double
Haversine:
0.00573422966122257 double
Polar:
0
this was due to the two factors u and v exactly canceling each other:
u 0.632619944868587 double
v -0.632619944868587 double
In another case, it gave a distance of 0.067129 m when the oblate spheroid answer was 0.002887 m. The problem was that cos(lon2 - lon1) was too close to 1, so cos function returned exactly 1.
Other than measuring sub-meter distances, the max errors (compared to an oblate spheroid formula) I found for the limited small-distance data I've fed in so far:
maxHaversineErrorRatio 0.00350976281908381 double
maxPolarErrorRatio 0.0510789996931342 double
where "1" would represent a 100% error in the answer; e.g. when it returned "0", that was an error of "1" (excluded from above "maxPolar"). So "0.01" would be an error of "1 part in 100" or 1%.
Comparing Polar error with Haversine error over distances less than 2000 meters to see how much worse this simpler formula is. So far, the worst I've seen is 51 parts per 1000 for Polar vs 4 parts per 1000 for Haversine. At about 58 degrees latitude.
Now implemented "Pythagorean with Latitude Adjustment".
It is MUCH more consistent than Polar for distances < 2000 m.
I originally thought the Polar problems were only when < 1 m,
but the result shown immediately below is quite troubling.
As distances approach zero, pythagorean/latitude approaches haversine.
For example this measurement ~ 217 meters:
lon1Xdeg 16.6531667510102 double
lat1Ydeg 57.7751705615804 double
lon2Xdeg 16.6564468739869 double
lat2Ydeg 57.7760263007586 double
oblate 217.201200413731
haversine 216.518428601051
polar 226.128616011973
pythag-cos 216.518428631907
havErrRatio 0.00314349925958048
polErrRatio 0.041102054598393
pycErrRatio 0.00314349911751603
Polar has a much worse error with these inputs; either there is some mistake in my code, or in Cos function I am running on, or I have to recommend not using Polar, even though most Polar measurements were much closer than this.
OTOH, Pythagorean, even with * cos(latitude) adjustment, has error that increases more rapidly than distance (ratio of max_error/distance increases for larger distances), so you need to carefully consider the maximum distance you will measure, and the acceptable error. In addition, it is not advisable to COMPARE two nearly-equal distances using Pythagorean, to decide which is shorter, as the error is different in different DIRECTIONS (evidence not shown).
Worst case measurements, errorRatio = Abs(error) / distance (Sweden; up to 2000 m):
t_maxHaversineErrorRatio 0.00351012021578681 double
t_maxPolarErrorRatio 66.0825360597085 double
t_maxPythagoreanErrorRatio 0.00350976281416454 double
As mentioned before, the extreme polar errors are for sub-meter distances, where it could report zero instead of 6 cm, or report over 0.5 m for a distance of 1 cm (hence the "66 x" worst case shown in t_maxPolarErrorRatio), but there are also some poor results at larger distances. [Needs to be tested again with a Cosine function that is known to be highly accurate.]
Measurements taken in C# code in Xamarin.Android running on a Moto E4.
C# code:
// x=longitude, y= latitude. oblate spheroid formula. TODO: From where?
public static double calculateDistanceDD_AED( double lon1Xdeg, double lat1Ydeg, double lon2Xdeg, double lat2Ydeg )
{
double c_dblEarthRadius = 6378.135; // km
double c_dblFlattening = 1.0 / 298.257223563; // WGS84 inverse
// flattening
// Q: Why "-" for longitudes??
double p1x = -degreesToRadians( lon1Xdeg );
double p1y = degreesToRadians( lat1Ydeg );
double p2x = -degreesToRadians( lon2Xdeg );
double p2y = degreesToRadians( lat2Ydeg );
double F = (p1y + p2y) / 2;
double G = (p1y - p2y) / 2;
double L = (p1x - p2x) / 2;
double sing = Math.Sin( G );
double cosl = Math.Cos( L );
double cosf = Math.Cos( F );
double sinl = Math.Sin( L );
double sinf = Math.Sin( F );
double cosg = Math.Cos( G );
double S = sing * sing * cosl * cosl + cosf * cosf * sinl * sinl;
double C = cosg * cosg * cosl * cosl + sinf * sinf * sinl * sinl;
double W = Math.Atan2( Math.Sqrt( S ), Math.Sqrt( C ) );
if (W == 0.0)
return 0.0;
double R = Math.Sqrt( (S * C) ) / W;
double H1 = (3 * R - 1.0) / (2.0 * C);
double H2 = (3 * R + 1.0) / (2.0 * S);
double D = 2 * W * c_dblEarthRadius;
// Apply flattening factor
D = D * (1.0 + c_dblFlattening * H1 * sinf * sinf * cosg * cosg - c_dblFlattening * H2 * cosf * cosf * sing * sing);
// Transform to meters
D = D * 1000.0;
// tmstest
if (true)
{
// Compare Haversine.
double haversine = HaversineApproxDistanceGeo( lon1Xdeg, lat1Ydeg, lon2Xdeg, lat2Ydeg );
double error = haversine - D;
double absError = Math.Abs( error );
double errorRatio = absError / D;
if (errorRatio > t_maxHaversineErrorRatio)
{
if (errorRatio > t_maxHaversineErrorRatio * 1.1)
Helper.test();
t_maxHaversineErrorRatio = errorRatio;
}
// Compare Polar Coordinate Flat Earth.
double polarDistanceGeo = ApproxDistanceGeo_Polar( lon1Xdeg, lat1Ydeg, lon2Xdeg, lat2Ydeg, D );
double error2 = polarDistanceGeo - D;
double absError2 = Math.Abs( error2 );
double errorRatio2 = absError2 / D;
if (errorRatio2 > t_maxPolarErrorRatio)
{
if (polarDistanceGeo > 0)
{
if (errorRatio2 > t_maxPolarErrorRatio * 1.1)
Helper.test();
t_maxPolarErrorRatio = errorRatio2;
}
else
Helper.dubious();
}
// Compare Pythagorean Theorem with Latitude Adjustment.
double pythagoreanDistanceGeo = ApproxDistanceGeo_PythagoreanCosLatitude( lon1Xdeg, lat1Ydeg, lon2Xdeg, lat2Ydeg, D );
double error3 = pythagoreanDistanceGeo - D;
double absError3 = Math.Abs( error3 );
double errorRatio3 = absError3 / D;
if (errorRatio3 > t_maxPythagoreanErrorRatio)
{
if (D < 2000)
{
if (errorRatio3 > t_maxPythagoreanErrorRatio * 1.05)
Helper.test();
t_maxPythagoreanErrorRatio = errorRatio3;
}
}
}
return D;
}
// As a fraction of the distance.
private static double t_maxHaversineErrorRatio, t_maxPolarErrorRatio, t_maxPythagoreanErrorRatio;
// Average of equatorial and polar radii (meters).
public const double EarthAvgRadius = 6371000;
public const double EarthAvgCircumference = EarthAvgRadius * 2 * PI;
// CAUTION: This is an average of great circles; won't be the actual distance of any longitude or latitude degree.
public const double EarthAvgMeterPerGreatCircleDegree = EarthAvgCircumference / 360;
// Haversine formula (assumes Earth is sphere).
// "deg" = degrees.
// Perhaps based on Haversine Formula in https://cs.nyu.edu/visual/home/proj/tiger/gisfaq.html
public static double HaversineApproxDistanceGeo(double lon1Xdeg, double lat1Ydeg, double lon2Xdeg, double lat2Ydeg)
{
double lon1 = degreesToRadians( lon1Xdeg );
double lat1 = degreesToRadians( lat1Ydeg );
double lon2 = degreesToRadians( lon2Xdeg );
double lat2 = degreesToRadians( lat2Ydeg );
double dlon = lon2 - lon1;
double dlat = lat2 - lat1;
double sinDLat2 = Sin( dlat / 2 );
double sinDLon2 = Sin( dlon / 2 );
double a = sinDLat2 * sinDLat2 + Cos( lat1 ) * Cos( lat2 ) * sinDLon2 * sinDLon2;
double c = 2 * Atan2( Sqrt( a ), Sqrt( 1 - a ) );
double d = EarthAvgRadius * c;
return d;
}
// From https://stackoverflow.com/a/19772119/199364
// Based on Polar Coordinate Flat Earth in https://cs.nyu.edu/visual/home/proj/tiger/gisfaq.html
public static double ApproxDistanceGeo_Polar( double lon1deg, double lat1deg, double lon2deg, double lat2deg, double D = 0 )
{
double approxUnitDistSq = ApproxUnitDistSq_Polar(lon1deg, lat1deg, lon2deg, lat2deg, D);
double c = Sqrt( approxUnitDistSq );
return EarthAvgRadius * c;
}
// Might be useful to avoid taking Sqrt, when comparing to some threshold.
// Threshold would have to be adjusted to match: Power(threshold / EarthAvgRadius, 2)
private static double ApproxUnitDistSq_Polar(double lon1deg, double lat1deg, double lon2deg, double lat2deg, double D = 0 )
{
const double HalfPi = PI / 2; //1.5707963267949;
double lon1 = degreesToRadians(lon1deg);
double lat1 = degreesToRadians(lat1deg);
double lon2 = degreesToRadians(lon2deg);
double lat2 = degreesToRadians(lat2deg);
double a = HalfPi - lat1;
double b = HalfPi - lat2;
double u = a * a + b * b;
double dlon21 = lon2 - lon1;
double cosDeltaLon = Cos( dlon21 );
double v = -2 * a * b * cosDeltaLon;
// TBD: Is "Abs" necessary? That is, is "u + v" ever negative?
// (I think not; "v" looks like a secondary term. Though might be round-off issue near zero when a~=b.)
double approxUnitDistSq = Abs(u + v);
//if (approxUnitDistSq.nearlyEquals(0, 1E-16))
// Helper.dubious();
//else if (D > 0)
//{
// double dba = b - a;
// double unitD = D / EarthAvgRadius;
// double unitDSq = unitD * unitD;
// if (approxUnitDistSq > 2 * unitDSq)
// Helper.dubious();
// else if (approxUnitDistSq * 2 < unitDSq)
// Helper.dubious();
//}
return approxUnitDistSq;
}
// Pythagorean Theorem with Latitude Adjustment - from Ewan Todd - https://stackoverflow.com/a/1664836/199364
// Refined by ToolmakerSteve - https://stackoverflow.com/a/53468745/199364
public static double ApproxDistanceGeo_PythagoreanCosLatitude( double lon1deg, double lat1deg, double lon2deg, double lat2deg, double D = 0 )
{
double approxDegreesSq = ApproxDegreesSq_PythagoreanCosLatitude( lon1deg, lat1deg, lon2deg, lat2deg );
// approximate degrees on the great circle between the points.
double d_degrees = Sqrt( approxDegreesSq );
return d_degrees * EarthAvgMeterPerGreatCircleDegree;
}
public static double ApproxDegreesSq_PythagoreanCosLatitude( double lon1deg, double lat1deg, double lon2deg, double lat2deg )
{
double avgLatDeg = average( lat1deg , lat2deg );
double avgLat = degreesToRadians( avgLatDeg );
double d_ew = (lon2deg - lon1deg) * Cos( avgLat );
double d_ns = (lat2deg - lat1deg);
double approxDegreesSq = d_ew * d_ew + d_ns * d_ns;
return approxDegreesSq;
}
I am done using SQL query
select *, (acos(sin(input_lat* 0.01745329)*sin(lattitude *0.01745329) + cos(input_lat *0.01745329)*cos(lattitude *0.01745329)*cos((input_long -longitude)*0.01745329))* 57.29577951 )* 69.16 As D from table_name
Following is the module (coded in f90) containing three formulas discussed in the previous answers. You can either put this module at the top of your program
(before PROGRAM MAIN) or compile it separately and include the module directory during compilation. The following module contains three formulas. First two are great-circle distances based on the assumption that earth is spherical.
module spherical_dists
contains
subroutine great_circle_distance(lon1,lat1,lon2,lat2,dist)
!https://en.wikipedia.org/wiki/Great-circle_distance
! It takes lon, lats of two points on an assumed spherical earth and
! calculates the distance between them along the great circle connecting the two points
implicit none
real,intent(in)::lon1,lon2,lat1,lat2
real,intent(out)::dist
real,parameter::pi=3.141592,mean_earth_radius=6371.0088
real::lonr1,lonr2,latr1,latr2
real::delangl,dellon
lonr1=lon1*(pi/180.);lonr2=lon2*(pi/180.)
latr1=lat1*(pi/180.);latr2=lat2*(pi/180.)
dellon=lonr2-lonr1
delangl=acos(sin(latr1)*sin(latr2)+cos(latr1)*cos(latr2)*cos(dellon))
dist=delangl*mean_earth_radius
end subroutine
subroutine haversine_formula(lon1,lat1,lon2,lat2,dist)
! https://en.wikipedia.org/wiki/Haversine_formula
! This is similar above but numerically better conditioned for small distances
implicit none
real,intent(in)::lon1,lon2,lat1,lat2
!lon, lats of two points
real,intent(out)::dist
real,parameter::pi=3.141592,mean_earth_radius=6371.0088
real::lonr1,lonr2,latr1,latr2
real::delangl,dellon,dellat,a
! degrees are converted to radians
lonr1=lon1*(pi/180.);lonr2=lon2*(pi/180.)
latr1=lat1*(pi/180.);latr2=lat2*(pi/180.)
dellon=lonr2-lonr1 ! These dels simplify the haversine formula
dellat=latr2-latr1
! The actual haversine formula
a=(sin(dellat/2))**2+cos(latr1)*cos(latr2)*(sin(dellon/2))**2
delangl=2*asin(sqrt(a)) !2*asin(sqrt(a))
dist=delangl*mean_earth_radius
end subroutine
subroutine vincenty_formula(lon1,lat1,lon2,lat2,dist)
!https://en.wikipedia.org/wiki/Vincenty%27s_formulae
!It's a better approximation over previous two, since it considers earth to in oblate spheroid, which better approximates the shape of the earth
implicit none
real,intent(in)::lon1,lon2,lat1,lat2
real,intent(out)::dist
real,parameter::pi=3.141592,mean_earth_radius=6371.0088
real::lonr1,lonr2,latr1,latr2
real::delangl,dellon,nom,denom
lonr1=lon1*(pi/180.);lonr2=lon2*(pi/180.)
latr1=lat1*(pi/180.);latr2=lat2*(pi/180.)
dellon=lonr2-lonr1
nom=sqrt((cos(latr2)*sin(dellon))**2. + (cos(latr1)*sin(latr2)-sin(latr1)*cos(latr2)*cos(dellon))**2.)
denom=sin(latr1)*sin(latr2)+cos(latr1)*cos(latr2)*cos(dellon)
delangl=atan2(nom,denom)
dist=delangl*mean_earth_radius
end subroutine
end module
On this page you can see the whole code and formulas how distances of locations are calculated in Android Location class
android/location/Location.java
EDIT: According the hint from #Richard I put the code of the linked function into my answer, to avoid invalidated link:
private static void computeDistanceAndBearing(double lat1, double lon1,
double lat2, double lon2, BearingDistanceCache results) {
// Based on http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
// using the "Inverse Formula" (section 4)
int MAXITERS = 20;
// Convert lat/long to radians
lat1 *= Math.PI / 180.0;
lat2 *= Math.PI / 180.0;
lon1 *= Math.PI / 180.0;
lon2 *= Math.PI / 180.0;
double a = 6378137.0; // WGS84 major axis
double b = 6356752.3142; // WGS84 semi-major axis
double f = (a - b) / a;
double aSqMinusBSqOverBSq = (a * a - b * b) / (b * b);
double L = lon2 - lon1;
double A = 0.0;
double U1 = Math.atan((1.0 - f) * Math.tan(lat1));
double U2 = Math.atan((1.0 - f) * Math.tan(lat2));
double cosU1 = Math.cos(U1);
double cosU2 = Math.cos(U2);
double sinU1 = Math.sin(U1);
double sinU2 = Math.sin(U2);
double cosU1cosU2 = cosU1 * cosU2;
double sinU1sinU2 = sinU1 * sinU2;
double sigma = 0.0;
double deltaSigma = 0.0;
double cosSqAlpha = 0.0;
double cos2SM = 0.0;
double cosSigma = 0.0;
double sinSigma = 0.0;
double cosLambda = 0.0;
double sinLambda = 0.0;
double lambda = L; // initial guess
for (int iter = 0; iter < MAXITERS; iter++) {
double lambdaOrig = lambda;
cosLambda = Math.cos(lambda);
sinLambda = Math.sin(lambda);
double t1 = cosU2 * sinLambda;
double t2 = cosU1 * sinU2 - sinU1 * cosU2 * cosLambda;
double sinSqSigma = t1 * t1 + t2 * t2; // (14)
sinSigma = Math.sqrt(sinSqSigma);
cosSigma = sinU1sinU2 + cosU1cosU2 * cosLambda; // (15)
sigma = Math.atan2(sinSigma, cosSigma); // (16)
double sinAlpha = (sinSigma == 0) ? 0.0 :
cosU1cosU2 * sinLambda / sinSigma; // (17)
cosSqAlpha = 1.0 - sinAlpha * sinAlpha;
cos2SM = (cosSqAlpha == 0) ? 0.0 :
cosSigma - 2.0 * sinU1sinU2 / cosSqAlpha; // (18)
double uSquared = cosSqAlpha * aSqMinusBSqOverBSq; // defn
A = 1 + (uSquared / 16384.0) * // (3)
(4096.0 + uSquared *
(-768 + uSquared * (320.0 - 175.0 * uSquared)));
double B = (uSquared / 1024.0) * // (4)
(256.0 + uSquared *
(-128.0 + uSquared * (74.0 - 47.0 * uSquared)));
double C = (f / 16.0) *
cosSqAlpha *
(4.0 + f * (4.0 - 3.0 * cosSqAlpha)); // (10)
double cos2SMSq = cos2SM * cos2SM;
deltaSigma = B * sinSigma * // (6)
(cos2SM + (B / 4.0) *
(cosSigma * (-1.0 + 2.0 * cos2SMSq) -
(B / 6.0) * cos2SM *
(-3.0 + 4.0 * sinSigma * sinSigma) *
(-3.0 + 4.0 * cos2SMSq)));
lambda = L +
(1.0 - C) * f * sinAlpha *
(sigma + C * sinSigma *
(cos2SM + C * cosSigma *
(-1.0 + 2.0 * cos2SM * cos2SM))); // (11)
double delta = (lambda - lambdaOrig) / lambda;
if (Math.abs(delta) < 1.0e-12) {
break;
}
}
float distance = (float) (b * A * (sigma - deltaSigma));
results.mDistance = distance;
float initialBearing = (float) Math.atan2(cosU2 * sinLambda,
cosU1 * sinU2 - sinU1 * cosU2 * cosLambda);
initialBearing *= 180.0 / Math.PI;
results.mInitialBearing = initialBearing;
float finalBearing = (float) Math.atan2(cosU1 * sinLambda,
-sinU1 * cosU2 + cosU1 * sinU2 * cosLambda);
finalBearing *= 180.0 / Math.PI;
results.mFinalBearing = finalBearing;
results.mLat1 = lat1;
results.mLat2 = lat2;
results.mLon1 = lon1;
results.mLon2 = lon2;
}
just use the distance formula Sqrt( (x2-x1)^2 + (y2-y1)^2 )