Transform screen coordinates to model coordinates - mouse

I've got some sort of newbie question.
In my application (processingjs) i use scale() and translate() to allow the user to zoom and scroll through the scene. As long as i keep the scale set to 1.0 i've got no issues. BUT whenever i use the scale (i.e. scale(0.5)) i'm lost...
I need the mouseX and mouseY translated to the scene coordinates, which i use to determine the mouseOver state of the object I draw on the scene.
Can anybody help me how to translate these coordinates?
Thanks in advance!
/Richard

Unfortunately for me this required a code modification. I'll look at submitting this to the Processing.JS code repository at some point, but here's what I did.
First, you'll want to use modelX() and modelY() to get the coordinates of the mouse in world view. That will look like this:
float model_x = modelX(mouseX, mouseY);
float model_y = modelY(mouseX, mouseY);
Unfortunately Processing.JS doesn't seem to calculate the modelX() and modelY() values correctly in a 2D environment. To correct that I changed the functions to be as follows. Note the test for mv.length == 16 and the section at the end for 2D:
p.modelX = function(x, y, z) {
var mv = modelView.array();
if (mv.length == 16) {
var ci = cameraInv.array();
var ax = mv[0] * x + mv[1] * y + mv[2] * z + mv[3];
var ay = mv[4] * x + mv[5] * y + mv[6] * z + mv[7];
var az = mv[8] * x + mv[9] * y + mv[10] * z + mv[11];
var aw = mv[12] * x + mv[13] * y + mv[14] * z + mv[15];
var ox = 0, ow = 0;
var ox = ci[0] * ax + ci[1] * ay + ci[2] * az + ci[3] * aw;
var ow = ci[12] * ax + ci[13] * ay + ci[14] * az + ci[15] * aw;
return ow !== 0 ? ox / ow : ox
}
// We assume that we're in 2D
var mvi = modelView.get();
// NOTE that the modelViewInv doesn't seem to be correct in this case, so
// having to re-derive the inverse
mvi.invert();
return mvi.multX(x, y);
};
p.modelY = function(x, y, z) {
var mv = modelView.array();
if (mv.length == 16) {
var ci = cameraInv.array();
var ax = mv[0] * x + mv[1] * y + mv[2] * z + mv[3];
var ay = mv[4] * x + mv[5] * y + mv[6] * z + mv[7];
var az = mv[8] * x + mv[9] * y + mv[10] * z + mv[11];
var aw = mv[12] * x + mv[13] * y + mv[14] * z + mv[15];
var oy = ci[4] * ax + ci[5] * ay + ci[6] * az + ci[7] * aw;
var ow = ci[12] * ax + ci[13] * ay + ci[14] * az + ci[15] * aw;
return ow !== 0 ? oy / ow : oy
}
// We assume that we're in 2D
var mvi = modelView.get();
// NOTE that the modelViewInv doesn't seem to be correct in this case, so
// having to re-derive the inverse
mvi.invert();
return mvi.multY(x, y);
};
I hope that helps someone else who is having this problem.

Have you tried another method?
For example, assume that you are in a 2D environment, you can "map" all the frame in a sort of matrix.
Something like this:
int fWidth = 30;
int fHeight = 20;
float objWidth = 10;
float objHeight = 10;
void setup(){
fWidth = 30;
fHeight = 20;
objWidth = 10;
objHeight = 10;
size(fWidth * objWidth, fHeight * objHeight);
}
In this case you will have a 300*200 frame, but divided in 30*20 sections.
This allows you to move in somewhat ordered way your objects.
When you draw an object you have to give his sizes, so you can use objWidth and objHeight.
Here's the deal: you can make a "zoom-method" that edit the value of the object sizes.
In this way you drew a smaller/bigger object without editing any frame property.
This is a simple example because of your inaccurate question.
You can do it [in more complex ways], in a 3D environment too.

Related

How to modify this code to return Geopoint

