I have a line from A to B and a circle positioned at C with the radius R.
What is a good algorithm to use to check whether the line intersects the circle? And at what coordinate along the circles edge it occurred?
Taking
E is the starting point of the ray,
L is the end point of the ray,
C is the center of sphere you're testing against
r is the radius of that sphere
Compute:
d = L - E ( Direction vector of ray, from start to end )
f = E - C ( Vector from center sphere to ray start )
Then the intersection is found by..
Plugging:
P = E + t * d
This is a parametric equation:
Px = Ex + tdx
Py = Ey + tdy
into
(x - h)2 + (y - k)2 = r2
(h,k) = center of circle.
Note: We've simplified the problem to 2D here, the solution we get applies also in 3D
to get:
Expand
x2 - 2xh + h2 + y2 - 2yk + k2 - r2 = 0
Plug
x = ex + tdx
y = ey + tdy
( ex + tdx )2 - 2( ex + tdx )h + h2 +
( ey + tdy )2 - 2( ey + tdy )k + k2 - r2 = 0
Explode
ex2 + 2extdx + t2dx2 - 2exh - 2tdxh + h2 +
ey2 + 2eytdy + t2dy2 - 2eyk - 2tdyk + k2 - r2 = 0
Group
t2( dx2 + dy2 ) +
2t( exdx + eydy - dxh - dyk ) +
ex2 + ey2 -
2exh - 2eyk + h2 + k2 - r2 = 0
Finally,
t2( d · d ) + 2t( e · d - d · c ) + e · e - 2( e · c ) + c · c - r2 = 0
Where d is the vector d and · is the dot product.
And then,
t2( d · d ) + 2t( d · ( e - c ) ) + ( e - c ) · ( e - c ) - r2 = 0
Letting f = e - c
t2( d · d ) + 2t( d · f ) + f · f - r2 = 0
So we get:
t2 * (d · d) + 2t*( f · d ) + ( f · f - r2 ) = 0
So solving the quadratic equation:
float a = d.Dot( d ) ;
float b = 2*f.Dot( d ) ;
float c = f.Dot( f ) - r*r ;
float discriminant = b*b-4*a*c;
if( discriminant < 0 )
{
// no intersection
}
else
{
// ray didn't totally miss sphere,
// so there is a solution to
// the equation.
discriminant = sqrt( discriminant );
// either solution may be on or off the ray so need to test both
// t1 is always the smaller value, because BOTH discriminant and
// a are nonnegative.
float t1 = (-b - discriminant)/(2*a);
float t2 = (-b + discriminant)/(2*a);
// 3x HIT cases:
// -o-> --|--> | | --|->
// Impale(t1 hit,t2 hit), Poke(t1 hit,t2>1), ExitWound(t1<0, t2 hit),
// 3x MISS cases:
// -> o o -> | -> |
// FallShort (t1>1,t2>1), Past (t1<0,t2<0), CompletelyInside(t1<0, t2>1)
if( t1 >= 0 && t1 <= 1 )
{
// t1 is the intersection, and it's closer than t2
// (since t1 uses -b - discriminant)
// Impale, Poke
return true ;
}
// here t1 didn't intersect so we are either started
// inside the sphere or completely past it
if( t2 >= 0 && t2 <= 1 )
{
// ExitWound
return true ;
}
// no intn: FallShort, Past, CompletelyInside
return false ;
}
No one seems to consider projection, am I completely off track here?
Project the vector AC onto AB. The projected vector, AD, gives the new point D.
If the distance between D and C is smaller than (or equal to) R we have an intersection.
Like this:
Community Edit:
For anyone stumbling across this post later and wondering how such an algorithm can be implemented, here is a general implementation written in JavaScript using common vector manipulation functions.
/**
* Returns the distance from line segment AB to point C
*/
function distanceSegmentToPoint(A, B, C) {
// Compute vectors AC and AB
const AC = sub(C, A);
const AB = sub(B, A);
// Get point D by taking the projection of AC onto AB then adding the offset of A
const D = add(proj(AC, AB), A);
const AD = sub(D, A);
// D might not be on AB so calculate k of D down AB (aka solve AD = k * AB)
// We can use either component, but choose larger value to reduce the chance of dividing by zero
const k = Math.abs(AB.x) > Math.abs(AB.y) ? AD.x / AB.x : AD.y / AB.y;
// Check if D is off either end of the line segment
if (k <= 0.0) {
return Math.sqrt(hypot2(C, A));
} else if (k >= 1.0) {
return Math.sqrt(hypot2(C, B));
}
return Math.sqrt(hypot2(C, D));
}
For this implementation I used a couple common vector manipulation functions that you are likely to already have provided in whatever environment you might be working in. However if you do not already have these functions available, here is how they can be implemented.
// Define some common functions for working with vectors
const add = (a, b) => ({x: a.x + b.x, y: a.y + b.y});
const sub = (a, b) => ({x: a.x - b.x, y: a.y - b.y});
const dot = (a, b) => a.x * b.x + a.y * b.y;
const hypot2 = (a, b) => dot(sub(a, b), sub(a, b));
// Function for projecting some vector a onto b
function proj(a, b) {
const k = dot(a, b) / dot(b, b);
return {x: k * b.x, y: k * b.y};
}
I would use the algorithm to compute the distance between a point (circle center) and a line (line AB). This can then be used to determine the intersection points of the line with the circle.
Let say we have the points A, B, C. Ax and Ay are the x and y components of the A points. Same for B and C. The scalar R is the circle radius.
This algorithm requires that A, B and C are distinct points and that R is not 0.
Here is the algorithm
// compute the euclidean distance between A and B
LAB = sqrt( (Bx-Ax)²+(By-Ay)² )
// compute the direction vector D from A to B
Dx = (Bx-Ax)/LAB
Dy = (By-Ay)/LAB
// the equation of the line AB is x = Dx*t + Ax, y = Dy*t + Ay with 0 <= t <= LAB.
// compute the distance between the points A and E, where
// E is the point of AB closest the circle center (Cx, Cy)
t = Dx*(Cx-Ax) + Dy*(Cy-Ay)
// compute the coordinates of the point E
Ex = t*Dx+Ax
Ey = t*Dy+Ay
// compute the euclidean distance between E and C
LEC = sqrt((Ex-Cx)²+(Ey-Cy)²)
// test if the line intersects the circle
if( LEC < R )
{
// compute distance from t to circle intersection point
dt = sqrt( R² - LEC²)
// compute first intersection point
Fx = (t-dt)*Dx + Ax
Fy = (t-dt)*Dy + Ay
// compute second intersection point
Gx = (t+dt)*Dx + Ax
Gy = (t+dt)*Dy + Ay
}
// else test if the line is tangent to circle
else if( LEC == R )
// tangent point to circle is E
else
// line doesn't touch circle
Okay, I won't give you code, but since you have tagged this algorithm, I don't think that will matter to you.
First, you have to get a vector perpendicular to the line.
You will have an unknown variable in y = ax + c ( c will be unknown )
To solve for that, Calculate it's value when the line passes through the center of the circle.
That is,
Plug in the location of the circle center to the line equation and solve for c.
Then calculate the intersection point of the original line and its normal.
This will give you the closest point on the line to the circle.
Calculate the distance between this point and the circle center (using the magnitude of the vector).
If this is less than the radius of the circle - voila, we have an intersection!
Another method uses the triangle ABC area formula. The intersection test is simpler and more efficient than the projection method, but finding the coordinates of the intersection point requires more work. At least it will be delayed to the point it is required.
The formula to compute the triangle area is : area = bh/2
where b is the base length and h is the height. We chose the segment AB to be the base so that h is the shortest distance from C, the circle center, to the line.
Since the triangle area can also be computed by a vector dot product we can determine h.
// compute the triangle area times 2 (area = area2/2)
area2 = abs( (Bx-Ax)*(Cy-Ay) - (Cx-Ax)(By-Ay) )
// compute the AB segment length
LAB = sqrt( (Bx-Ax)² + (By-Ay)² )
// compute the triangle height
h = area2/LAB
// if the line intersects the circle
if( h < R )
{
...
}
UPDATE 1 :
You could optimize the code by using the fast inverse square root computation described here to get a good approximation of 1/LAB.
Computing the intersection point is not that difficult. Here it goes
// compute the line AB direction vector components
Dx = (Bx-Ax)/LAB
Dy = (By-Ay)/LAB
// compute the distance from A toward B of closest point to C
t = Dx*(Cx-Ax) + Dy*(Cy-Ay)
// t should be equal to sqrt( (Cx-Ax)² + (Cy-Ay)² - h² )
// compute the intersection point distance from t
dt = sqrt( R² - h² )
// compute first intersection point coordinate
Ex = Ax + (t-dt)*Dx
Ey = Ay + (t-dt)*Dy
// compute second intersection point coordinate
Fx = Ax + (t+dt)*Dx
Fy = Ay + (t+dt)*Dy
If h = R then the line AB is tangent to the circle and the value dt = 0 and E = F. The point coordinates are those of E and F.
You should check that A is different of B and the segment length is not null if this may happen in your application.
I wrote a small script to test intersection by projecting circle's center point on to line.
vector distVector = centerPoint - projectedPoint;
if(distVector.length() < circle.radius)
{
double distance = circle.radius - distVector.length();
vector moveVector = distVector.normalize() * distance;
circle.move(moveVector);
}
http://jsfiddle.net/ercang/ornh3594/1/
If you need to check the collision with the segment, you also need to consider circle center's distance to start and end points.
vector distVector = centerPoint - startPoint;
if(distVector.length() < circle.radius)
{
double distance = circle.radius - distVector.length();
vector moveVector = distVector.normalize() * distance;
circle.move(moveVector);
}
https://jsfiddle.net/ercang/menp0991/
This solution I found seemed a little easier to follow then some of the other ones.
Taking:
p1 and p2 as the points for the line, and
c as the center point for the circle and r for the radius
I would solve for the equation of the line in slope-intercept form. However, I didn't want to have to deal with difficult equations with c as a point, so I just shifted the coordinate system over so that the circle is at 0,0
p3 = p1 - c
p4 = p2 - c
By the way, whenever I subtract points from each other I am subtracting the x's and then subtracting the y's, and putting them into a new point, just in case someone didn't know.
Anyway, I now solve for the equation of the line with p3 and p4:
m = (p4_y - p3_y) / (p4_x - p3) (the underscore is an attempt at subscript)
y = mx + b
y - mx = b (just put in a point for x and y, and insert the m we found)
Ok. Now I need to set these equations equal. First I need to solve the circle's equation for x
x^2 + y^2 = r^2
y^2 = r^2 - x^2
y = sqrt(r^2 - x^2)
Then I set them equal:
mx + b = sqrt(r^2 - x^2)
And solve for the quadratic equation (0 = ax^2 + bx + c):
(mx + b)^2 = r^2 - x^2
(mx)^2 + 2mbx + b^2 = r^2 - x^2
0 = m^2 * x^2 + x^2 + 2mbx + b^2 - r^2
0 = (m^2 + 1) * x^2 + 2mbx + b^2 - r^2
Now I have my a, b, and c.
a = m^2 + 1
b = 2mb
c = b^2 - r^2
So I put this into the quadratic formula:
(-b ± sqrt(b^2 - 4ac)) / 2a
And substitute in by values then simplify as much as possible:
(-2mb ± sqrt(b^2 - 4ac)) / 2a
(-2mb ± sqrt((-2mb)^2 - 4(m^2 + 1)(b^2 - r^2))) / 2(m^2 + 1)
(-2mb ± sqrt(4m^2 * b^2 - 4(m^2 * b^2 - m^2 * r^2 + b^2 - r^2))) / 2m^2 + 2
(-2mb ± sqrt(4 * (m^2 * b^2 - (m^2 * b^2 - m^2 * r^2 + b^2 - r^2))))/ 2m^2 + 2
(-2mb ± sqrt(4 * (m^2 * b^2 - m^2 * b^2 + m^2 * r^2 - b^2 + r^2)))/ 2m^2 + 2
(-2mb ± sqrt(4 * (m^2 * r^2 - b^2 + r^2)))/ 2m^2 + 2
(-2mb ± sqrt(4) * sqrt(m^2 * r^2 - b^2 + r^2))/ 2m^2 + 2
(-2mb ± 2 * sqrt(m^2 * r^2 - b^2 + r^2))/ 2m^2 + 2
(-2mb ± 2 * sqrt(m^2 * r^2 + r^2 - b^2))/ 2m^2 + 2
(-2mb ± 2 * sqrt(r^2 * (m^2 + 1) - b^2))/ 2m^2 + 2
This is almost as far as it will simplify. Finally, separate out to equations with the ±:
(-2mb + 2 * sqrt(r^2 * (m^2 + 1) - b^2))/ 2m^2 + 2 or
(-2mb - 2 * sqrt(r^2 * (m^2 + 1) - b^2))/ 2m^2 + 2
Then simply plug the result of both of those equations into the x in mx + b. For clarity, I wrote some JavaScript code to show how to use this:
function interceptOnCircle(p1,p2,c,r){
//p1 is the first line point
//p2 is the second line point
//c is the circle's center
//r is the circle's radius
var p3 = {x:p1.x - c.x, y:p1.y - c.y} //shifted line points
var p4 = {x:p2.x - c.x, y:p2.y - c.y}
var m = (p4.y - p3.y) / (p4.x - p3.x); //slope of the line
var b = p3.y - m * p3.x; //y-intercept of line
var underRadical = Math.pow((Math.pow(r,2)*(Math.pow(m,2)+1)),2)-Math.pow(b,2)); //the value under the square root sign
if (underRadical < 0){
//line completely missed
return false;
} else {
var t1 = (-2*m*b+2*Math.sqrt(underRadical))/(2 * Math.pow(m,2) + 2); //one of the intercept x's
var t2 = (-2*m*b-2*Math.sqrt(underRadical))/(2 * Math.pow(m,2) + 2); //other intercept's x
var i1 = {x:t1,y:m*t1+b} //intercept point 1
var i2 = {x:t2,y:m*t2+b} //intercept point 2
return [i1,i2];
}
}
I hope this helps!
P.S. If anyone finds any errors or has any suggestions, please comment. I am very new and welcome all help/suggestions.
Here's an implementation in Javascript. My approach is to first convert the line segment into an infinite line then find the intersection point(s). From there I check if the point(s) found are on the line segment. The code is well documented, you should be able to follow along.
You can try out the code here on this live demo.
The code was taken from my algorithms repo.
// Small epsilon value
var EPS = 0.0000001;
// point (x, y)
function Point(x, y) {
this.x = x;
this.y = y;
}
// Circle with center at (x,y) and radius r
function Circle(x, y, r) {
this.x = x;
this.y = y;
this.r = r;
}
// A line segment (x1, y1), (x2, y2)
function LineSegment(x1, y1, x2, y2) {
var d = Math.sqrt( (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2) );
if (d < EPS) throw 'A point is not a line segment';
this.x1 = x1; this.y1 = y1;
this.x2 = x2; this.y2 = y2;
}
// An infinite line defined as: ax + by = c
function Line(a, b, c) {
this.a = a; this.b = b; this.c = c;
// Normalize line for good measure
if (Math.abs(b) < EPS) {
c /= a; a = 1; b = 0;
} else {
a = (Math.abs(a) < EPS) ? 0 : a / b;
c /= b; b = 1;
}
}
// Given a line in standard form: ax + by = c and a circle with
// a center at (x,y) with radius r this method finds the intersection
// of the line and the circle (if any).
function circleLineIntersection(circle, line) {
var a = line.a, b = line.b, c = line.c;
var x = circle.x, y = circle.y, r = circle.r;
// Solve for the variable x with the formulas: ax + by = c (equation of line)
// and (x-X)^2 + (y-Y)^2 = r^2 (equation of circle where X,Y are known) and expand to obtain quadratic:
// (a^2 + b^2)x^2 + (2abY - 2ac + - 2b^2X)x + (b^2X^2 + b^2Y^2 - 2bcY + c^2 - b^2r^2) = 0
// Then use quadratic formula X = (-b +- sqrt(a^2 - 4ac))/2a to find the
// roots of the equation (if they exist) and this will tell us the intersection points
// In general a quadratic is written as: Ax^2 + Bx + C = 0
// (a^2 + b^2)x^2 + (2abY - 2ac + - 2b^2X)x + (b^2X^2 + b^2Y^2 - 2bcY + c^2 - b^2r^2) = 0
var A = a*a + b*b;
var B = 2*a*b*y - 2*a*c - 2*b*b*x;
var C = b*b*x*x + b*b*y*y - 2*b*c*y + c*c - b*b*r*r;
// Use quadratic formula x = (-b +- sqrt(a^2 - 4ac))/2a to find the
// roots of the equation (if they exist).
var D = B*B - 4*A*C;
var x1,y1,x2,y2;
// Handle vertical line case with b = 0
if (Math.abs(b) < EPS) {
// Line equation is ax + by = c, but b = 0, so x = c/a
x1 = c/a;
// No intersection
if (Math.abs(x-x1) > r) return [];
// Vertical line is tangent to circle
if (Math.abs((x1-r)-x) < EPS || Math.abs((x1+r)-x) < EPS)
return [new Point(x1, y)];
var dx = Math.abs(x1 - x);
var dy = Math.sqrt(r*r-dx*dx);
// Vertical line cuts through circle
return [
new Point(x1,y+dy),
new Point(x1,y-dy)
];
// Line is tangent to circle
} else if (Math.abs(D) < EPS) {
x1 = -B/(2*A);
y1 = (c - a*x1)/b;
return [new Point(x1,y1)];
// No intersection
} else if (D < 0) {
return [];
} else {
D = Math.sqrt(D);
x1 = (-B+D)/(2*A);
y1 = (c - a*x1)/b;
x2 = (-B-D)/(2*A);
y2 = (c - a*x2)/b;
return [
new Point(x1, y1),
new Point(x2, y2)
];
}
}
// Converts a line segment to a line in general form
function segmentToGeneralForm(x1,y1,x2,y2) {
var a = y1 - y2;
var b = x2 - x1;
var c = x2*y1 - x1*y2;
return new Line(a,b,c);
}
// Checks if a point 'pt' is inside the rect defined by (x1,y1), (x2,y2)
function pointInRectangle(pt,x1,y1,x2,y2) {
var x = Math.min(x1,x2), X = Math.max(x1,x2);
var y = Math.min(y1,y2), Y = Math.max(y1,y2);
return x - EPS <= pt.x && pt.x <= X + EPS &&
y - EPS <= pt.y && pt.y <= Y + EPS;
}
// Finds the intersection(s) of a line segment and a circle
function lineSegmentCircleIntersection(segment, circle) {
var x1 = segment.x1, y1 = segment.y1, x2 = segment.x2, y2 = segment.y2;
var line = segmentToGeneralForm(x1,y1,x2,y2);
var pts = circleLineIntersection(circle, line);
// No intersection
if (pts.length === 0) return [];
var pt1 = pts[0];
var includePt1 = pointInRectangle(pt1,x1,y1,x2,y2);
// Check for unique intersection
if (pts.length === 1) {
if (includePt1) return [pt1];
return [];
}
var pt2 = pts[1];
var includePt2 = pointInRectangle(pt2,x1,y1,x2,y2);
// Check for remaining intersections
if (includePt1 && includePt2) return [pt1, pt2];
if (includePt1) return [pt1];
if (includePt2) return [pt2];
return [];
}
You can find a point on a infinite line that is nearest to circle center by projecting vector AC onto vector AB. Calculate the distance between that point and circle center. If it is greater that R, there is no intersection. If the distance is equal to R, line is a tangent of the circle and the point nearest to circle center is actually the intersection point. If distance less that R, then there are 2 intersection points. They lie at the same distance from the point nearest to circle center. That distance can easily be calculated using Pythagorean theorem. Here's algorithm in pseudocode:
{
dX = bX - aX;
dY = bY - aY;
if ((dX == 0) && (dY == 0))
{
// A and B are the same points, no way to calculate intersection
return;
}
dl = (dX * dX + dY * dY);
t = ((cX - aX) * dX + (cY - aY) * dY) / dl;
// point on a line nearest to circle center
nearestX = aX + t * dX;
nearestY = aY + t * dY;
dist = point_dist(nearestX, nearestY, cX, cY);
if (dist == R)
{
// line segment touches circle; one intersection point
iX = nearestX;
iY = nearestY;
if (t < 0 || t > 1)
{
// intersection point is not actually within line segment
}
}
else if (dist < R)
{
// two possible intersection points
dt = sqrt(R * R - dist * dist) / sqrt(dl);
// intersection point nearest to A
t1 = t - dt;
i1X = aX + t1 * dX;
i1Y = aY + t1 * dY;
if (t1 < 0 || t1 > 1)
{
// intersection point is not actually within line segment
}
// intersection point farthest from A
t2 = t + dt;
i2X = aX + t2 * dX;
i2Y = aY + t2 * dY;
if (t2 < 0 || t2 > 1)
{
// intersection point is not actually within line segment
}
}
else
{
// no intersection
}
}
EDIT: added code to check whether found intersection points actually are within line segment.
Just an addition to this thread...
Below is a version of the code posted by pahlevan, but for C#/XNA and tidied up a little:
/// <summary>
/// Intersects a line and a circle.
/// </summary>
/// <param name="location">the location of the circle</param>
/// <param name="radius">the radius of the circle</param>
/// <param name="lineFrom">the starting point of the line</param>
/// <param name="lineTo">the ending point of the line</param>
/// <returns>true if the line and circle intersect each other</returns>
public static bool IntersectLineCircle(Vector2 location, float radius, Vector2 lineFrom, Vector2 lineTo)
{
float ab2, acab, h2;
Vector2 ac = location - lineFrom;
Vector2 ab = lineTo - lineFrom;
Vector2.Dot(ref ab, ref ab, out ab2);
Vector2.Dot(ref ac, ref ab, out acab);
float t = acab / ab2;
if (t < 0)
t = 0;
else if (t > 1)
t = 1;
Vector2 h = ((ab * t) + lineFrom) - location;
Vector2.Dot(ref h, ref h, out h2);
return (h2 <= (radius * radius));
}
Weirdly I can answer but not comment...
I liked Multitaskpro's approach of shifting everything to make the centre of the circle fall on the origin. Unfortunately there are two problems in his code. First in the under-the-square-root part you need to remove the double power. So not:
var underRadical = Math.pow((Math.pow(r,2)*(Math.pow(m,2)+1)),2)-Math.pow(b,2));
but:
var underRadical = Math.pow(r,2)*(Math.pow(m,2)+1)) - Math.pow(b,2);
In the final coordinates he forgets to shift the solution back. So not:
var i1 = {x:t1,y:m*t1+b}
but:
var i1 = {x:t1+c.x, y:m*t1+b+c.y};
The whole function then becomes:
function interceptOnCircle(p1, p2, c, r) {
//p1 is the first line point
//p2 is the second line point
//c is the circle's center
//r is the circle's radius
var p3 = {x:p1.x - c.x, y:p1.y - c.y}; //shifted line points
var p4 = {x:p2.x - c.x, y:p2.y - c.y};
var m = (p4.y - p3.y) / (p4.x - p3.x); //slope of the line
var b = p3.y - m * p3.x; //y-intercept of line
var underRadical = Math.pow(r,2)*Math.pow(m,2) + Math.pow(r,2) - Math.pow(b,2); //the value under the square root sign
if (underRadical < 0) {
//line completely missed
return false;
} else {
var t1 = (-m*b + Math.sqrt(underRadical))/(Math.pow(m,2) + 1); //one of the intercept x's
var t2 = (-m*b - Math.sqrt(underRadical))/(Math.pow(m,2) + 1); //other intercept's x
var i1 = {x:t1+c.x, y:m*t1+b+c.y}; //intercept point 1
var i2 = {x:t2+c.x, y:m*t2+b+c.y}; //intercept point 2
return [i1, i2];
}
}
In this post circle line collision will be checked by checking distance between circle center and point on line segment (Ipoint) that represent intersection point between normal N (Image 2) from circle center to line segment.
(https://i.stack.imgur.com/3o6do.png)
On image 1 one circle and one line are shown, vector A point to line start point, vector B point to line end point, vector C point to circle center. Now we must find vector E (from line start point to circle center) and vector D (from line start point to line end point) this calculation is shown on image 1.
(https://i.stack.imgur.com/7098a.png)
At image 2 we can see that vector E is projected on Vector D by "dot product" of vector E and unit vector D, result of dot product is scalar Xp that represent the distance between line start point and point of intersection (Ipoint) of vector N and vector D.
Next vector X is found by multiplying unit vector D and scalar Xp.
Now we need to find vector Z (vector to Ipoint), its easy its simple vector addition of vector A (start point on line) and vector X. Next we need to deal with special cases we must check is Ipoint on line segment, if its not we must find out is it left of it or right of it, we will use vector closest to determine which point is closest to circle.
(https://i.stack.imgur.com/p9WIr.png)
When projection Xp is negative Ipoint is left of line segment, vector closest is equal to vector of line start point, when projection Xp is greater then magnitude of vector D then Ipoint is right of line segment then closest vector is equal to vector of line end point in any other case closest vector is equal to vector Z.
Now when we have closest vector , we need to find vector from circle center to Ipoint (dist vector), its simple we just need to subtract closest vector from center vector. Next just check if vector dist magnitude is less then circle radius if it is then they collide, if its not there is no collision.
(https://i.stack.imgur.com/QJ63q.png)
For end, we can return some values for resolving collision , easiest way is to return overlap of collision (subtract radius from vector dist magnitude) and return axis of collision, its vector D. Also intersection point is vector Z if needed.
You'll need some math here:
Suppose A = (Xa, Ya), B = (Xb, Yb) and C = (Xc, Yc). Any point on the line from A to B has coordinates (alpha*Xa + (1-alpha)Xb, alphaYa + (1-alpha)*Yb) = P
If the point P has distance R to C, it must be on the circle. What you want is to solve
distance(P, C) = R
that is
(alpha*Xa + (1-alpha)*Xb)^2 + (alpha*Ya + (1-alpha)*Yb)^2 = R^2
alpha^2*Xa^2 + alpha^2*Xb^2 - 2*alpha*Xb^2 + Xb^2 + alpha^2*Ya^2 + alpha^2*Yb^2 - 2*alpha*Yb^2 + Yb^2=R^2
(Xa^2 + Xb^2 + Ya^2 + Yb^2)*alpha^2 - 2*(Xb^2 + Yb^2)*alpha + (Xb^2 + Yb^2 - R^2) = 0
if you apply the ABC-formula to this equation to solve it for alpha, and compute the coordinates of P using the solution(s) for alpha, you get the intersection points, if any exist.
' VB.NET - Code
Function CheckLineSegmentCircleIntersection(x1 As Double, y1 As Double, x2 As Double, y2 As Double, xc As Double, yc As Double, r As Double) As Boolean
Static xd As Double = 0.0F
Static yd As Double = 0.0F
Static t As Double = 0.0F
Static d As Double = 0.0F
Static dx_2_1 As Double = 0.0F
Static dy_2_1 As Double = 0.0F
dx_2_1 = x2 - x1
dy_2_1 = y2 - y1
t = ((yc - y1) * dy_2_1 + (xc - x1) * dx_2_1) / (dy_2_1 * dy_2_1 + dx_2_1 * dx_2_1)
If 0 <= t And t <= 1 Then
xd = x1 + t * dx_2_1
yd = y1 + t * dy_2_1
d = Math.Sqrt((xd - xc) * (xd - xc) + (yd - yc) * (yd - yc))
Return d <= r
Else
d = Math.Sqrt((xc - x1) * (xc - x1) + (yc - y1) * (yc - y1))
If d <= r Then
Return True
Else
d = Math.Sqrt((xc - x2) * (xc - x2) + (yc - y2) * (yc - y2))
If d <= r Then
Return True
Else
Return False
End If
End If
End If
End Function
I have created this function for iOS following the answer given by chmike
+ (NSArray *)intersectionPointsOfCircleWithCenter:(CGPoint)center withRadius:(float)radius toLinePoint1:(CGPoint)p1 andLinePoint2:(CGPoint)p2
{
NSMutableArray *intersectionPoints = [NSMutableArray array];
float Ax = p1.x;
float Ay = p1.y;
float Bx = p2.x;
float By = p2.y;
float Cx = center.x;
float Cy = center.y;
float R = radius;
// compute the euclidean distance between A and B
float LAB = sqrt( pow(Bx-Ax, 2)+pow(By-Ay, 2) );
// compute the direction vector D from A to B
float Dx = (Bx-Ax)/LAB;
float Dy = (By-Ay)/LAB;
// Now the line equation is x = Dx*t + Ax, y = Dy*t + Ay with 0 <= t <= 1.
// compute the value t of the closest point to the circle center (Cx, Cy)
float t = Dx*(Cx-Ax) + Dy*(Cy-Ay);
// This is the projection of C on the line from A to B.
// compute the coordinates of the point E on line and closest to C
float Ex = t*Dx+Ax;
float Ey = t*Dy+Ay;
// compute the euclidean distance from E to C
float LEC = sqrt( pow(Ex-Cx, 2)+ pow(Ey-Cy, 2) );
// test if the line intersects the circle
if( LEC < R )
{
// compute distance from t to circle intersection point
float dt = sqrt( pow(R, 2) - pow(LEC,2) );
// compute first intersection point
float Fx = (t-dt)*Dx + Ax;
float Fy = (t-dt)*Dy + Ay;
// compute second intersection point
float Gx = (t+dt)*Dx + Ax;
float Gy = (t+dt)*Dy + Ay;
[intersectionPoints addObject:[NSValue valueWithCGPoint:CGPointMake(Fx, Fy)]];
[intersectionPoints addObject:[NSValue valueWithCGPoint:CGPointMake(Gx, Gy)]];
}
// else test if the line is tangent to circle
else if( LEC == R ) {
// tangent point to circle is E
[intersectionPoints addObject:[NSValue valueWithCGPoint:CGPointMake(Ex, Ey)]];
}
else {
// line doesn't touch circle
}
return intersectionPoints;
}
Circle is really a bad guy :) So a good way is to avoid true circle, if you can. If you are doing collision check for games you can go with some simplifications and have just 3 dot products, and a few comparisons.
I call this "fat point" or "thin circle". its kind of a ellipse with zero radius in a direction parallel to a segment. but full radius in a direction perpendicular to a segment
First, i would consider renaming and switching coordinate system to avoid excessive data:
s0s1 = B-A;
s0qp = C-A;
rSqr = r*r;
Second, index h in hvec2f means than vector must favor horisontal operations, like dot()/det(). Which means its components are to be placed in a separate xmm registers, to avoid shuffling/hadd'ing/hsub'ing. And here we go, with most performant version of simpliest collision detection for 2D game:
bool fat_point_collides_segment(const hvec2f& s0qp, const hvec2f& s0s1, const float& rSqr) {
auto a = dot(s0s1, s0s1);
//if( a != 0 ) // if you haven't zero-length segments omit this, as it would save you 1 _mm_comineq_ss() instruction and 1 memory fetch
{
auto b = dot(s0s1, s0qp);
auto t = b / a; // length of projection of s0qp onto s0s1
//std::cout << "t = " << t << "\n";
if ((t >= 0) && (t <= 1)) //
{
auto c = dot(s0qp, s0qp);
auto r2 = c - a * t * t;
return (r2 <= rSqr); // true if collides
}
}
return false;
}
I doubt you can optimize it any further. I am using it for neural-network driven car racing collision detection, to process millions of millions iteration steps.
If the line's coordinates are A.x, A.y and B.x, B.y and the circles center is C.x, C.y then the lines formulae are:
x = A.x * t + B.x * (1 - t)
y = A.y * t + B.y * (1 - t)
where 0<=t<=1
and the circle is
(C.x - x)^2 + (C.y - y)^2 = R^2
if you substitute x and y formulae of the line into the circles formula you get a second order equation of t and its solutions are the intersection points (if there are any). If you get a t which is smaller than 0 or greater than 1 then its not a solution but it shows that the line is 'pointing' to the direction of the circle.
If you find the distance between the center of the sphere (since it's 3D I assume you mean sphere and not circle) and the line, then check if that distance is less than the radius that will do the trick.
The collision point is obviously the closest point between the line and the sphere (which will be calculated when you're calculating the distance between the sphere and the line)
Distance between a point and a line:
http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
Another one in c# (partial Circle class).
Tested and works like a charm.
public class Circle : IEquatable<Circle>
{
// ******************************************************************
// The center of a circle
private Point _center;
// The radius of a circle
private double _radius;
// ******************************************************************
/// <summary>
/// Find all intersections (0, 1, 2) of the circle with a line defined by its 2 points.
/// Using: http://math.stackexchange.com/questions/228841/how-do-i-calculate-the-intersections-of-a-straight-line-and-a-circle
/// Note: p is the Center.X and q is Center.Y
/// </summary>
/// <param name="linePoint1"></param>
/// <param name="linePoint2"></param>
/// <returns></returns>
public List<Point> GetIntersections(Point linePoint1, Point linePoint2)
{
List<Point> intersections = new List<Point>();
double dx = linePoint2.X - linePoint1.X;
if (dx.AboutEquals(0)) // Straight vertical line
{
if (linePoint1.X.AboutEquals(Center.X - Radius) || linePoint1.X.AboutEquals(Center.X + Radius))
{
Point pt = new Point(linePoint1.X, Center.Y);
intersections.Add(pt);
}
else if (linePoint1.X > Center.X - Radius && linePoint1.X < Center.X + Radius)
{
double x = linePoint1.X - Center.X;
Point pt = new Point(linePoint1.X, Center.Y + Math.Sqrt(Radius * Radius - (x * x)));
intersections.Add(pt);
pt = new Point(linePoint1.X, Center.Y - Math.Sqrt(Radius * Radius - (x * x)));
intersections.Add(pt);
}
return intersections;
}
// Line function (y = mx + b)
double dy = linePoint2.Y - linePoint1.Y;
double m = dy / dx;
double b = linePoint1.Y - m * linePoint1.X;
double A = m * m + 1;
double B = 2 * (m * b - m * _center.Y - Center.X);
double C = Center.X * Center.X + Center.Y * Center.Y - Radius * Radius - 2 * b * Center.Y + b * b;
double discriminant = B * B - 4 * A * C;
if (discriminant < 0)
{
return intersections; // there is no intersections
}
if (discriminant.AboutEquals(0)) // Tangeante (touch on 1 point only)
{
double x = -B / (2 * A);
double y = m * x + b;
intersections.Add(new Point(x, y));
}
else // Secant (touch on 2 points)
{
double x = (-B + Math.Sqrt(discriminant)) / (2 * A);
double y = m * x + b;
intersections.Add(new Point(x, y));
x = (-B - Math.Sqrt(discriminant)) / (2 * A);
y = m * x + b;
intersections.Add(new Point(x, y));
}
return intersections;
}
// ******************************************************************
// Get the center
[XmlElement("Center")]
public Point Center
{
get { return _center; }
set
{
_center = value;
}
}
// ******************************************************************
// Get the radius
[XmlElement]
public double Radius
{
get { return _radius; }
set { _radius = value; }
}
//// ******************************************************************
//[XmlArrayItemAttribute("DoublePoint")]
//public List<Point> Coordinates
//{
// get { return _coordinates; }
//}
// ******************************************************************
// Construct a circle without any specification
public Circle()
{
_center.X = 0;
_center.Y = 0;
_radius = 0;
}
// ******************************************************************
// Construct a circle without any specification
public Circle(double radius)
{
_center.X = 0;
_center.Y = 0;
_radius = radius;
}
// ******************************************************************
// Construct a circle with the specified circle
public Circle(Circle circle)
{
_center = circle._center;
_radius = circle._radius;
}
// ******************************************************************
// Construct a circle with the specified center and radius
public Circle(Point center, double radius)
{
_center = center;
_radius = radius;
}
// ******************************************************************
// Construct a circle based on one point
public Circle(Point center)
{
_center = center;
_radius = 0;
}
// ******************************************************************
// Construct a circle based on two points
public Circle(Point p1, Point p2)
{
Circle2Points(p1, p2);
}
Required:
using System;
namespace Mathematic
{
public static class DoubleExtension
{
// ******************************************************************
// Base on Hans Passant Answer on:
// http://stackoverflow.com/questions/2411392/double-epsilon-for-equality-greater-than-less-than-less-than-or-equal-to-gre
/// <summary>
/// Compare two double taking in account the double precision potential error.
/// Take care: truncation errors accumulate on calculation. More you do, more you should increase the epsilon.
public static bool AboutEquals(this double value1, double value2)
{
if (double.IsPositiveInfinity(value1))
return double.IsPositiveInfinity(value2);
if (double.IsNegativeInfinity(value1))
return double.IsNegativeInfinity(value2);
if (double.IsNaN(value1))
return double.IsNaN(value2);
double epsilon = Math.Max(Math.Abs(value1), Math.Abs(value2)) * 1E-15;
return Math.Abs(value1 - value2) <= epsilon;
}
// ******************************************************************
// Base on Hans Passant Answer on:
// http://stackoverflow.com/questions/2411392/double-epsilon-for-equality-greater-than-less-than-less-than-or-equal-to-gre
/// <summary>
/// Compare two double taking in account the double precision potential error.
/// Take care: truncation errors accumulate on calculation. More you do, more you should increase the epsilon.
/// You get really better performance when you can determine the contextual epsilon first.
/// </summary>
/// <param name="value1"></param>
/// <param name="value2"></param>
/// <param name="precalculatedContextualEpsilon"></param>
/// <returns></returns>
public static bool AboutEquals(this double value1, double value2, double precalculatedContextualEpsilon)
{
if (double.IsPositiveInfinity(value1))
return double.IsPositiveInfinity(value2);
if (double.IsNegativeInfinity(value1))
return double.IsNegativeInfinity(value2);
if (double.IsNaN(value1))
return double.IsNaN(value2);
return Math.Abs(value1 - value2) <= precalculatedContextualEpsilon;
}
// ******************************************************************
public static double GetContextualEpsilon(this double biggestPossibleContextualValue)
{
return biggestPossibleContextualValue * 1E-15;
}
// ******************************************************************
/// <summary>
/// Mathlab equivalent
/// </summary>
/// <param name="dividend"></param>
/// <param name="divisor"></param>
/// <returns></returns>
public static double Mod(this double dividend, double divisor)
{
return dividend - System.Math.Floor(dividend / divisor) * divisor;
}
// ******************************************************************
}
}
Here is good solution in JavaScript (with all required mathematics and live illustration)
https://bl.ocks.org/milkbread/11000965
Though is_on function in that solution needs modifications:
function is_on(a, b, c) {
return Math.abs(distance(a,c) + distance(c,b) - distance(a,b))<0.000001;
}
I know it's been a while since this thread was open. From the answer given by chmike and improved by Aqib Mumtaz. They give a good answer but only works for a infinite line as said Aqib. So I add some comparisons to know if the line segment touch the circle, I write it in Python.
def LineIntersectCircle(c, r, p1, p2):
#p1 is the first line point
#p2 is the second line point
#c is the circle's center
#r is the circle's radius
p3 = [p1[0]-c[0], p1[1]-c[1]]
p4 = [p2[0]-c[0], p2[1]-c[1]]
m = (p4[1] - p3[1]) / (p4[0] - p3[0])
b = p3[1] - m * p3[0]
underRadical = math.pow(r,2)*math.pow(m,2) + math.pow(r,2) - math.pow(b,2)
if (underRadical < 0):
print("NOT")
else:
t1 = (-2*m*b+2*math.sqrt(underRadical)) / (2 * math.pow(m,2) + 2)
t2 = (-2*m*b-2*math.sqrt(underRadical)) / (2 * math.pow(m,2) + 2)
i1 = [t1+c[0], m * t1 + b + c[1]]
i2 = [t2+c[0], m * t2 + b + c[1]]
if p1[0] > p2[0]: #Si el punto 1 es mayor al 2 en X
if (i1[0] < p1[0]) and (i1[0] > p2[0]): #Si el punto iX esta entre 2 y 1 en X
if p1[1] > p2[1]: #Si el punto 1 es mayor al 2 en Y
if (i1[1] < p1[1]) and (i1[1] > p2[1]): #Si el punto iy esta entre 2 y 1
print("Intersection")
if p1[1] < p2[1]: #Si el punto 2 es mayo al 2 en Y
if (i1[1] > p1[1]) and (i1[1] < p2[1]): #Si el punto iy esta entre 1 y 2
print("Intersection")
if p1[0] < p2[0]: #Si el punto 2 es mayor al 1 en X
if (i1[0] > p1[0]) and (i1[0] < p2[0]): #Si el punto iX esta entre 1 y 2 en X
if p1[1] > p2[1]: #Si el punto 1 es mayor al 2 en Y
if (i1[1] < p1[1]) and (i1[1] > p2[1]): #Si el punto iy esta entre 2 y 1
print("Intersection")
if p1[1] < p2[1]: #Si el punto 2 es mayo al 2 en Y
if (i1[1] > p1[1]) and (i1[1] < p2[1]): #Si el punto iy esta entre 1 y 2
print("Intersection")
if p1[0] > p2[0]: #Si el punto 1 es mayor al 2 en X
if (i2[0] < p1[0]) and (i2[0] > p2[0]): #Si el punto iX esta entre 2 y 1 en X
if p1[1] > p2[1]: #Si el punto 1 es mayor al 2 en Y
if (i2[1] < p1[1]) and (i2[1] > p2[1]): #Si el punto iy esta entre 2 y 1
print("Intersection")
if p1[1] < p2[1]: #Si el punto 2 es mayo al 2 en Y
if (i2[1] > p1[1]) and (i2[1] < p2[1]): #Si el punto iy esta entre 1 y 2
print("Intersection")
if p1[0] < p2[0]: #Si el punto 2 es mayor al 1 en X
if (i2[0] > p1[0]) and (i2[0] < p2[0]): #Si el punto iX esta entre 1 y 2 en X
if p1[1] > p2[1]: #Si el punto 1 es mayor al 2 en Y
if (i2[1] < p1[1]) and (i2[1] > p2[1]): #Si el punto iy esta entre 2 y 1
print("Intersection")
if p1[1] < p2[1]: #Si el punto 2 es mayo al 2 en Y
if (i2[1] > p1[1]) and (i2[1] < p2[1]): #Si el punto iy esta entre 1 y 2
print("Intersection")
This Java Function returns a DVec2 Object. It takes a DVec2 for the center of the circle, the radius of the circle, and a Line.
public static DVec2 CircLine(DVec2 C, double r, Line line)
{
DVec2 A = line.p1;
DVec2 B = line.p2;
DVec2 P;
DVec2 AC = new DVec2( C );
AC.sub(A);
DVec2 AB = new DVec2( B );
AB.sub(A);
double ab2 = AB.dot(AB);
double acab = AC.dot(AB);
double t = acab / ab2;
if (t < 0.0)
t = 0.0;
else if (t > 1.0)
t = 1.0;
//P = A + t * AB;
P = new DVec2( AB );
P.mul( t );
P.add( A );
DVec2 H = new DVec2( P );
H.sub( C );
double h2 = H.dot(H);
double r2 = r * r;
if(h2 > r2)
return null;
else
return P;
}
I just needed that, so I came up with this solution. The language is maxscript, but it should be easily translated to any other language.
sideA, sideB and CircleRadius are scalars, the rest of the variables are points as [x,y,z]. I'm assuming z=0 to solve on the plane XY
fn projectPoint p1 p2 p3 = --project p1 perpendicular to the line p2-p3
(
local v= normalize (p3-p2)
local p= (p1-p2)
p2+((dot v p)*v)
)
fn findIntersectionLineCircle CircleCenter CircleRadius LineP1 LineP2=
(
pp=projectPoint CircleCenter LineP1 LineP2
sideA=distance pp CircleCenter
--use pythagoras to solve the third side
sideB=sqrt(CircleRadius^2-sideA^2) -- this will return NaN if they don't intersect
IntersectV=normalize (pp-CircleCenter)
perpV=[IntersectV.y,-IntersectV.x,IntersectV.z]
--project the point to both sides to find the solutions
solution1=pp+(sideB*perpV)
solution2=pp-(sideB*perpV)
return #(solution1,solution2)
)
Here is my solution in TypeScript, following the idea that #Mizipzor suggested (using projection):
/**
* Determines whether a line segment defined by a start and end point intersects with a sphere defined by a center point and a radius
* #param a the start point of the line segment
* #param b the end point of the line segment
* #param c the center point of the sphere
* #param r the radius of the sphere
*/
export function lineSphereIntersects(
a: IPoint,
b: IPoint,
c: IPoint,
r: number
): boolean {
// find the three sides of the triangle formed by the three points
const ab: number = distance(a, b);
const ac: number = distance(a, c);
const bc: number = distance(b, c);
// check to see if either ends of the line segment are inside of the sphere
if (ac < r || bc < r) {
return true;
}
// find the angle between the line segment and the center of the sphere
const numerator: number = Math.pow(ac, 2) + Math.pow(ab, 2) - Math.pow(bc, 2);
const denominator: number = 2 * ac * ab;
const cab: number = Math.acos(numerator / denominator);
// find the distance from the center of the sphere and the line segment
const cd: number = Math.sin(cab) * ac;
// if the radius is at least as long as the distance between the center and the line
if (r >= cd) {
// find the distance between the line start and the point on the line closest to
// the center of the sphere
const ad: number = Math.cos(cab) * ac;
// intersection occurs when the point on the line closest to the sphere center is
// no further away than the end of the line
return ad <= ab;
}
return false;
}
export function distance(a: IPoint, b: IPoint): number {
return Math.sqrt(
Math.pow(b.z - a.z, 2) + Math.pow(b.y - a.y, 2) + Math.pow(b.x - a.x, 2)
);
}
export interface IPoint {
x: number;
y: number;
z: number;
}
Solution in python, based on #Joe Skeen
def check_line_segment_circle_intersection(line, point, radious):
""" Checks whether a point intersects with a line defined by two points.
A `point` is list with two values: [2, 3]
A `line` is list with two points: [point1, point2]
"""
line_distance = distance(line[0], line[1])
distance_start_to_point = distance(line[0], point)
distance_end_to_point = distance(line[1], point)
if (distance_start_to_point <= radious or distance_end_to_point <= radious):
return True
# angle between line and point with law of cosines
numerator = (math.pow(distance_start_to_point, 2)
+ math.pow(line_distance, 2)
- math.pow(distance_end_to_point, 2))
denominator = 2 * distance_start_to_point * line_distance
ratio = numerator / denominator
ratio = ratio if ratio <= 1 else 1 # To account for float errors
ratio = ratio if ratio >= -1 else -1 # To account for float errors
angle = math.acos(ratio)
# distance from the point to the line with sin projection
distance_line_to_point = math.sin(angle) * distance_start_to_point
if distance_line_to_point <= radious:
point_projection_in_line = math.cos(angle) * distance_start_to_point
# Intersection occurs whent the point projection in the line is less
# than the line distance and positive
return point_projection_in_line <= line_distance and point_projection_in_line >= 0
return False
def distance(point1, point2):
return math.sqrt(
math.pow(point1[1] - point2[1], 2) +
math.pow(point1[0] - point2[0], 2)
)
Maybe there is another way to solve this problem using rotation of coordinate system.
Normally, if one segment is horizontal or vertical, which means parallel to x or y axis, it's quite easy to solve the intersection point since we already know one coordinate of the intersection, if any. The rest is obviously finding the other coordinate using circle's equation.
Inspired by this idea, we could apply coordinates system rotation to make one axis's direction coincide with segment's direction.
Let's take an example of circle x^2+y^2=1 and segment P1-P2 with P1(-1.5,0.5) and P2(-0.5,-0.5) in x-y system. And the following equations to remind you of the rotation principles, where theta is the angle anticlockwise, x'-y' is the system after rotation :
x' = x * cos(theta) + y * sin(theta)
y' = - x * sin(theta) + y * cos(theta)
and inversely
x = x' * cos(theta) - y' * sin(theta)
y = x' * sin(theta) + y' * cos(theta)
Considering the segment P1-P2 direction (45° in terms of -x), we could take theta=45°. Taking the second equations of rotation into circle's equation in x-y system : x^2+y^2=1 and after simple operations we get the 'same' equation in x'-y' system : x'^2+y'^2=1.
Segment endpoints become in x'-y' system using the first equations of rotation => P1(-sqrt(2)/2, sqrt(2)), P2(-sqrt(2)/2, 0).
Assuming the intersection as P. We have in x'-y' Px = -sqrt(2)/2. Using the new equation of circle, we get Py = +sqrt(2)/2. Converting P into original x-y system, we get finally P(-1,0).
To implement this numerically, we could firstly have a look at segment's direction : horizontal, vertical or not. If it belongs to the two first cases, it's simple like I said. If the last case, apply the algorithms above.
To juge if there is intersection, we could compare the solution with the endpoints coordinates, to see whether there is one root between them.
I believe this method could be also applied to other curves as long as we have its equation. The only weakness is that we should solve the equation in x'-y' system for the other coordinate, which might be difficult.
Here is a solution written in golang. The method is similar to some other answers posted here, but not quite the same. It is easy to implement, and has been tested. Here are the steps:
Translate coordinates so that the circle is at the origin.
Express the line segment as parametrized functions of t for both the x and y coordinates. If t is 0, the function's values are one end point of the segment, and if t is 1, the function's values are the other end point.
Solve, if possible, the quadratic equation resulting from constraining values of t that produce x, y coordinates with distances from the origin equal to the circle's radius.
Throw out solutions where t is < 0 or > 1 ( <= 0 or >= 1 for an open segment). Those points are not contained in the segment.
Translate back to original coordinates.
The values for A, B, and C for the quadratic are derived here, where (n-et) and (m-dt) are the equations for the line's x and y coordinates, respectively. r is the radius of the circle.
(n-et)(n-et) + (m-dt)(m-dt) = rr
nn - 2etn + etet + mm - 2mdt + dtdt = rr
(ee+dd)tt - 2(en + dm)t + nn + mm - rr = 0
Therefore A = ee+dd, B = - 2(en + dm), and C = nn + mm - rr.
Here is the golang code for the function:
package geom
import (
"math"
)
// SegmentCircleIntersection return points of intersection between a circle and
// a line segment. The Boolean intersects returns true if one or
// more solutions exist. If only one solution exists,
// x1 == x2 and y1 == y2.
// s1x and s1y are coordinates for one end point of the segment, and
// s2x and s2y are coordinates for the other end of the segment.
// cx and cy are the coordinates of the center of the circle and
// r is the radius of the circle.
func SegmentCircleIntersection(s1x, s1y, s2x, s2y, cx, cy, r float64) (x1, y1, x2, y2 float64, intersects bool) {
// (n-et) and (m-dt) are expressions for the x and y coordinates
// of a parameterized line in coordinates whose origin is the
// center of the circle.
// When t = 0, (n-et) == s1x - cx and (m-dt) == s1y - cy
// When t = 1, (n-et) == s2x - cx and (m-dt) == s2y - cy.
n := s2x - cx
m := s2y - cy
e := s2x - s1x
d := s2y - s1y
// lineFunc checks if the t parameter is in the segment and if so
// calculates the line point in the unshifted coordinates (adds back
// cx and cy.
lineFunc := func(t float64) (x, y float64, inBounds bool) {
inBounds = t >= 0 && t <= 1 // Check bounds on closed segment
// To check bounds for an open segment use t > 0 && t < 1
if inBounds { // Calc coords for point in segment
x = n - e*t + cx
y = m - d*t + cy
}
return
}
// Since we want the points on the line distance r from the origin,
// (n-et)(n-et) + (m-dt)(m-dt) = rr.
// Expanding and collecting terms yeilds the following quadratic equation:
A, B, C := e*e+d*d, -2*(e*n+m*d), n*n+m*m-r*r
D := B*B - 4*A*C // discriminant of quadratic
if D < 0 {
return // No solution
}
D = math.Sqrt(D)
var p1In, p2In bool
x1, y1, p1In = lineFunc((-B + D) / (2 * A)) // First root
if D == 0.0 {
intersects = p1In
x2, y2 = x1, y1
return // Only possible solution, quadratic has one root.
}
x2, y2, p2In = lineFunc((-B - D) / (2 * A)) // Second root
intersects = p1In || p2In
if p1In == false { // Only x2, y2 may be valid solutions
x1, y1 = x2, y2
} else if p2In == false { // Only x1, y1 are valid solutions
x2, y2 = x1, y1
}
return
}
I tested it with this function, which confirms that solution points are within the line segment and on the circle. It makes a test segment and sweeps it around the given circle:
package geom_test
import (
"testing"
. "**put your package path here**"
)
func CheckEpsilon(t *testing.T, v, epsilon float64, message string) {
if v > epsilon || v < -epsilon {
t.Error(message, v, epsilon)
t.FailNow()
}
}
func TestSegmentCircleIntersection(t *testing.T) {
epsilon := 1e-10 // Something smallish
x1, y1 := 5.0, 2.0 // segment end point 1
x2, y2 := 50.0, 30.0 // segment end point 2
cx, cy := 100.0, 90.0 // center of circle
r := 80.0
segx, segy := x2-x1, y2-y1
testCntr, solutionCntr := 0, 0
for i := -100; i < 100; i++ {
for j := -100; j < 100; j++ {
testCntr++
s1x, s2x := x1+float64(i), x2+float64(i)
s1y, s2y := y1+float64(j), y2+float64(j)
sc1x, sc1y := s1x-cx, s1y-cy
seg1Inside := sc1x*sc1x+sc1y*sc1y < r*r
sc2x, sc2y := s2x-cx, s2y-cy
seg2Inside := sc2x*sc2x+sc2y*sc2y < r*r
p1x, p1y, p2x, p2y, intersects := SegmentCircleIntersection(s1x, s1y, s2x, s2y, cx, cy, r)
if intersects {
solutionCntr++
//Check if points are on circle
c1x, c1y := p1x-cx, p1y-cy
deltaLen1 := (c1x*c1x + c1y*c1y) - r*r
CheckEpsilon(t, deltaLen1, epsilon, "p1 not on circle")
c2x, c2y := p2x-cx, p2y-cy
deltaLen2 := (c2x*c2x + c2y*c2y) - r*r
CheckEpsilon(t, deltaLen2, epsilon, "p2 not on circle")
// Check if points are on the line through the line segment
// "cross product" of vector from a segment point to the point
// and the vector for the segment should be near zero
vp1x, vp1y := p1x-s1x, p1y-s1y
crossProd1 := vp1x*segy - vp1y*segx
CheckEpsilon(t, crossProd1, epsilon, "p1 not on line ")
vp2x, vp2y := p2x-s1x, p2y-s1y
crossProd2 := vp2x*segy - vp2y*segx
CheckEpsilon(t, crossProd2, epsilon, "p2 not on line ")
// Check if point is between points s1 and s2 on line
// This means the sign of the dot prod of the segment vector
// and point to segment end point vectors are opposite for
// either end.
wp1x, wp1y := p1x-s2x, p1y-s2y
dp1v := vp1x*segx + vp1y*segy
dp1w := wp1x*segx + wp1y*segy
if (dp1v < 0 && dp1w < 0) || (dp1v > 0 && dp1w > 0) {
t.Error("point not contained in segment ", dp1v, dp1w)
t.FailNow()
}
wp2x, wp2y := p2x-s2x, p2y-s2y
dp2v := vp2x*segx + vp2y*segy
dp2w := wp2x*segx + wp2y*segy
if (dp2v < 0 && dp2w < 0) || (dp2v > 0 && dp2w > 0) {
t.Error("point not contained in segment ", dp2v, dp2w)
t.FailNow()
}
if s1x == s2x && s2y == s1y { //Only one solution
// Test that one end of the segment is withing the radius of the circle
// and one is not
if seg1Inside && seg2Inside {
t.Error("Only one solution but both line segment ends inside")
t.FailNow()
}
if !seg1Inside && !seg2Inside {
t.Error("Only one solution but both line segment ends outside")
t.FailNow()
}
}
} else { // No intersection, check if both points outside or inside
if (seg1Inside && !seg2Inside) || (!seg1Inside && seg2Inside) {
t.Error("No solution but only one point in radius of circle")
t.FailNow()
}
}
}
}
t.Log("Tested ", testCntr, " examples and found ", solutionCntr, " solutions.")
}
Here is the output of the test:
=== RUN TestSegmentCircleIntersection
--- PASS: TestSegmentCircleIntersection (0.00s)
geom_test.go:105: Tested 40000 examples and found 7343 solutions.
Finally, the method is easily extendable to the case of a ray starting at one point, going through the other and extending to infinity, by only testing if t > 0 or t < 1 but not both.
Another solution, first considering the case where you don't care about collision location. Note that this particular function is built assuming vector input for xB and yB but can easily be modified if that is not the case. Variable names are defined at the start of the function
#Line segment points (A0, Af) defined by xA0, yA0, xAf, yAf; circle center denoted by xB, yB; rB=radius of circle, rA = radius of point (set to zero for your application)
def staticCollision_f(xA0, yA0, xAf, yAf, rA, xB, yB, rB): #note potential speed up here by casting all variables to same type and/or using Cython
#Build equations of a line for linear agents (convert y = mx + b to ax + by + c = 0 means that a = -m, b = 1, c = -b
m_v = (yAf - yA0) / (xAf - xA0)
b_v = yAf - m_v * xAf
rEff = rA + rB #radii are added since we are considering the agent path as a thin line
#Check if points (circles) are within line segment (find center of line segment and check if circle is within radius of this point)
segmentMask = np.sqrt( (yB - (yA0+yAf)/2)**2 + (xB - (xA0+xAf)/2)**2 ) < np.sqrt( (yAf - yA0)**2 + (xAf - xA0)**2 ) / 2 + rEff
#Calculate perpendicular distance between line and a point
dist_v = np.abs(-m_v * xB + yB - b_v) / np.sqrt(m_v**2 + 1)
collisionMask = (dist_v < rEff) & segmentMask
#return True if collision is detected
return collisionMask, collisionMask.any()
If you need the location of the collision, you can use the approach detailed on this site, and set the velocity of one of the agents to zero. This approach works well with vector inputs as well: http://twobitcoder.blogspot.com/2010/04/circle-collision-detection.html
Although I think using line-circle intersection, then checking if intersection points are between the endpoints is better and probably cheaper, I would like to add this more visual solution.
I like to think about the problem as a "point in sausage problem" which should work in any number of dimensions without changing the algorithm.
This solution will not find the intersection points.
Here is what I come up with:
(I use "less than", but "less than or equal to" could also be used depending on what we're testing.)
Make sure the Circle_Point is less than Radius distance from the infinite line. (Use favorite method here).
Calculate distance from both Segment_Points to the Circle_Point.
Test if the bigger Circle_Point-Segment_Point distance is less than sqrt(Segment_Length^2+Radius^2).
(This is the distance from a segment-point to a theoretical point, which is radius-distance from the other segment-point and the infinite-line (right-angle). See image.)
3T. If true: Circle_Point is inside sausage.
3F. If false: If the smaller Circle_Point-Segment_Point distance is less than Radius, then Circle_Point is inside sausage.
Image: Thickest line segment is the selected segment, there is no example circle. A bit crude, some pixels misalign slightly.
function boolean pointInSausage(sp1,sp2,r,c) {
if ( !(pointLineDist(c,sp1,sp2) < r) ) {
return false;
}
double a = dist(sp1,c);
double b = dist(sp2,c);
double l;
double s;
if (a>b) {
l = a;
s = b;
} else {
l = b;
s = a;
}
double segLength = dist(sp1,sp2);
if ( l < sqrt(segLength*segLength+r*r) ) {
return true;
}
return s < r;
}
Tell if any issues are found and I'll edit or retract.
i want to convert x,y,z coordinates to polar coordinates. I am getting (-) in y coordiantes. Can someone explain me why I am getting it. It would be great help.
I am reading these values (xyz , az_elev_r) from a software and can't be changed.I am just not sure of the order of angles( az & elev). Using my code I get -y instead of y. It means there is 180 rotation.My code is:
xyz=[-0.564 3.689 -0.735;
2.011 5.067 -1.031;
-1.181 3.943 -1.825; % Reference values
];
%% az_elev_r-->xyz
az_elev_r=[ 261.30 -11.24 3.80;
291.65 -10.692 5.548;
253.34 -23.897 4.50]; % Also Reference (degree)
az_elev_r(:,1:2)=deg2rad(az_elev_r(:,1:2));
r=az_elev_r(:,3);
az=az_elev_r(:,1);
elev=az_elev_r(:,2);
x=r.*cos(az).*cos(elev)
y=r.*sin(az).*cos(elev)
z=r.*sin(elev)
Your az_elev_r matrix is not consistent with your xyz reference.
>> [az, el, r] = cart2sph(xyz(:,1), xyz(:,2), xyz(:,3));
>> rad2deg(az)
ans =
98.6924675475501
68.3527736950233
106.673911589314
Your answers are consistent with the values returned by the sph2cart function. (Example starts with your original input, before the dec2rad replacement.
>> [x, y, z] = sph2cart(deg2rad(az_elev_r(:,1)), deg2rad(az_elev_r(:,2)), az_elev_r(:,3))
x =
-0.563766229670505
2.01131973806906
-1.17951822049783
y =
-3.68422880893852
-5.06709019311118
-3.94153436658676
z =
-0.740692730942158
-1.02931719412937
-1.82292172199717
Incidentally, you're code will be more readable if you just use the sph2cart function, and work in radians, unless you are trying to understand the conversions for their own sake.
OpenCV has the code for conversion to polar coordinates and back. This conversion is useful for finding object rotation through correlation or otherwise creating object-centred 'rotation-independent' representation of objects. It is useful to visualize each of the polar coordinates as well as their joint image. The images below should be self_explanatory. The polar plot has angle as a horizontal axis and Radius as a vertical axis, so that 4 peaks correspond to the 4 corners of the input image. The code (C++ with OpenCV) is attached.
//================================
// Name : PolarCoord.cpp
// Author : V.Ivanchenko cudassimo#gmail.com
// Version :
// Copyright : Your copyright notice
// Description : Hello World in C++, Ansi-style
//======================================
#include <iostream>
#include "opencv.hpp"
using namespace std;
using namespace cv;
#define VALID(x, y, w, h) ((x)>=0 && (y)>=0 && (x)<(w) && (y)<(h)) // validates index
/*
* 1. Original binary image HxW CV_8U
* |
* |
* V
* 2. Two coordinate Mats HxW CV_32F
* |
* |
* V
* 3. Visualization CV_8U
* a. gray HxW for a single coordinate image
* b. binary Rx360 for two coordinate images
*/
// convert a binary 2D image into two Mats with float coordiantes
void imageToCoord(const Mat& img, Mat& X, Mat& Y, bool centered = true) {
if (img.empty())
return;
int h = img.rows;
int w = img.cols;
X.create(h, w, CV_32F);
Y.create(h, w, CV_32F);
float Cx = w/2.0f;
float Cy = h/2.0f;
for (int i=0; i<h; ++i){
const uchar* img_row = img.ptr<uchar>(i);
float* x_row = X.ptr<float>(i);
float* y_row = Y.ptr<float>(i);
for (int j=0; j<w; ++j) {
if (img_row[j]>0) {
float x = j;
float y = i;
if (centered) {
x-=Cx;
y-=Cy;
}
x_row[j] = x;
y_row[j] = y;
}
} // j
} // i
} //imageToCoord()
// convert a single float ploar coord Mat to a gray image
void polarToImg(const Mat& PolarCoord, Mat& img) {
if (PolarCoord.empty())
return;
int h = PolarCoord.rows;
int w = PolarCoord.cols;
img.create(h, w, CV_8U);
float maxVal = std::numeric_limits<float>::min();
// find maxVal
for (int i=0; i<h; ++i){
const float* x_row = PolarCoord.ptr<float>(i);
for (int j=0; j<w; ++j) {
if (maxVal < x_row[j])
maxVal = x_row[j];
} // j
} // i
// create an image
if (maxVal>0) {
float k = 255.0/maxVal;
for (int i=0; i<h; ++i){
uchar* img_row = img.ptr<uchar>(i);
const float* x_row = PolarCoord.ptr<float>(i);
for (int j=0; j<w; ++j) {
img_row[j] = saturate_cast<uchar>(k*x_row[j]);
}// j
} // i
} // if
} // plarToImg()
// convert two polar coord Mats to a binary image
void polarToImg(const Mat& radius, const Mat& angle, Mat& img) {
if (angle.empty() || radius.empty())
return;
int h = angle.rows;
int w = angle.cols;
assert(radius.cols==w && radius.rows==h);
const int imgH = sqrt(h*h+w*w)+0.5f; // radius
const int imgW = 360; // angle, deg
img.create(imgH, imgW, CV_8U);
// create an image
for (int i=0; i<h; ++i){
const float* ang_row = angle.ptr<float>(i);
const float* r_row = radius.ptr<float>(i);
for (int j=0; j<w; ++j) {
int x = ang_row[j] + 0.5f;
int y = r_row[j] + 0.5f;
if (x>0) {
cout<<x<<endl;
}
if (VALID(x, y, imgW, imgH))
img.at<uchar>(y, x) = 255;
else {
cout<<"Invalid x, y: "<<x<<", "<<y<<endl;
}
}// j
} // i
} // plarToImg()
int main() {
cout << "Cartesian to polar" << endl; // prints "Syntax training in openCV"
const int W=400, H=400;
Mat Minput(H, W, CV_8U);
Minput(Rect(W/4, H/4, W/2, H/2)) = 255;
Mat X, Y, Angle, Radius, Mr, Mang, Mpolar;
// processing
imageToCoord(Minput, X, Y); // extract coordinates
cartToPolar(X, Y, Radius, Angle, true);// convert coordiantes
// visualize
polarToImg(Radius, Mr);
polarToImg(Angle, Mang);
polarToImg(Radius, Angle, Mpolar);
// debug
//cout<<Mpolar<<endl;
namedWindow("input", 0);
namedWindow("angle", 0);
namedWindow("radius", 0);
namedWindow("Polar", 0);
const int winw=200, winh=200;
resizeWindow("input", winw, winh);
resizeWindow("angle", winw, winh);
resizeWindow("radius", winw, winh);
resizeWindow("Polar", 360, (int)sqrt(H*H + W*W));
moveWindow("input", 0, 0);
moveWindow("angle", winw, 0);
moveWindow("radius", 2*winw, 0);
moveWindow("Polar", 3*winw, 0);
imshow("input", Minput);
imshow("angle", Mang);
imshow("radius", Mr);
imshow("Polar", Mpolar);
waitKey(-1);
return 0;
}