Find the tangent of a point on a cubic bezier curve - ios

For a cubic Bézier curve, with the usual four points a, b, c and d,
for a given value t,
how to most elegantly find the tangent at that point?

The tangent of a curve is simply its derivative. The parametric equation that Michal uses:
P(t) = (1 - t)^3 * P0 + 3t(1-t)^2 * P1 + 3t^2 (1-t) * P2 + t^3 * P3
should have a derivative of
dP(t) / dt = -3(1-t)^2 * P0 + 3(1-t)^2 * P1 - 6t(1-t) * P1 - 3t^2 * P2 + 6t(1-t) * P2 + 3t^2 * P3
Which, by the way, appears to be wrong in your earlier question. I believe you're using the slope for a quadratic Bezier curve there, not cubic.
From there, it should be trivial to implement a C function that performs this calculation, like Michal has already provided for the curve itself.

Here is fully tested code to copy and paste:
It draws approxidistant points along the curve, and it draws the tangents.
bezierInterpolation finds the points
bezierTangent finds the tangents
There are TWO VERSIONS of bezierInterpolation supplied below:
bezierInterpolation works perfectly.
altBezierInterpolation is exactly the same, BUT it is written in an expanded, clear, explanatory manner. It makes the arithmetic much easier to understand.
Use either of those two routines: the results are identical.
In both cases, use bezierTangent to find the tangents. (Note: Michal's fabulous code base here.)
A full example of how to use with drawRect: is also included.
// MBBezierView.m original BY MICHAL stackoverflow #4058979
#import "MBBezierView.h"
CGFloat bezierInterpolation(
CGFloat t, CGFloat a, CGFloat b, CGFloat c, CGFloat d) {
// see also below for another way to do this, that follows the 'coefficients'
// idea, and is a little clearer
CGFloat t2 = t * t;
CGFloat t3 = t2 * t;
return a + (-a * 3 + t * (3 * a - a * t)) * t
+ (3 * b + t * (-6 * b + b * 3 * t)) * t
+ (c * 3 - c * 3 * t) * t2
+ d * t3;
}
CGFloat altBezierInterpolation(
CGFloat t, CGFloat a, CGFloat b, CGFloat c, CGFloat d)
{
// here's an alternative to Michal's bezierInterpolation above.
// the result is absolutely identical.
// of course, you could calculate the four 'coefficients' only once for
// both this and the slope calculation, if desired.
CGFloat C1 = ( d - (3.0 * c) + (3.0 * b) - a );
CGFloat C2 = ( (3.0 * c) - (6.0 * b) + (3.0 * a) );
CGFloat C3 = ( (3.0 * b) - (3.0 * a) );
CGFloat C4 = ( a );
// it's now easy to calculate the point, using those coefficients:
return ( C1*t*t*t + C2*t*t + C3*t + C4 );
}
CGFloat bezierTangent(CGFloat t, CGFloat a, CGFloat b, CGFloat c, CGFloat d)
{
// note that abcd are aka x0 x1 x2 x3
/* the four coefficients ..
A = x3 - 3 * x2 + 3 * x1 - x0
B = 3 * x2 - 6 * x1 + 3 * x0
C = 3 * x1 - 3 * x0
D = x0
and then...
Vx = 3At2 + 2Bt + C */
// first calcuate what are usually know as the coeffients,
// they are trivial based on the four control points:
CGFloat C1 = ( d - (3.0 * c) + (3.0 * b) - a );
CGFloat C2 = ( (3.0 * c) - (6.0 * b) + (3.0 * a) );
CGFloat C3 = ( (3.0 * b) - (3.0 * a) );
CGFloat C4 = ( a ); // (not needed for this calculation)
// finally it is easy to calculate the slope element,
// using those coefficients:
return ( ( 3.0 * C1 * t* t ) + ( 2.0 * C2 * t ) + C3 );
// note that this routine works for both the x and y side;
// simply run this routine twice, once for x once for y
// note that there are sometimes said to be 8 (not 4) coefficients,
// these are simply the four for x and four for y,
// calculated as above in each case.
}
#implementation MBBezierView
- (void)drawRect:(CGRect)rect {
CGPoint p1, p2, p3, p4;
p1 = CGPointMake(30, rect.size.height * 0.33);
p2 = CGPointMake(CGRectGetMidX(rect), CGRectGetMinY(rect));
p3 = CGPointMake(CGRectGetMidX(rect), CGRectGetMaxY(rect));
p4 = CGPointMake(-30 + CGRectGetMaxX(rect), rect.size.height * 0.66);
[[UIColor blackColor] set];
[[UIBezierPath bezierPathWithRect:rect] fill];
[[UIColor redColor] setStroke];
UIBezierPath *bezierPath = [[[UIBezierPath alloc] init] autorelease];
[bezierPath moveToPoint:p1];
[bezierPath addCurveToPoint:p4 controlPoint1:p2 controlPoint2:p3];
[bezierPath stroke];
[[UIColor brownColor] setStroke];
// now mark in points along the bezier!
for (CGFloat t = 0.0; t <= 1.00001; t += 0.05) {
[[UIColor brownColor] setStroke];
CGPoint point = CGPointMake(
bezierInterpolation(t, p1.x, p2.x, p3.x, p4.x),
bezierInterpolation(t, p1.y, p2.y, p3.y, p4.y));
// there, use either bezierInterpolation or altBezierInterpolation,
// identical results for the position
// just draw that point to indicate it...
UIBezierPath *pointPath =
[UIBezierPath bezierPathWithArcCenter:point
radius:5 startAngle:0 endAngle:2*M_PI clockwise:YES];
[pointPath stroke];
// now find the tangent if someone on stackoverflow knows how
CGPoint vel = CGPointMake(
bezierTangent(t, p1.x, p2.x, p3.x, p4.x),
bezierTangent(t, p1.y, p2.y, p3.y, p4.y));
// the following code simply draws an indication of the tangent
CGPoint demo = CGPointMake( point.x + (vel.x*0.3),
point.y + (vel.y*0.33) );
// (the only reason for the .3 is to make the pointers shorter)
[[UIColor whiteColor] setStroke];
UIBezierPath *vp = [UIBezierPath bezierPath];
[vp moveToPoint:point];
[vp addLineToPoint:demo];
[vp stroke];
}
}
#end
to draw that class...
MBBezierView *mm = [[MBBezierView alloc]
initWithFrame:CGRectMake(400,20, 600,700)];
[mm setNeedsDisplay];
[self addSubview:mm];
Here are the two routines to calculate approximately equidistant points, and the tangents of those, along a bezier cubic.
For clarity and reliability, these routines are written in the simplest, most explanatory, way possible.
CGFloat bezierPoint(CGFloat t, CGFloat a, CGFloat b, CGFloat c, CGFloat d)
{
CGFloat C1 = ( d - (3.0 * c) + (3.0 * b) - a );
CGFloat C2 = ( (3.0 * c) - (6.0 * b) + (3.0 * a) );
CGFloat C3 = ( (3.0 * b) - (3.0 * a) );
CGFloat C4 = ( a );
return ( C1*t*t*t + C2*t*t + C3*t + C4 );
}
CGFloat bezierTangent(CGFloat t, CGFloat a, CGFloat b, CGFloat c, CGFloat d)
{
CGFloat C1 = ( d - (3.0 * c) + (3.0 * b) - a );
CGFloat C2 = ( (3.0 * c) - (6.0 * b) + (3.0 * a) );
CGFloat C3 = ( (3.0 * b) - (3.0 * a) );
CGFloat C4 = ( a );
return ( ( 3.0 * C1 * t* t ) + ( 2.0 * C2 * t ) + C3 );
}
The four precalculated values, C1 C2 C3 C4, are sometimes called the coefficients of the bezier. (Recall that a b c d are usually called the four control points.)
Of course, t runs from 0 to 1, for example every 0.05.
Simply call these routines once for X, and then once separately for Y.
Hope it helps someone!
Important facts:
(1) It is an absolute fact that: unfortunately, there is, definitely, NO method, provided by Apple, to extract points from a UIBezierPath. True as of 2019.
(2) Don't forget it's as easy as pie to animate something along a UIBezierPath. Google many examples.
(3) Many ask, "Can't CGPathApply be used to extract the points from a UIBezierPath?" No, CGPathApply is totally unrelated: it simply gives you a list of your "instructions in making any path" (so, "start here", "draw a straight line to this point", etc etc.) The name is confusing but CGPathApply is totally unrelated to bezier paths.
For game programmers - as #Engineer points out you may well want the normal of the tangent, fortunately Apple has vector math built-in:
https://developer.apple.com/documentation/accelerate/simd/working_with_vectors
https://developer.apple.com/documentation/simd/2896658-simd_normalize

I found it too error-prone to use the supplied equations. Too easy to miss a subtle t or misplaced bracket.
By contrast, Wikipedia provides a much clearer, cleaner, derivative IMHO:
...which implements easily in code as:
3f * oneMinusT * oneMinusT * (p1 - p0)
+ 6f * t * oneMinusT * (p2 - p1)
+ 3f * t * t * (p3 - p2)
(assuming you have vector-minus configured in your language of choice; question isn't marked as ObjC specifically, and iOS now has several langs available)

Here goes my Swift implementation.
Which I tried my best to optimize for speed, by eliminating all redundant math operations. i.e. make the minimal numbers of calls to math operations. And use the least possible number of multiplications (which are much more expensive than sums).
There are 0 multiplications to create the bezier.
Then 3 multiplications to get a point of bezier.
And 2 multiplications to get a tangent to the bezier.
struct CubicBezier {
private typealias Me = CubicBezier
typealias Vector = CGVector
typealias Point = CGPoint
typealias Num = CGFloat
typealias Coeficients = (C: Num, S: Num, M: Num, L: Num)
let xCoeficients: Coeficients
let yCoeficients: Coeficients
static func coeficientsOfCurve(from c0: Num, through c1: Num, andThrough c2: Num, to c3: Num) -> Coeficients
{
let _3c0 = c0 + c0 + c0
let _3c1 = c1 + c1 + c1
let _3c2 = c2 + c2 + c2
let _6c1 = _3c1 + _3c1
let C = c3 - _3c2 + _3c1 - c0
let S = _3c2 - _6c1 + _3c0
let M = _3c1 - _3c0
let L = c0
return (C, S, M, L)
}
static func xOrYofCurveWith(coeficients coefs: Coeficients, at t: Num) -> Num
{
let (C, S, M, L) = coefs
return ((C * t + S) * t + M) * t + L
}
static func xOrYofTangentToCurveWith(coeficients coefs: Coeficients, at t: Num) -> Num
{
let (C, S, M, _) = coefs
return ((C + C + C) * t + S + S) * t + M
}
init(from start: Point, through c1: Point, andThrough c2: Point, to end: Point)
{
xCoeficients = Me.coeficientsOfCurve(from: start.x, through: c1.x, andThrough: c2.x, to: end.x)
yCoeficients = Me.coeficientsOfCurve(from: start.y, through: c1.y, andThrough: c2.y, to: end.y)
}
func x(at t: Num) -> Num {
return Me.xOrYofCurveWith(coeficients: xCoeficients, at: t)
}
func y(at t: Num) -> Num {
return Me.xOrYofCurveWith(coeficients: yCoeficients, at: t)
}
func dx(at t: Num) -> Num {
return Me.xOrYofTangentToCurveWith(coeficients: xCoeficients, at: t)
}
func dy(at t: Num) -> Num {
return Me.xOrYofTangentToCurveWith(coeficients: yCoeficients, at: t)
}
func point(at t: Num) -> Point {
return .init(x: x(at: t), y: y(at: t))
}
func tangent(at t: Num) -> Vector {
return .init(dx: dx(at: t), dy: dy(at: t))
}
}
Use like:
let bezier = CubicBezier.init(from: .zero, through: .zero, andThrough: .zero, to: .zero)
let point02 = bezier.point(at: 0.2)
let point07 = bezier.point(at: 0.7)
let tangent01 = bezier.tangent(at: 0.1)
let tangent05 = bezier.tangent(at: 0.5)

I couldn't get any of this to work until I realized that for parametric equations, (dy/dt)/(dx/dt) = dy/dx

Related

Is there a way to check if an XYZ triplet is a valid color?

The XYZ color space encompasses all possible colors, not just those which can be generated by a particular device like a monitor. Not all XYZ triplets represent a color that is physically possible. Is there a way, given an XYZ triplet, to determine if it represents a real color?
I wanted to generate a CIE 1931 chromaticity diagram (seen bellow) for myself, but wasn't sure how to go about it. It's easy to, for example, take all combinations of sRGB triplets and then transform them into the xy coordinates of the chromaticity diagram and then plot them. You cannot use this same approach in the XYZ color space though since not all combinations are valid colors. So far the best I have come up with is a stochastic approach, where I generate a random spectral distribution by summing a random number of random Gaussians, then converting it to XYZ using the standard observer functions.
Having thought about it a little more I felt the obvious solution is to generate a list of xy points around the edge of spectral locus, corresponding to pure monochromatic colors. It seems to me that this can be done by directly inputting the visible frequencies (~380-780nm) into the CIE XYZ standard observer color matching functions. Treating these points like a convex polygon you could determine if a point is within the spectral locus using one algorithm or another. In my case, since what I really wanted to do is simply generate the chromaticity diagram, I simply input these points into a graphics library's polygon drawing routine and then for each pixel of the polygon I can transform it into sRGB.
I believe this solution is similar to the one used by the library that Kel linked in a comment. I'm not entirely sure, as I am not familiar with Python.
function RGBfromXYZ(X, Y, Z) {
const R = 3.2404542 * X - 1.5371385 * Y - 0.4985314 * Z
const G = -0.969266 * X + 1.8760108 * Y + 0.0415560 * Z
const B = 0.0556434 * X - 0.2040259 * Y + 1.0572252 * Z
return [R, G, B]
}
function XYZfromYxy(Y, x, y) {
const X = Y / y * x
const Z = Y / y * (1 - x - y)
return [X, Y, Z]
}
function srgb_from_linear(x) {
if (x <= 0.0031308) {
return x * 12.92
} else {
return 1.055 * Math.pow(x, 1/2.4) - 0.055
}
}
// Analytic Approximations to the CIE XYZ Color Matching Functions
// from Sloan http://jcgt.org/published/0002/02/01/paper.pdf
function xFit_1931(x) {
const t1 = (x - 442) * (x < 442 ? 0.0624 : 0.0374)
const t2 = (x -599.8) * (x < 599.8 ? 0.0264 : 0.0323)
const t3 = (x - 501.1) * (x < 501.1 ? 0.0490 : 0.0382)
return 0.362 * Math.exp(-0.5 * t1 * t1) + 1.056 * Math.exp(-0.5 * t2 * t2) - 0.065 * Math.exp(-0.5 * t3 * t3)
}
function yFit_1931(x) {
const t1 = (x - 568.8) * (x < 568.8 ? 0.0213 : 0.0247)
const t2 = (x - 530.9) * (x < 530.9 ? 0.0613 : 0.0322)
return 0.821 * Math.exp(-0.5 * t1 * t1) + 0.286 * Math.exp(-0.5 * t2 * t2)
}
function zFit_1931(x) {
const t1 = (x - 437) * (x < 437 ? 0.0845 : 0.0278)
const t2 = (x - 459) * (x < 459 ? 0.0385 : 0.0725)
return 1.217 * Math.exp(-0.5 * t1 * t1) + 0.681 * Math.exp(-0.5 * t2 * t2)
}
const canvas = document.createElement("canvas")
document.body.append(canvas)
canvas.width = canvas.height = 512
const ctx = canvas.getContext("2d")
const locus_points = []
for (let i = 440; i < 650; ++i) {
const [X, Y, Z] = [xFit_1931(i), yFit_1931(i), zFit_1931(i)]
const x = (X / (X + Y + Z)) * canvas.width
const y = (Y / (X + Y + Z)) * canvas.height
locus_points.push([x, y])
}
ctx.beginPath()
ctx.moveTo(...locus_points[0])
locus_points.slice(1).forEach(point => ctx.lineTo(...point))
ctx.closePath()
ctx.fill()
const imageData = ctx.getImageData(0, 0, canvas.width, canvas.height)
for (let y = 0; y < canvas.height; ++y) {
for (let x = 0; x < canvas.width; ++x) {
const alpha = imageData.data[(y * canvas.width + x) * 4 + 3]
if (alpha > 0) {
const [X, Y, Z] = XYZfromYxy(1, x / canvas.width, y / canvas.height)
const [R, G, B] = RGBfromXYZ(X, Y, Z)
const r = Math.round(srgb_from_linear(R / Math.sqrt(R**2 + G**2 + B**2)) * 255)
const g = Math.round(srgb_from_linear(G / Math.sqrt(R**2 + G**2 + B**2)) * 255)
const b = Math.round(srgb_from_linear(B / Math.sqrt(R**2 + G**2 + B**2)) * 255)
imageData.data[(y * canvas.width + x) * 4 + 0] = r
imageData.data[(y * canvas.width + x) * 4 + 1] = g
imageData.data[(y * canvas.width + x) * 4 + 2] = b
}
}
}
ctx.putImageData(imageData, 0, 0)

Two drawn lines affecting each other [duplicate]

Closed. This question does not meet Stack Overflow guidelines. It is not currently accepting answers.
This question does not appear to be about programming within the scope defined in the help center.
Closed 6 years ago.
Locked. This question and its answers are locked because the question is off-topic but has historical significance. It is not currently accepting new answers or interactions.
How do I determine whether or not two lines intersect, and if they do, at what x,y point?
There’s a nice approach to this problem that uses vector cross products. Define the 2-dimensional vector cross product v × w to be vx wy − vy wx.
Suppose the two line segments run from p to p + r and from q to q + s. Then any point on the first line is representable as p + t r (for a scalar parameter t) and any point on the second line as q + u s (for a scalar parameter u).
The two lines intersect if we can find t and u such that:
p + t r = q + u s
Cross both sides with s, getting
(p + t r) × s = (q + u s) × s
And since s × s = 0, this means
t (r × s) = (q − p) × s
And therefore, solving for t:
t = (q − p) × s / (r × s)
In the same way, we can solve for u:
(p + t r) × r = (q + u s) × r
u (s × r) = (p − q) × r
u = (p − q) × r / (s × r)
To reduce the number of computation steps, it's convenient to rewrite this as follows (remembering that s × r = − r × s):
u = (q − p) × r / (r × s)
Now there are four cases:
If r × s = 0 and (q − p) × r = 0, then the two lines are collinear.
In this case, express the endpoints of the second segment (q and q + s) in terms of the equation of the first line segment (p + t r):
t0 = (q − p) · r / (r · r)
t1 = (q + s − p) · r / (r · r) = t0 + s · r / (r · r)
If the interval between t0 and t1 intersects the interval [0, 1] then the line segments are collinear and overlapping; otherwise they are collinear and disjoint.
Note that if s and r point in opposite directions, then s · r < 0 and so the interval to be checked is [t1, t0] rather than [t0, t1].
If r × s = 0 and (q − p) × r ≠ 0, then the two lines are parallel and non-intersecting.
If r × s ≠ 0 and 0 ≤ t ≤ 1 and 0 ≤ u ≤ 1, the two line segments meet at the point p + t r = q + u s.
Otherwise, the two line segments are not parallel but do not intersect.
Credit: this method is the 2-dimensional specialization of the 3D line intersection algorithm from the article "Intersection of two lines in three-space" by Ronald Goldman, published in Graphics Gems, page 304. In three dimensions, the usual case is that the lines are skew (neither parallel nor intersecting) in which case the method gives the points of closest approach of the two lines.
FWIW, the following function (in C) both detects line intersections and determines the intersection point. It is based on an algorithm in Andre LeMothe's "Tricks of the Windows Game Programming Gurus". It's not dissimilar to some of the algorithm's in other answers (e.g. Gareth's). LeMothe then uses Cramer's Rule (don't ask me) to solve the equations themselves.
I can attest that it works in my feeble asteroids clone, and seems to deal correctly with the edge cases described in other answers by Elemental, Dan and Wodzu. It's also probably faster than the code posted by KingNestor because it's all multiplication and division, no square roots!
I guess there's some potential for divide by zero in there, though it hasn't been an issue in my case. Easy enough to modify to avoid the crash anyway.
// Returns 1 if the lines intersect, otherwise 0. In addition, if the lines
// intersect the intersection point may be stored in the floats i_x and i_y.
char get_line_intersection(float p0_x, float p0_y, float p1_x, float p1_y,
float p2_x, float p2_y, float p3_x, float p3_y, float *i_x, float *i_y)
{
float s1_x, s1_y, s2_x, s2_y;
s1_x = p1_x - p0_x; s1_y = p1_y - p0_y;
s2_x = p3_x - p2_x; s2_y = p3_y - p2_y;
float s, t;
s = (-s1_y * (p0_x - p2_x) + s1_x * (p0_y - p2_y)) / (-s2_x * s1_y + s1_x * s2_y);
t = ( s2_x * (p0_y - p2_y) - s2_y * (p0_x - p2_x)) / (-s2_x * s1_y + s1_x * s2_y);
if (s >= 0 && s <= 1 && t >= 0 && t <= 1)
{
// Collision detected
if (i_x != NULL)
*i_x = p0_x + (t * s1_x);
if (i_y != NULL)
*i_y = p0_y + (t * s1_y);
return 1;
}
return 0; // No collision
}
BTW, I must say that in LeMothe's book, though he apparently gets the algorithm right, the concrete example he shows plugs in the wrong numbers and does calculations wrong. For example:
(4 * (4 - 1) + 12 * (7 - 1)) / (17 * 4 + 12 * 10)
= 844/0.88
= 0.44
That confused me for hours. :(
The problem reduces to this question: Do two lines from A to B and from C to D intersect? Then you can ask it four times (between the line and each of the four sides of the rectangle).
Here's the vector math for doing it. I'm assuming the line from A to B is the line in question and the line from C to D is one of the rectangle lines. My notation is that Ax is the "x-coordinate of A" and Cy is the "y-coordinate of C." And "*" means dot-product, so e.g. A*B = Ax*Bx + Ay*By.
E = B-A = ( Bx-Ax, By-Ay )
F = D-C = ( Dx-Cx, Dy-Cy )
P = ( -Ey, Ex )
h = ( (A-C) * P ) / ( F * P )
This h number is the key. If h is between 0 and 1, the lines intersect, otherwise they don't. If F*P is zero, of course you cannot make the calculation, but in this case the lines are parallel and therefore only intersect in the obvious cases.
The exact point of intersection is C + F*h.
More Fun:
If h is exactly 0 or 1 the lines touch at an end-point. You can consider this an "intersection" or not as you see fit.
Specifically, h is how much you have to multiply the length of the line in order to exactly touch the other line.
Therefore, If h<0, it means the rectangle line is "behind" the given line (with "direction" being "from A to B"), and if h>1 the rectangle line is "in front" of the given line.
Derivation:
A and C are vectors that point to the start of the line; E and F are the vectors from the ends of A and C that form the line.
For any two non-parallel lines in the plane, there must be exactly one pair of scalar g and h such that this equation holds:
A + E*g = C + F*h
Why? Because two non-parallel lines must intersect, which means you can scale both lines by some amount each and touch each other.
(At first this looks like a single equation with two unknowns! But it isn't when you consider that this is a 2D vector equation, which means this is really a pair of equations in x and y.)
We have to eliminate one of these variables. An easy way is to make the E term zero. To do that, take the dot-product of both sides of the equation using a vector that will dot to zero with E. That vector I called P above, and I did the obvious transformation of E.
You now have:
A*P = C*P + F*P*h
(A-C)*P = (F*P)*h
( (A-C)*P ) / (F*P) = h
I have tried to implement the algorithm so elegantly described by Jason above; unfortunately while working though the mathematics in the debugging I found many cases for which it doesn't work.
For example consider the points A(10,10) B(20,20) C(10,1) D(1,10) gives h=.5 and yet it is clear by examination that these segments are no-where near each other.
Graphing this makes it clear that 0 < h < 1 criteria only indicates that the intercept point would lie on CD if it existed but tells one nothing of whether that point lies on AB.
To ensure that there is a cross point you must do the symmetrical calculation for the variable g and the requirement for interception is:
0 < g < 1 AND 0 < h < 1
Here's an improvement to Gavin's answer. marcp's solution is similar also, but neither postpone the division.
This actually turns out to be a practical application of Gareth Rees' answer as well, because the cross-product's equivalent in 2D is the perp-dot-product, which is what this code uses three of. Switching to 3D and using the cross-product, interpolating both s and t at the end, results in the two closest points between the lines in 3D.
Anyway, the 2D solution:
int get_line_intersection(float p0_x, float p0_y, float p1_x, float p1_y,
float p2_x, float p2_y, float p3_x, float p3_y, float *i_x, float *i_y)
{
float s02_x, s02_y, s10_x, s10_y, s32_x, s32_y, s_numer, t_numer, denom, t;
s10_x = p1_x - p0_x;
s10_y = p1_y - p0_y;
s32_x = p3_x - p2_x;
s32_y = p3_y - p2_y;
denom = s10_x * s32_y - s32_x * s10_y;
if (denom == 0)
return 0; // Collinear
bool denomPositive = denom > 0;
s02_x = p0_x - p2_x;
s02_y = p0_y - p2_y;
s_numer = s10_x * s02_y - s10_y * s02_x;
if ((s_numer < 0) == denomPositive)
return 0; // No collision
t_numer = s32_x * s02_y - s32_y * s02_x;
if ((t_numer < 0) == denomPositive)
return 0; // No collision
if (((s_numer > denom) == denomPositive) || ((t_numer > denom) == denomPositive))
return 0; // No collision
// Collision detected
t = t_numer / denom;
if (i_x != NULL)
*i_x = p0_x + (t * s10_x);
if (i_y != NULL)
*i_y = p0_y + (t * s10_y);
return 1;
}
Basically it postpones the division until the last moment, and moves most of the tests until before certain calculations are done, thereby adding early-outs. Finally, it also avoids the division by zero case which occurs when the lines are parallel.
You also might want to consider using an epsilon test rather than comparison against zero. Lines that are extremely close to parallel can produce results that are slightly off. This is not a bug, it is a limitation with floating point math.
Question C: How do you detect whether or not two line segments intersect?
I have searched for the same topic, and I wasn't happy with the answers. So I have written an article that explains very detailed how to check if two line segments intersect with a lot of images. There is complete (and tested) Java-code.
Here is the article, cropped to the most important parts:
The algorithm, that checks if line segment a intersects with line segment b, looks like this:
What are bounding boxes? Here are two bounding boxes of two line segments:
If both bounding boxes have an intersection, you move line segment a so that one point is at (0|0). Now you have a line through the origin defined by a. Now move line segment b the same way and check if the new points of line segment b are on different sides of line a. If this is the case, check it the other way around. If this is also the case, the line segments intersect. If not, they don't intersect.
Question A: Where do two line segments intersect?
You know that two line segments a and b intersect. If you don't know that, check it with the tools I gave you in "Question C".
Now you can go through some cases and get the solution with 7th grade math (see code and interactive example).
Question B: How do you detect whether or not two lines intersect?
Let's say your point A = (x1, y1), point B = (x2, y2), C = (x_3, y_3), D = (x_4, y_4).
Your first line is defined by AB (with A != B), and your second one by CD (with C != D).
function doLinesIntersect(AB, CD) {
if (x1 == x2) {
return !(x3 == x4 && x1 != x3);
} else if (x3 == x4) {
return true;
} else {
// Both lines are not parallel to the y-axis
m1 = (y1-y2)/(x1-x2);
m2 = (y3-y4)/(x3-x4);
return m1 != m2;
}
}
Question D: Where do two lines intersect?
Check with Question B if they intersect at all.
The lines a and b are defined by two points for each line.
You can basically apply the same logic was used in Question A.
The answer once accepted here is incorrect (it has since been unaccepted, so hooray!). It does not correctly eliminate all non-intersections. Trivially it may appear to work but it can fail, especially in the case that 0 and 1 are considered valid for h.
Consider the following case:
Lines at (4,1)-(5,1) and (0,0)-(0,2)
These are perpendicular lines which clearly do not overlap.
A=(4,1)
B=(5,1)
C=(0,0)
D=(0,2)
E=(5,1)-(4,1)=(-1,0)
F=(0,2)-(0,0)=(0,-2)
P=(0,1)
h=((4,1)-(0,0)) dot (0,1) / ((0,-2) dot (0,1)) = 0
According to the above answer, these two line segments meet at an endpoint (values of 0 and 1). That endpoint would be:
(0,0)+(0,-2)*0=(0,0)
So, apparently the two line segments meet at (0,0), which is on line CD, but not on line AB. So what is going wrong? The answer is that the values of 0 and 1 are not valid and only sometimes HAPPEN to correctly predict endpoint intersection. When the extension of one line (but not the other) would meet the line segment, the algorithm predicts an intersection of line segments, but this is not correct. I imagine that by testing starting with AB vs CD and then also testing with CD vs AB, this problem would be eliminated. Only if both fall between 0 and 1 inclusively can they be said to intersect.
I recommend using the vector cross product method if you must predict end-points.
-Dan
Python version of iMalc's answer:
def find_intersection( p0, p1, p2, p3 ) :
s10_x = p1[0] - p0[0]
s10_y = p1[1] - p0[1]
s32_x = p3[0] - p2[0]
s32_y = p3[1] - p2[1]
denom = s10_x * s32_y - s32_x * s10_y
if denom == 0 : return None # collinear
denom_is_positive = denom > 0
s02_x = p0[0] - p2[0]
s02_y = p0[1] - p2[1]
s_numer = s10_x * s02_y - s10_y * s02_x
if (s_numer < 0) == denom_is_positive : return None # no collision
t_numer = s32_x * s02_y - s32_y * s02_x
if (t_numer < 0) == denom_is_positive : return None # no collision
if (s_numer > denom) == denom_is_positive or (t_numer > denom) == denom_is_positive : return None # no collision
# collision detected
t = t_numer / denom
intersection_point = [ p0[0] + (t * s10_x), p0[1] + (t * s10_y) ]
return intersection_point
Finding the correct intersection of two line segments is a non-trivial task with lots of edge cases. Here's a well documented, working and tested solution in Java.
In essence, there are three things that can happen when finding the intersection of two line segments:
The segments do not intersect
There is a unique intersection point
The intersection is another segment
NOTE: In the code, I assume that a line segment (x1, y1), (x2, y2) with x1 = x2 and y1 = y2 is a valid line segment. Mathematically speaking, a line segment consists of distinct points, but I am allowing segments to be points in this implementation for completeness.
Code is taken from my github repo
/**
* This snippet finds the intersection of two line segments.
* The intersection may either be empty, a single point or the
* intersection is a subsegment there's an overlap.
*/
import static java.lang.Math.abs;
import static java.lang.Math.max;
import static java.lang.Math.min;
import java.util.ArrayList;
import java.util.List;
public class LineSegmentLineSegmentIntersection {
// Small epsilon used for double value comparison.
private static final double EPS = 1e-5;
// 2D Point class.
public static class Pt {
double x, y;
public Pt(double x, double y) {
this.x = x;
this.y = y;
}
public boolean equals(Pt pt) {
return abs(x - pt.x) < EPS && abs(y - pt.y) < EPS;
}
}
// Finds the orientation of point 'c' relative to the line segment (a, b)
// Returns 0 if all three points are collinear.
// Returns -1 if 'c' is clockwise to segment (a, b), i.e right of line formed by the segment.
// Returns +1 if 'c' is counter clockwise to segment (a, b), i.e left of line
// formed by the segment.
public static int orientation(Pt a, Pt b, Pt c) {
double value = (b.y - a.y) * (c.x - b.x) -
(b.x - a.x) * (c.y - b.y);
if (abs(value) < EPS) return 0;
return (value > 0) ? -1 : +1;
}
// Tests whether point 'c' is on the line segment (a, b).
// Ensure first that point c is collinear to segment (a, b) and
// then check whether c is within the rectangle formed by (a, b)
public static boolean pointOnLine(Pt a, Pt b, Pt c) {
return orientation(a, b, c) == 0 &&
min(a.x, b.x) <= c.x && c.x <= max(a.x, b.x) &&
min(a.y, b.y) <= c.y && c.y <= max(a.y, b.y);
}
// Determines whether two segments intersect.
public static boolean segmentsIntersect(Pt p1, Pt p2, Pt p3, Pt p4) {
// Get the orientation of points p3 and p4 in relation
// to the line segment (p1, p2)
int o1 = orientation(p1, p2, p3);
int o2 = orientation(p1, p2, p4);
int o3 = orientation(p3, p4, p1);
int o4 = orientation(p3, p4, p2);
// If the points p1, p2 are on opposite sides of the infinite
// line formed by (p3, p4) and conversly p3, p4 are on opposite
// sides of the infinite line formed by (p1, p2) then there is
// an intersection.
if (o1 != o2 && o3 != o4) return true;
// Collinear special cases (perhaps these if checks can be simplified?)
if (o1 == 0 && pointOnLine(p1, p2, p3)) return true;
if (o2 == 0 && pointOnLine(p1, p2, p4)) return true;
if (o3 == 0 && pointOnLine(p3, p4, p1)) return true;
if (o4 == 0 && pointOnLine(p3, p4, p2)) return true;
return false;
}
public static List<Pt> getCommonEndpoints(Pt p1, Pt p2, Pt p3, Pt p4) {
List<Pt> points = new ArrayList<>();
if (p1.equals(p3)) {
points.add(p1);
if (p2.equals(p4)) points.add(p2);
} else if (p1.equals(p4)) {
points.add(p1);
if (p2.equals(p3)) points.add(p2);
} else if (p2.equals(p3)) {
points.add(p2);
if (p1.equals(p4)) points.add(p1);
} else if (p2.equals(p4)) {
points.add(p2);
if (p1.equals(p3)) points.add(p1);
}
return points;
}
// Finds the intersection point(s) of two line segments. Unlike regular line
// segments, segments which are points (x1 = x2 and y1 = y2) are allowed.
public static Pt[] lineSegmentLineSegmentIntersection(Pt p1, Pt p2, Pt p3, Pt p4) {
// No intersection.
if (!segmentsIntersect(p1, p2, p3, p4)) return new Pt[]{};
// Both segments are a single point.
if (p1.equals(p2) && p2.equals(p3) && p3.equals(p4))
return new Pt[]{p1};
List<Pt> endpoints = getCommonEndpoints(p1, p2, p3, p4);
int n = endpoints.size();
// One of the line segments is an intersecting single point.
// NOTE: checking only n == 1 is insufficient to return early
// because the solution might be a sub segment.
boolean singleton = p1.equals(p2) || p3.equals(p4);
if (n == 1 && singleton) return new Pt[]{endpoints.get(0)};
// Segments are equal.
if (n == 2) return new Pt[]{endpoints.get(0), endpoints.get(1)};
boolean collinearSegments = (orientation(p1, p2, p3) == 0) &&
(orientation(p1, p2, p4) == 0);
// The intersection will be a sub-segment of the two
// segments since they overlap each other.
if (collinearSegments) {
// Segment #2 is enclosed in segment #1
if (pointOnLine(p1, p2, p3) && pointOnLine(p1, p2, p4))
return new Pt[]{p3, p4};
// Segment #1 is enclosed in segment #2
if (pointOnLine(p3, p4, p1) && pointOnLine(p3, p4, p2))
return new Pt[]{p1, p2};
// The subsegment is part of segment #1 and part of segment #2.
// Find the middle points which correspond to this segment.
Pt midPoint1 = pointOnLine(p1, p2, p3) ? p3 : p4;
Pt midPoint2 = pointOnLine(p3, p4, p1) ? p1 : p2;
// There is actually only one middle point!
if (midPoint1.equals(midPoint2)) return new Pt[]{midPoint1};
return new Pt[]{midPoint1, midPoint2};
}
/* Beyond this point there is a unique intersection point. */
// Segment #1 is a vertical line.
if (abs(p1.x - p2.x) < EPS) {
double m = (p4.y - p3.y) / (p4.x - p3.x);
double b = p3.y - m * p3.x;
return new Pt[]{new Pt(p1.x, m * p1.x + b)};
}
// Segment #2 is a vertical line.
if (abs(p3.x - p4.x) < EPS) {
double m = (p2.y - p1.y) / (p2.x - p1.x);
double b = p1.y - m * p1.x;
return new Pt[]{new Pt(p3.x, m * p3.x + b)};
}
double m1 = (p2.y - p1.y) / (p2.x - p1.x);
double m2 = (p4.y - p3.y) / (p4.x - p3.x);
double b1 = p1.y - m1 * p1.x;
double b2 = p3.y - m2 * p3.x;
double x = (b2 - b1) / (m1 - m2);
double y = (m1 * b2 - m2 * b1) / (m1 - m2);
return new Pt[]{new Pt(x, y)};
}
}
Here is a simple usage example:
public static void main(String[] args) {
// Segment #1 is (p1, p2), segment #2 is (p3, p4)
Pt p1, p2, p3, p4;
p1 = new Pt(-2, 4); p2 = new Pt(3, 3);
p3 = new Pt(0, 0); p4 = new Pt(2, 4);
Pt[] points = lineSegmentLineSegmentIntersection(p1, p2, p3, p4);
Pt point = points[0];
// Prints: (1.636, 3.273)
System.out.printf("(%.3f, %.3f)\n", point.x, point.y);
p1 = new Pt(-10, 0); p2 = new Pt(+10, 0);
p3 = new Pt(-5, 0); p4 = new Pt(+5, 0);
points = lineSegmentLineSegmentIntersection(p1, p2, p3, p4);
Pt point1 = points[0], point2 = points[1];
// Prints: (-5.000, 0.000) (5.000, 0.000)
System.out.printf("(%.3f, %.3f) (%.3f, %.3f)\n", point1.x, point1.y, point2.x, point2.y);
}
Just wanted to mention that a good explanation and explicit solution can be found in the Numeric Recipes series. I've got the 3rd edition and the answer is on page 1117, section 21.4. Another solution with a different nomenclature can be found in a paper by Marina Gavrilova Reliable Line Section Intersection Testing. Her solution is, to my mind, a little simpler.
My implementation is below:
bool NuGeometry::IsBetween(const double& x0, const double& x, const double& x1){
return (x >= x0) && (x <= x1);
}
bool NuGeometry::FindIntersection(const double& x0, const double& y0,
const double& x1, const double& y1,
const double& a0, const double& b0,
const double& a1, const double& b1,
double& xy, double& ab) {
// four endpoints are x0, y0 & x1,y1 & a0,b0 & a1,b1
// returned values xy and ab are the fractional distance along xy and ab
// and are only defined when the result is true
bool partial = false;
double denom = (b0 - b1) * (x0 - x1) - (y0 - y1) * (a0 - a1);
if (denom == 0) {
xy = -1;
ab = -1;
} else {
xy = (a0 * (y1 - b1) + a1 * (b0 - y1) + x1 * (b1 - b0)) / denom;
partial = NuGeometry::IsBetween(0, xy, 1);
if (partial) {
// no point calculating this unless xy is between 0 & 1
ab = (y1 * (x0 - a1) + b1 * (x1 - x0) + y0 * (a1 - x1)) / denom;
}
}
if ( partial && NuGeometry::IsBetween(0, ab, 1)) {
ab = 1-ab;
xy = 1-xy;
return true;
} else return false;
}
Plenty of solutions are available above, but I think below solution is pretty simple and easy to understand.
Two segments Vector AB and Vector CD intersect if and only if
The endpoints a and b are on opposite sides of the segment CD.
The endpoints c and d are on opposite side of the segment AB.
More specifically a and b are on opposite side of segment CD if and only if exactly one of the two triples a,c,d and b,c,d is in counterclockwise order.
Intersect(a, b, c, d)
if CCW(a, c, d) == CCW(b, c, d)
return false;
else if CCW(a, b, c) == CCW(a, b, d)
return false;
else
return true;
Here CCW represent counterclockwise which returns true/false based on the orientation of the points.
Source : http://compgeom.cs.uiuc.edu/~jeffe/teaching/373/notes/x06-sweepline.pdf
Page 2
C and Objective-C
Based on Gareth Rees' answer
const AGKLine AGKLineZero = (AGKLine){(CGPoint){0.0, 0.0}, (CGPoint){0.0, 0.0}};
AGKLine AGKLineMake(CGPoint start, CGPoint end)
{
return (AGKLine){start, end};
}
double AGKLineLength(AGKLine l)
{
return CGPointLengthBetween_AGK(l.start, l.end);
}
BOOL AGKLineIntersection(AGKLine l1, AGKLine l2, CGPoint *out_pointOfIntersection)
{
// http://stackoverflow.com/a/565282/202451
CGPoint p = l1.start;
CGPoint q = l2.start;
CGPoint r = CGPointSubtract_AGK(l1.end, l1.start);
CGPoint s = CGPointSubtract_AGK(l2.end, l2.start);
double s_r_crossProduct = CGPointCrossProductZComponent_AGK(r, s);
double t = CGPointCrossProductZComponent_AGK(CGPointSubtract_AGK(q, p), s) / s_r_crossProduct;
double u = CGPointCrossProductZComponent_AGK(CGPointSubtract_AGK(q, p), r) / s_r_crossProduct;
if(t < 0 || t > 1.0 || u < 0 || u > 1.0)
{
if(out_pointOfIntersection != NULL)
{
*out_pointOfIntersection = CGPointZero;
}
return NO;
}
else
{
if(out_pointOfIntersection != NULL)
{
CGPoint i = CGPointAdd_AGK(p, CGPointMultiply_AGK(r, t));
*out_pointOfIntersection = i;
}
return YES;
}
}
CGFloat CGPointCrossProductZComponent_AGK(CGPoint v1, CGPoint v2)
{
return v1.x * v2.y - v1.y * v2.x;
}
CGPoint CGPointSubtract_AGK(CGPoint p1, CGPoint p2)
{
return (CGPoint){p1.x - p2.x, p1.y - p2.y};
}
CGPoint CGPointAdd_AGK(CGPoint p1, CGPoint p2)
{
return (CGPoint){p1.x + p2.x, p1.y + p2.y};
}
CGFloat CGPointCrossProductZComponent_AGK(CGPoint v1, CGPoint v2)
{
return v1.x * v2.y - v1.y * v2.x;
}
CGPoint CGPointMultiply_AGK(CGPoint p1, CGFloat factor)
{
return (CGPoint){p1.x * factor, p1.y * factor};
}
Many of the functions and structs are private, but you should pretty easy be able to know what's going on.
This is public on this repo https://github.com/hfossli/AGGeometryKit/
This is working well for me. Taken from here.
// calculates intersection and checks for parallel lines.
// also checks that the intersection point is actually on
// the line segment p1-p2
Point findIntersection(Point p1,Point p2,
Point p3,Point p4) {
float xD1,yD1,xD2,yD2,xD3,yD3;
float dot,deg,len1,len2;
float segmentLen1,segmentLen2;
float ua,ub,div;
// calculate differences
xD1=p2.x-p1.x;
xD2=p4.x-p3.x;
yD1=p2.y-p1.y;
yD2=p4.y-p3.y;
xD3=p1.x-p3.x;
yD3=p1.y-p3.y;
// calculate the lengths of the two lines
len1=sqrt(xD1*xD1+yD1*yD1);
len2=sqrt(xD2*xD2+yD2*yD2);
// calculate angle between the two lines.
dot=(xD1*xD2+yD1*yD2); // dot product
deg=dot/(len1*len2);
// if abs(angle)==1 then the lines are parallell,
// so no intersection is possible
if(abs(deg)==1) return null;
// find intersection Pt between two lines
Point pt=new Point(0,0);
div=yD2*xD1-xD2*yD1;
ua=(xD2*yD3-yD2*xD3)/div;
ub=(xD1*yD3-yD1*xD3)/div;
pt.x=p1.x+ua*xD1;
pt.y=p1.y+ua*yD1;
// calculate the combined length of the two segments
// between Pt-p1 and Pt-p2
xD1=pt.x-p1.x;
xD2=pt.x-p2.x;
yD1=pt.y-p1.y;
yD2=pt.y-p2.y;
segmentLen1=sqrt(xD1*xD1+yD1*yD1)+sqrt(xD2*xD2+yD2*yD2);
// calculate the combined length of the two segments
// between Pt-p3 and Pt-p4
xD1=pt.x-p3.x;
xD2=pt.x-p4.x;
yD1=pt.y-p3.y;
yD2=pt.y-p4.y;
segmentLen2=sqrt(xD1*xD1+yD1*yD1)+sqrt(xD2*xD2+yD2*yD2);
// if the lengths of both sets of segments are the same as
// the lenghts of the two lines the point is actually
// on the line segment.
// if the point isn’t on the line, return null
if(abs(len1-segmentLen1)>0.01 || abs(len2-segmentLen2)>0.01)
return null;
// return the valid intersection
return pt;
}
class Point{
float x,y;
Point(float x, float y){
this.x = x;
this.y = y;
}
void set(float x, float y){
this.x = x;
this.y = y;
}
}
I tried some of these answers, but they didnt work for me (sorry guys); after some more net searching I found this.
With a little modification to his code I now have this function that will return the point of intersection or if no intersection is found it will return -1,-1.
Public Function intercetion(ByVal ax As Integer, ByVal ay As Integer, ByVal bx As Integer, ByVal by As Integer, ByVal cx As Integer, ByVal cy As Integer, ByVal dx As Integer, ByVal dy As Integer) As Point
'// Determines the intersection point of the line segment defined by points A and B
'// with the line segment defined by points C and D.
'//
'// Returns YES if the intersection point was found, and stores that point in X,Y.
'// Returns NO if there is no determinable intersection point, in which case X,Y will
'// be unmodified.
Dim distAB, theCos, theSin, newX, ABpos As Double
'// Fail if either line segment is zero-length.
If ax = bx And ay = by Or cx = dx And cy = dy Then Return New Point(-1, -1)
'// Fail if the segments share an end-point.
If ax = cx And ay = cy Or bx = cx And by = cy Or ax = dx And ay = dy Or bx = dx And by = dy Then Return New Point(-1, -1)
'// (1) Translate the system so that point A is on the origin.
bx -= ax
by -= ay
cx -= ax
cy -= ay
dx -= ax
dy -= ay
'// Discover the length of segment A-B.
distAB = Math.Sqrt(bx * bx + by * by)
'// (2) Rotate the system so that point B is on the positive X axis.
theCos = bx / distAB
theSin = by / distAB
newX = cx * theCos + cy * theSin
cy = cy * theCos - cx * theSin
cx = newX
newX = dx * theCos + dy * theSin
dy = dy * theCos - dx * theSin
dx = newX
'// Fail if segment C-D doesn't cross line A-B.
If cy < 0 And dy < 0 Or cy >= 0 And dy >= 0 Then Return New Point(-1, -1)
'// (3) Discover the position of the intersection point along line A-B.
ABpos = dx + (cx - dx) * dy / (dy - cy)
'// Fail if segment C-D crosses line A-B outside of segment A-B.
If ABpos < 0 Or ABpos > distAB Then Return New Point(-1, -1)
'// (4) Apply the discovered position to line A-B in the original coordinate system.
'*X=Ax+ABpos*theCos
'*Y=Ay+ABpos*theSin
'// Success.
Return New Point(ax + ABpos * theCos, ay + ABpos * theSin)
End Function
There seems to be some interest in Gavin's answer for which cortijon proposed a javascript version in the comments and iMalc provided a version with slightly fewer computations. Some have pointed out shortcomings with various code proposals and others have commented on the efficiency of some code proposals.
The algorithm provided by iMalc via Gavin's answer is the one that I am currently using in a javascript project and I just wanted to provide a cleaned up version here if it may help anyone.
// Some variables for reuse, others may do this differently
var p0x, p1x, p2x, p3x, ix,
p0y, p1y, p2y, p3y, iy,
collisionDetected;
// do stuff, call other functions, set endpoints...
// note: for my purpose I use |t| < |d| as opposed to
// |t| <= |d| which is equivalent to 0 <= t < 1 rather than
// 0 <= t <= 1 as in Gavin's answer - results may vary
var lineSegmentIntersection = function(){
var d, dx1, dx2, dx3, dy1, dy2, dy3, s, t;
dx1 = p1x - p0x; dy1 = p1y - p0y;
dx2 = p3x - p2x; dy2 = p3y - p2y;
dx3 = p0x - p2x; dy3 = p0y - p2y;
collisionDetected = 0;
d = dx1 * dy2 - dx2 * dy1;
if(d !== 0){
s = dx1 * dy3 - dx3 * dy1;
if((s <= 0 && d < 0 && s >= d) || (s >= 0 && d > 0 && s <= d)){
t = dx2 * dy3 - dx3 * dy2;
if((t <= 0 && d < 0 && t > d) || (t >= 0 && d > 0 && t < d)){
t = t / d;
collisionDetected = 1;
ix = p0x + t * dx1;
iy = p0y + t * dy1;
}
}
}
};
I think there is a much much simpler solution for this problem. I came up with another idea today and it seems to work just fine (at least in 2D for now). All you have to do, is to calculate the intersection between two lines, then check if the calculated intersection point is within the boundig boxes of both line segments. If it is, the line segments intersect. That's it.
EDIT:
This is how I calculate the intersection (I don't know anymore where I found this code snippet)
Point3D
comes from
System.Windows.Media.Media3D
public static Point3D? Intersection(Point3D start1, Point3D end1, Point3D start2, Point3D end2) {
double a1 = end1.Y - start1.Y;
double b1 = start1.X - end1.X;
double c1 = a1 * start1.X + b1 * start1.Y;
double a2 = end2.Y - start2.Y;
double b2 = start2.X - end2.X;
double c2 = a2 * start2.X + b2 * start2.Y;
double det = a1 * b2 - a2 * b1;
if (det == 0) { // lines are parallel
return null;
}
double x = (b2 * c1 - b1 * c2) / det;
double y = (a1 * c2 - a2 * c1) / det;
return new Point3D(x, y, 0.0);
}
and this is my (simplified for the purpose of the answer) BoundingBox class:
public class BoundingBox {
private Point3D min = new Point3D();
private Point3D max = new Point3D();
public BoundingBox(Point3D point) {
min = point;
max = point;
}
public Point3D Min {
get { return min; }
set { min = value; }
}
public Point3D Max {
get { return max; }
set { max = value; }
}
public bool Contains(BoundingBox box) {
bool contains =
min.X <= box.min.X && max.X >= box.max.X &&
min.Y <= box.min.Y && max.Y >= box.max.Y &&
min.Z <= box.min.Z && max.Z >= box.max.Z;
return contains;
}
public bool Contains(Point3D point) {
return Contains(new BoundingBox(point));
}
}
This solution may help
public static float GetLineYIntesept(PointF p, float slope)
{
return p.Y - slope * p.X;
}
public static PointF FindIntersection(PointF line1Start, PointF line1End, PointF line2Start, PointF line2End)
{
float slope1 = (line1End.Y - line1Start.Y) / (line1End.X - line1Start.X);
float slope2 = (line2End.Y - line2Start.Y) / (line2End.X - line2Start.X);
float yinter1 = GetLineYIntesept(line1Start, slope1);
float yinter2 = GetLineYIntesept(line2Start, slope2);
if (slope1 == slope2 && yinter1 != yinter2)
return PointF.Empty;
float x = (yinter2 - yinter1) / (slope1 - slope2);
float y = slope1 * x + yinter1;
return new PointF(x, y);
}
I ported Kris's above answer to JavaScript. After trying numerous different answers, his provided the correct points. I thought I was going crazy that I wasn't getting the points I needed.
function getLineLineCollision(p0, p1, p2, p3) {
var s1, s2;
s1 = {x: p1.x - p0.x, y: p1.y - p0.y};
s2 = {x: p3.x - p2.x, y: p3.y - p2.y};
var s10_x = p1.x - p0.x;
var s10_y = p1.y - p0.y;
var s32_x = p3.x - p2.x;
var s32_y = p3.y - p2.y;
var denom = s10_x * s32_y - s32_x * s10_y;
if(denom == 0) {
return false;
}
var denom_positive = denom > 0;
var s02_x = p0.x - p2.x;
var s02_y = p0.y - p2.y;
var s_numer = s10_x * s02_y - s10_y * s02_x;
if((s_numer < 0) == denom_positive) {
return false;
}
var t_numer = s32_x * s02_y - s32_y * s02_x;
if((t_numer < 0) == denom_positive) {
return false;
}
if((s_numer > denom) == denom_positive || (t_numer > denom) == denom_positive) {
return false;
}
var t = t_numer / denom;
var p = {x: p0.x + (t * s10_x), y: p0.y + (t * s10_y)};
return p;
}
I tried lot of ways and then I decided to write my own. So here it is:
bool IsBetween (float x, float b1, float b2)
{
return ( ((x >= (b1 - 0.1f)) &&
(x <= (b2 + 0.1f))) ||
((x >= (b2 - 0.1f)) &&
(x <= (b1 + 0.1f))));
}
bool IsSegmentsColliding( POINTFLOAT lineA,
POINTFLOAT lineB,
POINTFLOAT line2A,
POINTFLOAT line2B)
{
float deltaX1 = lineB.x - lineA.x;
float deltaX2 = line2B.x - line2A.x;
float deltaY1 = lineB.y - lineA.y;
float deltaY2 = line2B.y - line2A.y;
if (abs(deltaX1) < 0.01f &&
abs(deltaX2) < 0.01f) // Both are vertical lines
return false;
if (abs((deltaY1 / deltaX1) -
(deltaY2 / deltaX2)) < 0.001f) // Two parallel line
return false;
float xCol = ( ( (deltaX1 * deltaX2) *
(line2A.y - lineA.y)) -
(line2A.x * deltaY2 * deltaX1) +
(lineA.x * deltaY1 * deltaX2)) /
((deltaY1 * deltaX2) - (deltaY2 * deltaX1));
float yCol = 0;
if (deltaX1 < 0.01f) // L1 is a vertical line
yCol = ((xCol * deltaY2) +
(line2A.y * deltaX2) -
(line2A.x * deltaY2)) / deltaX2;
else // L1 is acceptable
yCol = ((xCol * deltaY1) +
(lineA.y * deltaX1) -
(lineA.x * deltaY1)) / deltaX1;
bool isCol = IsBetween(xCol, lineA.x, lineB.x) &&
IsBetween(yCol, lineA.y, lineB.y) &&
IsBetween(xCol, line2A.x, line2B.x) &&
IsBetween(yCol, line2A.y, line2B.y);
return isCol;
}
Based on these two formulas: (I simplified them from equation of lines and other formulas)
This based on Gareth Ree's answer. It also returns the overlap of the line segments if they do. Coded in C++, V is a simple vector class. Where the cross product of two vectors in 2D returns a single scalar. It was tested and passed by my schools automatic testing system.
//Required input point must be colinear with the line
bool on_segment(const V& p, const LineSegment& l)
{
//If a point is on the line, the sum of the vectors formed by the point to the line endpoints must be equal
V va = p - l.pa;
V vb = p - l.pb;
R ma = va.magnitude();
R mb = vb.magnitude();
R ml = (l.pb - l.pa).magnitude();
R s = ma + mb;
bool r = s <= ml + epsilon;
return r;
}
//Compute using vector math
// Returns 0 points if the lines do not intersect or overlap
// Returns 1 point if the lines intersect
// Returns 2 points if the lines overlap, contain the points where overlapping start starts and stop
std::vector<V> intersect(const LineSegment& la, const LineSegment& lb)
{
std::vector<V> r;
//http://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect
V oa, ob, da, db; //Origin and direction vectors
R sa, sb; //Scalar values
oa = la.pa;
da = la.pb - la.pa;
ob = lb.pa;
db = lb.pb - lb.pa;
if (da.cross(db) == 0 && (ob - oa).cross(da) == 0) //If colinear
{
if (on_segment(lb.pa, la) && on_segment(lb.pb, la))
{
r.push_back(lb.pa);
r.push_back(lb.pb);
dprintf("colinear, overlapping\n");
return r;
}
if (on_segment(la.pa, lb) && on_segment(la.pb, lb))
{
r.push_back(la.pa);
r.push_back(la.pb);
dprintf("colinear, overlapping\n");
return r;
}
if (on_segment(la.pa, lb))
r.push_back(la.pa);
if (on_segment(la.pb, lb))
r.push_back(la.pb);
if (on_segment(lb.pa, la))
r.push_back(lb.pa);
if (on_segment(lb.pb, la))
r.push_back(lb.pb);
if (r.size() == 0)
dprintf("colinear, non-overlapping\n");
else
dprintf("colinear, overlapping\n");
return r;
}
if (da.cross(db) == 0 && (ob - oa).cross(da) != 0)
{
dprintf("parallel non-intersecting\n");
return r;
}
//Math trick db cross db == 0, which is a single scalar in 2D.
//Crossing both sides with vector db gives:
sa = (ob - oa).cross(db) / da.cross(db);
//Crossing both sides with vector da gives
sb = (oa - ob).cross(da) / db.cross(da);
if (0 <= sa && sa <= 1 && 0 <= sb && sb <= 1)
{
dprintf("intersecting\n");
r.push_back(oa + da * sa);
return r;
}
dprintf("non-intersecting, non-parallel, non-colinear, non-overlapping\n");
return r;
}
Here's a basic implementation of a line segment in C#, with corresponding intersection detection code. It requires a 2D vector/point struct called Vector2f, though you can replace this with any other type that has X/Y properties. You could also replace float with double if that suits your needs better.
This code is used in my .NET physics library, Boing.
public struct LineSegment2f
{
public Vector2f From { get; }
public Vector2f To { get; }
public LineSegment2f(Vector2f #from, Vector2f to)
{
From = #from;
To = to;
}
public Vector2f Delta => new Vector2f(To.X - From.X, To.Y - From.Y);
/// <summary>
/// Attempt to intersect two line segments.
/// </summary>
/// <remarks>
/// Even if the line segments do not intersect, <paramref name="t"/> and <paramref name="u"/> will be set.
/// If the lines are parallel, <paramref name="t"/> and <paramref name="u"/> are set to <see cref="float.NaN"/>.
/// </remarks>
/// <param name="other">The line to attempt intersection of this line with.</param>
/// <param name="intersectionPoint">The point of intersection if within the line segments, or empty..</param>
/// <param name="t">The distance along this line at which intersection would occur, or NaN if lines are collinear/parallel.</param>
/// <param name="u">The distance along the other line at which intersection would occur, or NaN if lines are collinear/parallel.</param>
/// <returns><c>true</c> if the line segments intersect, otherwise <c>false</c>.</returns>
public bool TryIntersect(LineSegment2f other, out Vector2f intersectionPoint, out float t, out float u)
{
var p = From;
var q = other.From;
var r = Delta;
var s = other.Delta;
// t = (q − p) × s / (r × s)
// u = (q − p) × r / (r × s)
var denom = Fake2DCross(r, s);
if (denom == 0)
{
// lines are collinear or parallel
t = float.NaN;
u = float.NaN;
intersectionPoint = default(Vector2f);
return false;
}
var tNumer = Fake2DCross(q - p, s);
var uNumer = Fake2DCross(q - p, r);
t = tNumer / denom;
u = uNumer / denom;
if (t < 0 || t > 1 || u < 0 || u > 1)
{
// line segments do not intersect within their ranges
intersectionPoint = default(Vector2f);
return false;
}
intersectionPoint = p + r * t;
return true;
}
private static float Fake2DCross(Vector2f a, Vector2f b)
{
return a.X * b.Y - a.Y * b.X;
}
}
A C++ program to check if two given line segments intersect
#include <iostream>
using namespace std;
struct Point
{
int x;
int y;
};
// Given three colinear points p, q, r, the function checks if
// point q lies on line segment 'pr'
bool onSegment(Point p, Point q, Point r)
{
if (q.x <= max(p.x, r.x) && q.x >= min(p.x, r.x) &&
q.y <= max(p.y, r.y) && q.y >= min(p.y, r.y))
return true;
return false;
}
// To find orientation of ordered triplet (p, q, r).
// The function returns following values
// 0 --> p, q and r are colinear
// 1 --> Clockwise
// 2 --> Counterclockwise
int orientation(Point p, Point q, Point r)
{
// See 10th slides from following link for derivation of the formula
// http://www.dcs.gla.ac.uk/~pat/52233/slides/Geometry1x1.pdf
int val = (q.y - p.y) * (r.x - q.x) -
(q.x - p.x) * (r.y - q.y);
if (val == 0) return 0; // colinear
return (val > 0)? 1: 2; // clock or counterclock wise
}
// The main function that returns true if line segment 'p1q1'
// and 'p2q2' intersect.
bool doIntersect(Point p1, Point q1, Point p2, Point q2)
{
// Find the four orientations needed for general and
// special cases
int o1 = orientation(p1, q1, p2);
int o2 = orientation(p1, q1, q2);
int o3 = orientation(p2, q2, p1);
int o4 = orientation(p2, q2, q1);
// General case
if (o1 != o2 && o3 != o4)
return true;
// Special Cases
// p1, q1 and p2 are colinear and p2 lies on segment p1q1
if (o1 == 0 && onSegment(p1, p2, q1)) return true;
// p1, q1 and p2 are colinear and q2 lies on segment p1q1
if (o2 == 0 && onSegment(p1, q2, q1)) return true;
// p2, q2 and p1 are colinear and p1 lies on segment p2q2
if (o3 == 0 && onSegment(p2, p1, q2)) return true;
// p2, q2 and q1 are colinear and q1 lies on segment p2q2
if (o4 == 0 && onSegment(p2, q1, q2)) return true;
return false; // Doesn't fall in any of the above cases
}
// Driver program to test above functions
int main()
{
struct Point p1 = {1, 1}, q1 = {10, 1};
struct Point p2 = {1, 2}, q2 = {10, 2};
doIntersect(p1, q1, p2, q2)? cout << "Yes\n": cout << "No\n";
p1 = {10, 0}, q1 = {0, 10};
p2 = {0, 0}, q2 = {10, 10};
doIntersect(p1, q1, p2, q2)? cout << "Yes\n": cout << "No\n";
p1 = {-5, -5}, q1 = {0, 0};
p2 = {1, 1}, q2 = {10, 10};
doIntersect(p1, q1, p2, q2)? cout << "Yes\n": cout << "No\n";
return 0;
}
Based on #Gareth Rees answer, version for Python:
import numpy as np
def np_perp( a ) :
b = np.empty_like(a)
b[0] = a[1]
b[1] = -a[0]
return b
def np_cross_product(a, b):
return np.dot(a, np_perp(b))
def np_seg_intersect(a, b, considerCollinearOverlapAsIntersect = False):
# https://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect/565282#565282
# http://www.codeproject.com/Tips/862988/Find-the-intersection-point-of-two-line-segments
r = a[1] - a[0]
s = b[1] - b[0]
v = b[0] - a[0]
num = np_cross_product(v, r)
denom = np_cross_product(r, s)
# If r x s = 0 and (q - p) x r = 0, then the two lines are collinear.
if np.isclose(denom, 0) and np.isclose(num, 0):
# 1. If either 0 <= (q - p) * r <= r * r or 0 <= (p - q) * s <= * s
# then the two lines are overlapping,
if(considerCollinearOverlapAsIntersect):
vDotR = np.dot(v, r)
aDotS = np.dot(-v, s)
if (0 <= vDotR and vDotR <= np.dot(r,r)) or (0 <= aDotS and aDotS <= np.dot(s,s)):
return True
# 2. If neither 0 <= (q - p) * r = r * r nor 0 <= (p - q) * s <= s * s
# then the two lines are collinear but disjoint.
# No need to implement this expression, as it follows from the expression above.
return None
if np.isclose(denom, 0) and not np.isclose(num, 0):
# Parallel and non intersecting
return None
u = num / denom
t = np_cross_product(v, s) / denom
if u >= 0 and u <= 1 and t >= 0 and t <= 1:
res = b[0] + (s*u)
return res
# Otherwise, the two line segments are not parallel but do not intersect.
return None
If each side of the rectangle is a line segment, and the user drawn portion is a line segment, then you need to just check the user drawn segment for intersection with the four side line segments. This should be a fairly simple exercise given the start and end points of each segment.
Based on t3chb0t's answer:
int intersezione_linee(int x1, int y1, int x2, int y2, int x3, int y3, int x4, int y4, int& p_x, int& p_y)
{
//L1: estremi (x1,y1)(x2,y2) L2: estremi (x3,y3)(x3,y3)
int d;
d = (x1-x2)*(y3-y4) - (y1-y2)*(x3-x4);
if(!d)
return 0;
p_x = ((x1*y2-y1*x2)*(x3-x4) - (x1-x2)*(x3*y4-y3*x4))/d;
p_y = ((x1*y2-y1*x2)*(y3-y4) - (y1-y2)*(x3*y4-y3*x4))/d;
return 1;
}
int in_bounding_box(int x1, int y1, int x2, int y2, int p_x, int p_y)
{
return p_x>=x1 && p_x<=x2 && p_y>=y1 && p_y<=y2;
}
int intersezione_segmenti(int x1, int y1, int x2, int y2, int x3, int y3, int x4, int y4, int& p_x, int& p_y)
{
if (!intersezione_linee(x1,y1,x2,y2,x3,y3,x4,y4,p_x,p_y))
return 0;
return in_bounding_box(x1,y1,x2,y2,p_x,p_y) && in_bounding_box(x3,y3,x4,y4,p_x,p_y);
}
I read these algorithm from the book "multiple view geometry"
following text using
' as transpose sign
* as dot product
x as cross product, when using as operator
1. line definition
a point x_vec = (x, y)' lies on the line ax + by + c = 0
we denote L = (a, b, c)', the point as (x, y, 1)' as homogeneous coordinates
the line equation can be written as
(x, y, 1)(a, b, c)' = 0 or x' * L = 0
2. intersection of lines
we have two lines L1=(a1, b1, c1)', L2=(a2, b2, c2)'
assume x is a point, a vector, and x = L1 x L2 (L1 cross product L2).
be careful, x is always a 2D point, please read homogeneous coordinates if you are confused about (L1xL2) is a three elements vector, and x is a 2D coordinates.
according to triple product, we know that
L1 * ( L1 x L2 ) = 0, and L2 * (L1 x L2) = 0, because of L1,L2 co-plane
we substitute (L1xL2) with vector x, then we have L1*x=0, L2*x=0, which means x lie on both L1 and L2, x is the intersection point.
be careful, here x is homogeneous coordinates, if the last element of x is zero, it means L1 and L2 are parallel.
Many answers have wrapped up all the calculations into a single function. If you need to calculate the line slopes, y-intercepts, or x-intercepts for use elsewhere in your code, you'll be making those calculations redundantly. I have separated out the respective functions, used obvious variable names, and commented my code to make it easier to follow. I needed to know if lines intersect infinitely beyond their endpoints, so in JavaScript:
http://jsfiddle.net/skibulk/evmqq00u/
var point_a = {x:0, y:10},
point_b = {x:12, y:12},
point_c = {x:10, y:0},
point_d = {x:0, y:0},
slope_ab = slope(point_a, point_b),
slope_bc = slope(point_b, point_c),
slope_cd = slope(point_c, point_d),
slope_da = slope(point_d, point_a),
yint_ab = y_intercept(point_a, slope_ab),
yint_bc = y_intercept(point_b, slope_bc),
yint_cd = y_intercept(point_c, slope_cd),
yint_da = y_intercept(point_d, slope_da),
xint_ab = x_intercept(point_a, slope_ab, yint_ab),
xint_bc = x_intercept(point_b, slope_bc, yint_bc),
xint_cd = x_intercept(point_c, slope_cd, yint_cd),
xint_da = x_intercept(point_d, slope_da, yint_da),
point_aa = intersect(slope_da, yint_da, xint_da, slope_ab, yint_ab, xint_ab),
point_bb = intersect(slope_ab, yint_ab, xint_ab, slope_bc, yint_bc, xint_bc),
point_cc = intersect(slope_bc, yint_bc, xint_bc, slope_cd, yint_cd, xint_cd),
point_dd = intersect(slope_cd, yint_cd, xint_cd, slope_da, yint_da, xint_da);
console.log(point_a, point_b, point_c, point_d);
console.log(slope_ab, slope_bc, slope_cd, slope_da);
console.log(yint_ab, yint_bc, yint_cd, yint_da);
console.log(xint_ab, xint_bc, xint_cd, xint_da);
console.log(point_aa, point_bb, point_cc, point_dd);
function slope(point_a, point_b) {
var i = (point_b.y - point_a.y) / (point_b.x - point_a.x);
if (i === -Infinity) return Infinity;
if (i === -0) return 0;
return i;
}
function y_intercept(point, slope) {
// Horizontal Line
if (slope == 0) return point.y;
// Vertical Line
if (slope == Infinity)
{
// THE Y-Axis
if (point.x == 0) return Infinity;
// No Intercept
return null;
}
// Angled Line
return point.y - (slope * point.x);
}
function x_intercept(point, slope, yint) {
// Vertical Line
if (slope == Infinity) return point.x;
// Horizontal Line
if (slope == 0)
{
// THE X-Axis
if (point.y == 0) return Infinity;
// No Intercept
return null;
}
// Angled Line
return -yint / slope;
}
// Intersection of two infinite lines
function intersect(slope_a, yint_a, xint_a, slope_b, yint_b, xint_b) {
if (slope_a == slope_b)
{
// Equal Lines
if (yint_a == yint_b && xint_a == xint_b) return Infinity;
// Parallel Lines
return null;
}
// First Line Vertical
if (slope_a == Infinity)
{
return {
x: xint_a,
y: (slope_b * xint_a) + yint_b
};
}
// Second Line Vertical
if (slope_b == Infinity)
{
return {
x: xint_b,
y: (slope_a * xint_b) + yint_a
};
}
// Not Equal, Not Parallel, Not Vertical
var i = (yint_b - yint_a) / (slope_a - slope_b);
return {
x: i,
y: (slope_a * i) + yint_a
};
}

WGS84 Geoid Height Altitude Offset for external GPS data on IOS

For an application I'm writing we are interfacing IOS devices with an external sensor which outputs GPS data over a local wifi network. This data comes across in a "raw" format with respect to altitude. In general all GPS altitude needs to have a correction factor applied related to the WGS84 geoid height based on the current location.
For example, in the following Geo Control Point (http://www.ngs.noaa.gov/cgi-bin/ds_mark.prl?PidBox=HV9830) which resides at Lat 38 56 36.77159 and a Lon 077 01 08.34929
HV9830* NAD 83(2011) POSITION- 38 56 36.77159(N) 077 01 08.34929(W) ADJUSTED
HV9830* NAD 83(2011) ELLIP HT- 42.624 (meters) (06/27/12) ADJUSTED
HV9830* NAD 83(2011) EPOCH - 2010.00
HV9830* NAVD 88 ORTHO HEIGHT - 74.7 (meters) 245. (feet) VERTCON
HV9830 ______________________________________________________________________
HV9830 GEOID HEIGHT - -32.02 (meters) GEOID12A
HV9830 NAD 83(2011) X - 1,115,795.966 (meters) COMP
HV9830 NAD 83(2011) Y - -4,840,360.447 (meters) COMP
HV9830 NAD 83(2011) Z - 3,987,471.457 (meters) COMP
HV9830 LAPLACE CORR - -2.38 (seconds) DEFLEC12A
You can see that the Geoid Height is -32 meters. So given a RAW GPS reading near this point one would have to apply a correction of -32 meters in order to calculate the correct altitude. (Note:corrections are negative so you would actually be subtracting a negative and thus shifting the reading up 32 meters).
As opposed to Android it is our understanding that with regards to coreLocation this GeoidHeight information is automagically calculated internally by IOS. Where we are running into difficulty is that we are using a local wifi network with a sensor that calculates uncorrected GPS and collecting both the external sensor data as well as coreLocation readings for GPS. I was wondering if anybody was aware of a library (C/Objective-C) which has the Geoid information and can help me do these calculations on the fly when I'm reading the raw GPS signal from our sensor package.
Thank you for your help.
Side note: Please don't suggest I look at the following post: Get altitude by longitude and latitude in Android This si a good solution however we do not have a live internet connection so we cannot make a live query to Goole or USGS.
I've gone ahead and solved my problems here. What I did was create an ObjectiveC implementation of a c implementation of fortran code to do what I needed. The original c can be found here: http://sourceforge.net/projects/egm96-f477-c/
You would need to download the project from source forge in order to access the input files required for this code: CORCOEF and EGM96
My objective-c implementation is as follows:
GeoidCalculator.h
#import <Foundation/Foundation.h>
#interface GeoidCalculator : NSObject
+ (GeoidCalculator *)instance;
-(double) getHeightFromLat:(double)lat andLon:(double)lon;
-(double) getCurrentHeightOffset;
-(void) updatePositionWithLatitude:(double)lat andLongitude:(double)lon;
#end
GeoidCalculator.m
#import "GeoidCalculator.h"
#import <stdio.h>
#import <math.h>
#define l_value (65341)
#define _361 (361)
#implementation GeoidCalculator
static int nmax;
static double currentHeight;
static double cc[l_value+ 1], cs[l_value+ 1], hc[l_value+ 1], hs[l_value+ 1],
p[l_value+ 1], sinml[_361+ 1], cosml[_361+ 1], rleg[_361+ 1];
+ (GeoidCalculator *)instance {
static GeoidCalculator *_instance = nil;
#synchronized (self) {
if (_instance == nil) {
_instance = [[self alloc] init];
init_arrays();
currentHeight = -9999;
}
}
return _instance;
}
- (double)getHeightFromLat:(double)lat andLon:(double)lon {
[self updatePositionWithLatitude:lat andLongitude:lon];
return [self getCurrentHeightOffset];
}
- (double)getCurrentHeightOffset {
return currentHeight;
}
- (void)updatePositionWithLatitude:(double)lat andLongitude:(double)lon {
const double rad = 180 / M_PI;
double flat, flon, u;
flat = lat; flon = lon;
/*compute the geocentric latitude,geocentric radius,normal gravity*/
u = undulation(flat / rad, flon / rad, nmax, nmax + 1);
/*u is the geoid undulation from the egm96 potential coefficient model
including the height anomaly to geoid undulation correction term
and a correction term to have the undulations refer to the
wgs84 ellipsoid. the geoid undulation unit is meters.*/
currentHeight = u;
}
double hundu(unsigned nmax, double p[l_value+ 1],
double hc[l_value+ 1], double hs[l_value+ 1],
double sinml[_361+ 1], double cosml[_361+ 1], double gr, double re,
double cc[l_value+ 1], double cs[l_value+ 1]) {/*constants for wgs84(g873);gm in units of m**3/s**2*/
const double gm = .3986004418e15, ae = 6378137.;
double arn, ar, ac, a, b, sum, sumc, sum2, tempc, temp;
int k, n, m;
ar = ae / re;
arn = ar;
ac = a = b = 0;
k = 3;
for (n = 2; n <= nmax; n++) {
arn *= ar;
k++;
sum = p[k] * hc[k];
sumc = p[k] * cc[k];
sum2 = 0;
for (m = 1; m <= n; m++) {
k++;
tempc = cc[k] * cosml[m] + cs[k] * sinml[m];
temp = hc[k] * cosml[m] + hs[k] * sinml[m];
sumc += p[k] * tempc;
sum += p[k] * temp;
}
ac += sumc;
a += sum * arn;
}
ac += cc[1] + p[2] * cc[2] + p[3] * (cc[3] * cosml[1] + cs[3] * sinml[1]);
/*add haco=ac/100 to convert height anomaly on the ellipsoid to the undulation
add -0.53m to make undulation refer to the wgs84 ellipsoid.*/
return a * gm / (gr * re) + ac / 100 - .53;
}
void dscml(double rlon, unsigned nmax, double sinml[_361+ 1], double cosml[_361+ 1]) {
double a, b;
int m;
a = sin(rlon);
b = cos(rlon);
sinml[1] = a;
cosml[1] = b;
sinml[2] = 2 * b * a;
cosml[2] = 2 * b * b - 1;
for (m = 3; m <= nmax; m++) {
sinml[m] = 2 * b * sinml[m - 1] - sinml[m - 2];
cosml[m] = 2 * b * cosml[m - 1] - cosml[m - 2];
}
}
void dhcsin(unsigned nmax, double hc[l_value+ 1], double hs[l_value+ 1]) {
// potential coefficient file
//f_12 = fopen("EGM96", "rb");
NSString* path2 = [[NSBundle mainBundle] pathForResource:#"EGM96" ofType:#""];
FILE* f_12 = fopen(path2.UTF8String, "rb");
if (f_12 == NULL) {
NSLog([path2 stringByAppendingString:#" not found"]);
}
int n, m;
double j2, j4, j6, j8, j10, c, s, ec, es;
/*the even degree zonal coefficients given below were computed for the
wgs84(g873) system of constants and are identical to those values
used in the NIMA gridding procedure. computed using subroutine
grs written by N.K. PAVLIS*/
j2 = 0.108262982131e-2;
j4 = -.237091120053e-05;
j6 = 0.608346498882e-8;
j8 = -0.142681087920e-10;
j10 = 0.121439275882e-13;
m = ((nmax + 1) * (nmax + 2)) / 2;
for (n = 1; n <= m; n++)hc[n] = hs[n] = 0;
while (6 == fscanf(f_12, "%i %i %lf %lf %lf %lf", &n, &m, &c, &s, &ec, &es)) {
if (n > nmax)continue;
n = (n * (n + 1)) / 2 + m + 1;
hc[n] = c;
hs[n] = s;
}
hc[4] += j2 / sqrt(5);
hc[11] += j4 / 3;
hc[22] += j6 / sqrt(13);
hc[37] += j8 / sqrt(17);
hc[56] += j10 / sqrt(21);
fclose(f_12);
}
void legfdn(unsigned m, double theta, double rleg[_361+ 1], unsigned nmx)
/*this subroutine computes all normalized legendre function
in "rleg". order is always
m, and colatitude is always theta (radians). maximum deg
is nmx. all calculations in double precision.
ir must be set to zero before the first call to this sub.
the dimensions of arrays rleg must be at least equal to nmx+1.
Original programmer :Oscar L. Colombo, Dept. of Geodetic Science
the Ohio State University, August 1980
ineiev: I removed the derivatives, for they are never computed here*/
{
static double drts[1301], dirt[1301], cothet, sithet, rlnn[_361+ 1];
static int ir;
int nmx1 = nmx + 1, nmx2p = 2 * nmx + 1, m1 = m + 1, m2 = m + 2, m3 = m + 3, n, n1, n2;
if (!ir) {
ir = 1;
for (n = 1; n <= nmx2p; n++) {
drts[n] = sqrt(n);
dirt[n] = 1 / drts[n];
}
}
cothet = cos(theta);
sithet = sin(theta);
/*compute the legendre functions*/
rlnn[1] = 1;
rlnn[2] = sithet * drts[3];
for (n1 = 3; n1 <= m1; n1++) {
n = n1 - 1;
n2 = 2 * n;
rlnn[n1] = drts[n2 + 1] * dirt[n2] * sithet * rlnn[n];
}
switch (m) {
case 1:
rleg[2] = rlnn[2];
rleg[3] = drts[5] * cothet * rleg[2];
break;
case 0:
rleg[1] = 1;
rleg[2] = cothet * drts[3];
break;
}
rleg[m1] = rlnn[m1];
if (m2 <= nmx1) {
rleg[m2] = drts[m1 * 2 + 1] * cothet * rleg[m1];
if (m3 <= nmx1)
for (n1 = m3; n1 <= nmx1; n1++) {
n = n1 - 1;
if ((!m && n < 2) || (m == 1 && n < 3))continue;
n2 = 2 * n;
rleg[n1] = drts[n2 + 1] * dirt[n + m] * dirt[n - m] *
(drts[n2 - 1] * cothet * rleg[n1 - 1] - drts[n + m - 1] * drts[n - m - 1] * dirt[n2 - 3] * rleg[n1 - 2]);
}
}
}
void radgra(double lat, double lon, double *rlat, double *gr, double *re)
/*this subroutine computes geocentric distance to the point,
the geocentric latitude,and
an approximate value of normal gravity at the point based
the constants of the wgs84(g873) system are used*/
{
const double a = 6378137., e2 = .00669437999013, geqt = 9.7803253359, k = .00193185265246;
double n, t1 = sin(lat) * sin(lat), t2, x, y, z;
n = a / sqrt(1 - e2 * t1);
t2 = n * cos(lat);
x = t2 * cos(lon);
y = t2 * sin(lon);
z = (n * (1 - e2)) * sin(lat);
*re = sqrt(x * x + y * y + z * z);/*compute the geocentric radius*/
*rlat = atan(z / sqrt(x * x + y * y));/*compute the geocentric latitude*/
*gr = geqt * (1 + k * t1) / sqrt(1 - e2 * t1);/*compute normal gravity:units are m/sec**2*/
}
double undulation(double lat, double lon, int nmax, int k) {
double rlat, gr, re;
int i, j, m;
radgra(lat, lon, &rlat, &gr, &re);
rlat = M_PI / 2 - rlat;
for (j = 1; j <= k; j++) {
m = j - 1;
legfdn(m, rlat, rleg, nmax);
for (i = j; i <= k; i++)p[(i - 1) * i / 2 + m + 1] = rleg[i];
}
dscml(lon, nmax, sinml, cosml);
return hundu(nmax, p, hc, hs, sinml, cosml, gr, re, cc, cs);
}
void init_arrays(void) {
int ig, i, n, m;
double t1, t2;
NSString* path1 = [[NSBundle mainBundle] pathForResource:#"CORCOEF" ofType:#""];
//correction coefficient file: modified with 'sed -e"s/D/e/g"' to be read with fscanf
FILE* f_1 = fopen([path1 cStringUsingEncoding:1], "rb");
if (f_1 == NULL) {
NSLog([path1 stringByAppendingString:#" not found"]);
}
nmax = 360;
for (i = 1; i <= l_value; i++)cc[i] = cs[i] = 0;
while (4 == fscanf(f_1, "%i %i %lg %lg", &n, &m, &t1, &t2)) {
ig = (n * (n + 1)) / 2 + m + 1;
cc[ig] = t1;
cs[ig] = t2;
}
/*the correction coefficients are now read in*/
/*the potential coefficients are now read in and the reference
even degree zonal harmonic coefficients removed to degree 6*/
dhcsin(nmax, hc, hs);
fclose(f_1);
}
#end
I've done some limited testing against the Geoid Height Calculator (http://www.unavco.org/community_science/science-support/geoid/geoid.html) and looks like everything is a match
UPDATE iOS8 or Greater
As of IOS8 This code might not work correctly. You may need to change how the bundle is loaded:
[[NSBundle mainBundle] pathForResource:#"EGM96" ofType:#""];
Do some googling or add a comment here.
Impressive stuff Jeef! I just used your code to create this sqlite which may be easier to add/use in a project, assuming integer precision for lat/lon is good enough:
https://github.com/vectorstofinal/geoid_heights
You could use GeoTrans.
Provided by http://earth-info.nga.mil/GandG/geotrans/index.html
The keyword is "vertical datum". So you want to convert from WGS84 to e.g EGM96 vertical datum. Make sure which Geoid modell you want to use. EGM96 is one of that.
Maybe these answer help you, too:
How to calculate the altitude above from mean sea level
Next read the ios Open Source License Text: Available in
Settings -> General -> About -> Legal -> License ...
There you get a list of all libs that ios uses. One of them I found was the calculation of magnetic decilination usung a sw of USGS. Chances are verry high that the Geoid height calculation is listed there too.

Bilateration with iBeacons

I am trying to use multiple iBeacons to track the user's location in iOS. I know this can be done (somewhat) using 3 beacons and trilateration, but I would like to do it with two (bilateration). I know that I will likely end up with two answers. Does anyone know of a simple way to accomplish this, given the (x,y) location of the beacons (relative to the room), and an averaged RSSI from each beacon?
I have this code for trilateration that I altered to objective-c from javascript:
- (CGPoint)getTrilaterationWithBeacon1:(BBBeacon *)beacon1 Beacon2:(BBBeacon *)beacon2 Beacon3:(BBBeacon *)beacon3 {
float xa = beacon1.x;
float ya = beacon1.y;
float xb = beacon2.x;
float yb = beacon2.y;
float xc = beacon3.x;
float yc = beacon3.y;
float ra = beacon1.distance;
float rb = beacon2.distance;
float rc = beacon3.distance;
float S = (pow(xc, 2.) - pow(xb, 2.) + pow(yc, 2.) - pow(yb, 2.) + pow(rb, 2.) - pow(rc, 2.)) / 2.0;
float T = (pow(xa, 2.) - pow(xb, 2.) + pow(ya, 2.) - pow(yb, 2.) + pow(rb, 2.) - pow(ra, 2.)) / 2.0;
float y = ((T * (xb - xc)) - (S * (xb - xa))) / (((ya - yb) * (xb - xc)) - ((yc - yb) * (xb - xa)));
float x = ((y * (ya - yb)) - T) / (xb - xa);
CGPoint point = CGPointMake(x, y);
return point;
}
So this is the code I ended up using, thanks to ChuckCottrill's suggestion that I look for a formula to calculate intersection of two circles. It is modified from a C version I found online here: http://paulbourke.net/geometry/circlesphere/tvoght.c
The results are somewhat inconsistent due to the inconsistency of the RSSI values returned from the iBeacons.
I will still need to add code to select the correct point somehow (it gives two results).
- (CGPoint)getBilaterationWithBeacon1:(BBBeacon *)beacon1 Beacon2:(BBBeacon *)beacon2 {
float x0 = beacon1.locationX;
float y0 = beacon1.locationY;
float r0 = beacon1.filteredDistance;
float x1 = beacon2.locationX;
float y1 = beacon2.locationY;
float r1 = beacon2.filteredDistance;
float a, dx, dy, d, h, rx, ry;
float x2, y2;
/* dx and dy are the vertical and horizontal distances between
* the circle centers.
*/
dx = x1 - x0;
dy = y1 - y0;
/* Determine the straight-line distance between the centers. */
d = sqrt((dy*dy) + (dx*dx));
/* Check for solvability. */
if (d > (r0 + r1)) {
/* no solution. circles do not intersect. */
return CGPointMake(-1, -1);
}
if (d < abs(r0 - r1)) {
/* no solution. one circle is contained in the other */
return CGPointMake(-1, -1);
}
/* 'point 2' is the point where the line through the circle
* intersection points crosses the line between the circle
* centers.
*/
/* Determine the distance from point 0 to point 2. */
a = ((r0*r0) - (r1*r1) + (d*d)) / (2.0 * d) ;
/* Determine the coordinates of point 2. */
x2 = x0 + (dx * a/d);
y2 = y0 + (dy * a/d);
/* Determine the distance from point 2 to either of the
* intersection points.
*/
h = sqrt((r0*r0) - (a*a));
/* Now determine the offsets of the intersection points from
* point 2.
*/
rx = -dy * (h/d);
ry = dx * (h/d);
/* Determine the absolute intersection points. */
float xi = x2 + rx;
float xi_prime = x2 - rx;
float yi = y2 + ry;
float yi_prime = y2 - ry;
CGPoint point1 = CGPointMake(xi, yi);
CGPoint point2 = CGPointMake(xi_prime, yi_prime);
//pick one
return point2;
}

iOS Core Data To Query Against Geospatial Data Set

I'm using the Core Data Framework to manage a set of accounts which also include geospatial (GPS) coordinate data for each account. How can I query against this data based on position of the device to get a list of accounts within x feet and list them in order of distance?
to get you started, here's a method i use in my iOS app that returns the distance in meters between two CLLocationCoordinate2D locations, assuming the Google Spherical Mercator Projection (if you want to use another projection, you can specify the appropriate flattening ratio value (f) and semi-major axis value (a). if you want the forward and backward azimuth values between the coordinates, you can uncomment and return the faz and baz values along with the distance by defining your own struct. this method can be used to add the distance to each of your 'account' objects and the current location being reported by your CLLocationManager object, then you could easily sort and filter an array of account objects based on their distances.
based on code by Gerald Evenden located here: http://article.gmane.org/gmane.comp.gis.proj-4.devel/3478
#define PI 3.141592653589793238462643
#define EPS 5e-14
#define DEG_TO_RAD 0.0174532925199432958
// returns the geodesic distance in meters between two coordinates based on the google spherical mercator projection.
- (int) geodesicDistanceFromCoordinate: (CLLocationCoordinate2D) fromCoord toCoordinate: (CLLocationCoordinate2D) toCoord {
double c, d, e, r, x, y, sa, cx, cy, cz, sx, sy, c2a, cu1, cu2, su1, tu1, tu2, ts, phi1, lam1, phi2, lam2, f, baz, faz, s, a;
phi1 = fromCoord.latitude * DEG_TO_RAD;
lam1 = fromCoord.longitude * DEG_TO_RAD;
phi2 = toCoord.latitude * DEG_TO_RAD;
lam2 = toCoord.longitude * DEG_TO_RAD;
f = 0; //google's spherical mercator projection has no flattening
a = 6378137; //earth's axis in meters used in google's projection
r = 1. - f;
tu1 = r * tan(phi1);
tu2 = r * tan(phi2);
cu1 = 1. / sqrt(tu1 * tu1 + 1.);
su1 = cu1 * tu1;
cu2 = 1. / sqrt(tu2 * tu2 + 1.);
ts = cu1 * cu2;
baz = ts * tu2;
faz = baz * tu1;
x = lam2 - lam1;
do {
sx = sin(x);
cx = cos(x);
tu1 = cu2 * sx;
tu2 = baz - su1 * cu2 * cx;
sy = sqrt(tu1 * tu1 + tu2 * tu2);
cy = ts * cx + faz;
y = atan2(sy, cy);
sa = ts * sx / sy;
c2a = -sa * sa + 1.;
cz = faz + faz;
if (c2a > 0.)
cz = -cz / c2a + cy;
e = cz * cz * 2. - 1.;
c = ((c2a * -3. + 4.) * f + 4.) * c2a * f / 16.;
d = x;
x = ((e * cy * c + cz) * sy * c + y) * sa;
x = (1. - c) * x * f + lam2 - lam1;
} while (fabs(d - x) > EPS);
//forward azimuth faz = atan2(tu1, tu2);
//backward azimuth baz = atan2(cu1 * sx, baz * cx - su1 * cu2) + PI;
x = sqrt((1. / r / r - 1.) * c2a + 1.) + 1.;
x = (x - 2.) / x;
c = (x * x / 4. + 1.) / (1. - x);
d = (x * .375 * x - 1.) * x;
s = ((((sy * sy * 4. - 3.) * (1. - e - e) * cz * d / 6. - e * cy) * d / 4. + cz) * sy * d + y) * c * r;
return (int)(s * a);
}

Resources