I would like this code to return a newly constructed geopoint.
I need this,
GeoPoint prjTest=new GeoPoint(vxi+x,vyi+y);
to stick somewhere and return prjTest. I'm new to programming and I don't know well synthax.I tried many things, I can keep guessing for a long time. Please help. Thanks.
public class ProjectileTest
{
public ProjectileTest(float vi, float angle) /** renamed velocity -> vi */
{
//Starting location
double xi = 0, yi = 100;
final double gravity = -9.81;
//timeSlice declares the interval before checking the new location.
double timeSlice = 0.001; /** renamed time -> timeSlice */
double totalTime = 0; /** renamed recordedTime -> totalTime */
double vxi = vi * Math.cos(Math.toRadians(angle)); /** renamed xSpeed -> vxi */
double vyi = vi * Math.sin(Math.toRadians(angle)); /** renamed ySpeed -> vyi */
//This (secondsTillImpact) seems to give a very accurate time whenever the angle is positive.
double secondsTillImpact = Math.sqrt(2 * yi / -(gravity));
/** Not sure I agree. Does this formula take into account the upward motion
* of the projectile along its parabolic arc? My suspicion is that this
* formula only "works" when the initial theta is: 180 <= angle <= 360.
*
* Compare with the result predicted by quadratic(). Discarding the zero
* intercept which can't work for us (i.e. the negative one, because time
* only moves forward) leaves us with an expected result very close to the
* observed result.
*/
double y;
double x;/** Current position along the y-axis */
do {
// x = x + (xSpeed * time);
x = vxi * totalTime; /** Current position along the x-axis */
// y = y + (ySpeed * time);
y = yi + vyi * totalTime + .5 * gravity * (totalTime * totalTime);
// ySpeed = ySpeed + (gravity * time);
double vy = vyi + gravity * totalTime; /** Current velocity of vector y-component */
System.out.println("X: " + round2(x) + " Y: " + round2(y) + " YSpeed: " + round2(vy));
totalTime += timeSlice;
}
while (y > 0);
////////////////////////////++++++++ GeoPoint prjTest=new GeoPoint(vxi+x,vyi+y);
System.out.println("Incorrectly expected seconds: " + secondsTillImpact + "\nResult seconds: " + totalTime);
quadratic((.5 * gravity), vyi, yi);
}
public double round2(double n) {
return (int) (n * 100.0 + 0.5) / 100.0;
}
public void quadratic(double a, double b, double c) {
if (b * b - 4 * a * c < 0) {
System.out.println("No roots in R.");
} else {
double dRoot = Math.sqrt(b * b - 4 * a * c); /** root the discriminant */
double x1 = (-b + dRoot) / (2 * a); /** x-intercept 1 */
double x2 = (-b - dRoot) / (2 * a); /** x-intercept 2 */
System.out.println("x-int one: " + x1 + " x-int two: " + x2);
}
}
}

Flutter : Calculate destination point from current location and bearing

I'm looking to draw a line on a map from current user position with his bearing/heading.
But the line always have the same direction even if I rotate the phone to another direction. I wonder if I miss something in my calculation.
final la1 = userPosition.latitude;
final lo1 = userPosition.longitude;
const r = 6367; // earth radius
const d = 40; // distance
const dist = d / r;
final bearing = vector.radians(direction.value);
final la2 =
asin(sin(la1) * cos(dist) + cos(la1) * sin(dist) * cos(bearing));
final lo2 = lo1 +
atan2(sin(bearing) * sin(dist) * cos(la1),
cos(dist) - sin(la1) * sin(la2));
return LatLng(la2, lo2);
As you can see in the screenshots bellow, I create 2 lines with a different bearing (check map orientation), but they look the same.
import 'dart:math';
import 'package:google_maps_flutter/google_maps_flutter.dart';
import 'package:vector_math/vector_math.dart';
LatLng createCoord(LatLng coord, double bearing, double distance) {
var radius = 6371e3; //meters
var delta = (distance) / radius; // angular distance in radians
var teta = radians(bearing);
var phi1 = radians(coord.longitude);
var lambda1 = radians(coord.latitude);
var phi2 = asin(sin(phi1) * cos(delta) + cos(phi1) * sin(delta) * cos(teta));
var lambda2 = lambda1 +
atan2(sin(teta) * sin(delta) * cos(phi1),
cos(delta) - sin(phi1) * sin(phi2));
lambda2 = (lambda2 + 3 * pi) % (2 * pi) - pi; // normalise to -180..+180°
return LatLng(degrees(lambda2), degrees(phi2)); //[lon, lat]
}

Wgs to Mercator

We have this function we have been using to convert Wgs coordinates to Mercator. The goal is to have a thin split in latitude towards the poles, and a large one close to the equator to make the 3d match the imagery of the texture, all in an equirectangular projection.
Currently our function looks like this:
WgsToMercator(coord)
{
yRadian = coord.y * Math.PI / 180.0;
sinLat = Math.Sin(yRadian);
y = 0.5 - Math.Log((1 + sinLat) / (1 - sinLat)) / (Math.PI * 4); /// valeur entre 0 et 1, 1 correspondant a -90 degres et 0 a 90 degres
return y;
}
Current result is:
WgsToMercator(90) = 0;
WgsToMercator(45) = 0.36
WgsToMercator(0) = 0.5;
WgsToMercator(-45) = 0.64
WgsToMercator(-90) = 1;
Expected result would be:
WgsToMercator(90) = 0;
WgsToMercator(45) = 0.14
WgsToMercator(0) = 0.5;
WgsToMercator(-45) = 0.86
WgsToMercator(-90) = 1;
My math are rusty and can't find a way to get the expected result. Thanks a lot by advance
I can get approximately your numbers with sine squared, after subtracting your input from 90, taking half of the result, and converting to radians.
Something like (in Octave/Matlab):
function output = WgsToMercator(coord)
rebased_angle = 90 - coord;
half_angle = 0.5 * rebased_angle;
angle_radians = half_angle * (3.1415/180.0);
output = sin(angle_radians)*sin(angle_radians);
end
Which gets:
>> WgsToMercator(90)
ans = 0
>> WgsToMercator(45)
ans = 0.14644
>> WgsToMercator(0)
ans = 0.49998
>> WgsToMercator(-45)
ans = 0.85353
>> WgsToMercator(-90)
ans = 1.00000
In C# it's:
public float WgsToMercator(float coord)
{
var rebasedAngle = 90.0f - coord;
var halfAngle = 0.5f * rebasedAngle;
var angleRadians = halfAngle * (Mathf.PI / 180.0f);
return Mathf.Sin(angleRadians) * Mathf.Sin(angleRadians);
}
:EDIT:
Here's a plot of the function I provided (in blue) and the points you've given (red circles).

iOS OpenGL ES 2.0 Quaternion Rotation Slerp to XYZ Position

I am following the quaternion tutorial: http://www.raywenderlich.com/12667/how-to-rotate-a-3d-object-using-touches-with-opengl and am trying to rotate a globe to some XYZ location. I have an initial quaternion and generate a random XYZ location on the surface of the globe. I pass that XYZ location into the following function. The idea was to generate a lookAt vector with GLKMatrix4MakeLookAt and define the end Quaternion for the slerp step from the lookAt matrix.
- (void)rotateToLocationX:(float)x andY:(float)y andZ:(float)z {
// Turn on the interpolation for smooth rotation
_slerping = YES; // Begin auto rotating to this location
_slerpCur = 0;
_slerpMax = 1.0;
_slerpStart = _quat;
// The eye location is defined by the look at location multiplied by this modifier
float modifier = 1.0;
// Create a look at vector for which we will create a GLK4Matrix from
float xEye = x;
float yEye = y;
float zEye = z;
//NSLog(#"%f %f %f %f %f %f",xEye, yEye, zEye, x, y, z);
_currentSatelliteLocation = GLKMatrix4MakeLookAt(xEye, yEye, zEye, 0, 0, 0, 0, 1, 0);
_currentSatelliteLocation = GLKMatrix4Multiply(_currentSatelliteLocation,self.effect.transform.modelviewMatrix);
// Turn our 4x4 matrix into a quat and use it to mark the end point of our interpolation
//_currentSatelliteLocation = GLKMatrix4Translate(_currentSatelliteLocation, 0.0f, 0.0f, GLOBAL_EARTH_Z_LOCATION);
_slerpEnd = GLKQuaternionMakeWithMatrix4(_currentSatelliteLocation);
// Print info on the quat
GLKVector3 vec = GLKQuaternionAxis(_slerpEnd);
float angle = GLKQuaternionAngle(_slerpEnd);
//NSLog(#"%f %f %f %f",vec.x,vec.y,vec.z,angle);
NSLog(#"Quat end:");
[self printMatrix:_currentSatelliteLocation];
//[self printMatrix:self.effect.transform.modelviewMatrix];
}
The interpolation works, I get a smooth rotation, however the ending location is never the XYZ I input - I know this because my globe is a sphere and I am calculating XYZ from Lat Lon. I want to look directly down the 'lookAt' vector toward the center of the earth from that lat/lon location on the surface of the globe after the rotation. I think it may have something to do with the up vector but I've tried everything that made sense.
What am I doing wrong - How can I define a final quaternion that when I finish rotating, looks down a vector to the XYZ on the surface of the globe? Thanks!
Is the following your meaning:
Your globe center is (0, 0, 0), radius is R, the start position is (0, 0, R), your final position is (0, R, 0), so rotate the globe 90 degrees around X-asix?
If so, just set lookat function eye position to your final position, the look at parameters to the globe center.
m_target.x = 0.0f;
m_target.y = 0.0f;
m_target.z = 1.0f;
m_right.x = 1.0f;
m_right.y = 0.0f;
m_right.z = 0.0f;
m_up.x = 0.0f;
m_up.y = 1.0f;
m_up.z = 0.0f;
void CCamera::RotateX( float amount )
{
Point3D target = m_target;
Point3D up = m_up;
amount = amount / 180 * PI;
m_target.x = (cos(PI / 2 - amount) * up.x) + (cos(amount) * target.x);
m_target.y = (cos(PI / 2 - amount) * up.y) + (cos(amount) * target.y);
m_target.z = (cos(PI / 2 - amount) * up.z) + (cos(amount) * target.z);
m_up.x = (cos(amount) * up.x) + (cos(PI / 2 + amount) * target.x);
m_up.y = (cos(amount) * up.y) + (cos(PI / 2 + amount) * target.y);
m_up.z = (cos(amount) * up.z) + (cos(PI / 2 + amount) * target.z);
Normalize(m_target);
Normalize(m_up);
}
void CCamera::RotateY( float amount )
{
Point3D target = m_target;
Point3D right = m_right;
amount = amount / 180 * PI;
m_target.x = (cos(PI / 2 + amount) * right.x) + (cos(amount) * target.x);
m_target.y = (cos(PI / 2 + amount) * right.y) + (cos(amount) * target.y);
m_target.z = (cos(PI / 2 + amount) * right.z) + (cos(amount) * target.z);
m_right.x = (cos(amount) * right.x) + (cos(PI / 2 - amount) * target.x);
m_right.y = (cos(amount) * right.y) + (cos(PI / 2 - amount) * target.y);
m_right.z = (cos(amount) * right.z) + (cos(PI / 2 - amount) * target.z);
Normalize(m_target);
Normalize(m_right);
}
void CCamera::RotateZ( float amount )
{
Point3D right = m_right;
Point3D up = m_up;
amount = amount / 180 * PI;
m_up.x = (cos(amount) * up.x) + (cos(PI / 2 - amount) * right.x);
m_up.y = (cos(amount) * up.y) + (cos(PI / 2 - amount) * right.y);
m_up.z = (cos(amount) * up.z) + (cos(PI / 2 - amount) * right.z);
m_right.x = (cos(PI / 2 + amount) * up.x) + (cos(amount) * right.x);
m_right.y = (cos(PI / 2 + amount) * up.y) + (cos(amount) * right.y);
m_right.z = (cos(PI / 2 + amount) * up.z) + (cos(amount) * right.z);
Normalize(m_right);
Normalize(m_up);
}
void CCamera::Normalize( Point3D &p )
{
float length = sqrt(p.x * p.x + p.y * p.y + p.z * p.z);
if (1 == length || 0 == length)
{
return;
}
float scaleFactor = 1.0 / length;
p.x *= scaleFactor;
p.y *= scaleFactor;
p.z *= scaleFactor;
}
The answer to this question is a combination of the following rotateTo function and a change to the code from Ray's tutorial at ( http://www.raywenderlich.com/12667/how-to-rotate-a-3d-object-using-touches-with-opengl ). As one of the comments on that article says there is an arbitrary factor of 2.0 being multiplied in GLKQuaternion Q_rot = GLKQuaternionMakeWithAngleAndVector3Axis(angle * 2.0, axis);. Remove that "2" and use the following function to create the _slerpEnd - after that the globe will rotate smoothly to XYZ specified.
// Rotate the globe using Slerp interpolation to an XYZ coordinate
- (void)rotateToLocationX:(float)x andY:(float)y andZ:(float)z {
// Turn on the interpolation for smooth rotation
_slerping = YES; // Begin auto rotating to this location
_slerpCur = 0;
_slerpMax = 1.0;
_slerpStart = _quat;
// Create a look at vector for which we will create a GLK4Matrix from
float xEye = x;
float yEye = y;
float zEye = z;
_currentSatelliteLocation = GLKMatrix4MakeLookAt(xEye, yEye, zEye, 0, 0, 0, 0, 1, 0);
// Turn our 4x4 matrix into a quat and use it to mark the end point of our interpolation
_slerpEnd = GLKQuaternionMakeWithMatrix4(_currentSatelliteLocation);
}

intersection of line and circle with different slope [duplicate]

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.