c++ - distance from given point to given ellipse


Keywords:c++ 


Question: 

I have an ellipse, defined by Center Point, radiusX and radiusY, and I have a Point. I want to find the point on the ellipse that is closest to the given point. In the illustration below, that would be S1.

graph1

Now I already have code, but there is a logical error somewhere in it, and I seem to be unable to find it. I broke the problem down to the following code example:

#include <vector>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <math.h>

using namespace std;

void dostuff();

int main()
{
    dostuff();
    return 0;
}

typedef std::vector<cv::Point> vectorOfCvPoints;

void dostuff()
{

    const double ellipseCenterX = 250;
    const double ellipseCenterY = 250;
    const double ellipseRadiusX = 150;
    const double ellipseRadiusY = 100;

    vectorOfCvPoints datapoints;

    for (int i = 0; i < 360; i+=5)
    {
        double angle = i / 180.0 * CV_PI;
        double x = ellipseRadiusX * cos(angle);
        double y = ellipseRadiusY * sin(angle);
        x *= 1.4;
        y *= 1.4;
        x += ellipseCenterX;
        y += ellipseCenterY;
        datapoints.push_back(cv::Point(x,y));
    }

    cv::Mat drawing = cv::Mat::zeros( 500, 500, CV_8UC1 );

    for (int i = 0; i < datapoints.size(); i++)
    {
        const cv::Point & curPoint = datapoints[i];
        const double curPointX = curPoint.x;
        const double curPointY = curPoint.y * -1; //transform from image coordinates to geometric coordinates

        double angleToEllipseCenter = atan2(curPointY - ellipseCenterY * -1, curPointX - ellipseCenterX); //ellipseCenterY * -1 for transformation to geometric coords (from image coords)

        double nearestEllipseX = ellipseCenterX + ellipseRadiusX * cos(angleToEllipseCenter);
        double nearestEllipseY = ellipseCenterY * -1 + ellipseRadiusY * sin(angleToEllipseCenter); //ellipseCenterY * -1 for transformation to geometric coords (from image coords)


        cv::Point center(ellipseCenterX, ellipseCenterY);
        cv::Size axes(ellipseRadiusX, ellipseRadiusY);
        cv::ellipse(drawing, center, axes, 0, 0, 360, cv::Scalar(255));
        cv::line(drawing, curPoint, cv::Point(nearestEllipseX,nearestEllipseY*-1), cv::Scalar(180));

    }
    cv::namedWindow( "ellipse", CV_WINDOW_AUTOSIZE );
    cv::imshow( "ellipse", drawing );
    cv::waitKey(0);
}

It produces the following image:

snapshot1

You can see that it actually finds "near" points on the ellipse, but it are not the "nearest" points. What I intentionally want is this: (excuse my poor drawing)

snapshot2

would you extent the lines in the last image, they would cross the center of the ellipse, but this is not the case for the lines in the previous image.
I hope you get the picture. Can anyone tell me what I am doing wrong?


6 Answers: 

Consider a bounding circle around the given point (c, d), which passes through the nearest point on the ellipse. From the diagram it is clear that the closest point is such that a line drawn from it to the given point must be perpendicular to the shared tangent of the ellipse and circle. Any other points would be outside the circle and so must be further away from the given point.

enter image description here

So the point you are looking for is not the intersection between the line and the ellipse, but the point (x, y) in the diagram.

Gradient of tangent:

enter image description here

Gradient of line:

enter image description here

Condition for perpedicular lines - product of gradients = -1:

enter image description here

enter image description here

enter image description here

When rearranged and substituted into the equation of your ellipse...

enter image description here

...this will give two nasty quartic (4th-degree polynomial) equations in terms of either x or y. AFAIK there are no general analytical (exact algebraic) methods to solve them. You could try an iterative method - look up the Newton-Raphson iterative root-finding algorithm.

Take a look at this very good paper on the subject:

Sorry for the incomplete answer - I totally blame the laws of mathematics and nature...

EDIT: oops, i seem to have a and b the wrong way round in the diagram xD

 

Here is the code translated to C# implemented from this paper to solve for the ellipse:

Note that this code is untested - if you find any errors let me know.

    //Pseudocode for robustly computing the closest ellipse point and distance to a query point. It
    //is required that e0 >= e1 > 0, y0 >= 0, and y1 >= 0.
    //e0,e1 = ellipse dimension 0 and 1, where 0 is greater and both are positive.
    //y0,y1 = initial point on ellipse axis (center of ellipse is 0,0)
    //x0,x1 = intersection point

    double GetRoot ( double r0 , double z0 , double z1 , double g )
    {
        double n0 = r0*z0;
        double s0 = z1 - 1; 
        double s1 = ( g < 0 ? 0 : Math.Sqrt(n0*n0+z1*z1) - 1 ) ;
        double s = 0;
        for ( int i = 0; i < maxIter; ++i ){
            s = ( s0 + s1 ) / 2 ;
            if ( s == s0 || s == s1 ) {break; }
            double ratio0 = n0 /( s + r0 );
            double ratio1 = z1 /( s + 1 );
            g = ratio0*ratio0 + ratio1*ratio1 - 1 ;
            if (g > 0) {s0 = s;} else if (g < 0) {s1 = s ;} else {break ;}
        }
        return s;
    }
    double DistancePointEllipse( double e0 , double e1 , double y0 , double y1 , out double x0 , out double x1)
    {
        double distance;
        if ( y1 > 0){
            if ( y0 > 0){
                double z0 = y0 / e0; 
                double z1 = y1 / e1; 
                double g = z0*z0+z1*z1 - 1;
                if ( g != 0){
                    double r0 = (e0/e1)*(e0/e1);
                    double sbar = GetRoot(r0 , z0 , z1 , g);
                    x0 = r0 * y0 /( sbar + r0 );
                    x1 = y1 /( sbar + 1 );
                    distance = Math.Sqrt( (x0-y0)*(x0-y0) + (x1-y1)*(x1-y1) );
                    }else{
                        x0 = y0; 
                        x1 = y1;
                        distance = 0;
                    }
                }
                else // y0 == 0
                    x0 = 0 ; x1 = e1 ; distance = Math.Abs( y1 - e1 );
        }else{ // y1 == 0
            double numer0 = e0*y0 , denom0 = e0*e0 - e1*e1;
            if ( numer0 < denom0 ){
                    double xde0 = numer0/denom0;
                    x0 = e0*xde0 ; x1 = e1*Math.Sqrt(1 - xde0*xde0 );
                    distance = Math.Sqrt( (x0-y0)*(x0-y0) + x1*x1 );
                }else{
                    x0 = e0; 
                    x1 = 0; 
                    distance = Math.Abs( y0 - e0 );
            }
        }
        return distance;
    }
 

There is a relatively simple numerical method with better convergence than Newtons Method. I have a blog post about why it works

This implementation works without any trig functions:

def solve(semi_major, semi_minor, p):  
    px = abs(p[0])
    py = abs(p[1])

    tx = 0.707
    ty = 0.707

    a = semi_major
    b = semi_minor

    for x in range(0, 3):
        x = a * tx
        y = b * ty

        ex = (a*a - b*b) * tx**3 / a
        ey = (b*b - a*a) * ty**3 / b

        rx = x - ex
        ry = y - ey

        qx = px - ex
        qy = py - ey

        r = math.hypot(ry, rx)
        q = math.hypot(qy, qx)

        tx = min(1, max(0, (qx * r / q + ex) / a))
        ty = min(1, max(0, (qy * r / q + ey) / b))
        t = math.hypot(ty, tx)
        tx /= t 
        ty /= t 

    return (math.copysign(a * tx, p[0]), math.copysign(b * ty, p[1]))

Convergence

Credit to Adrian Stephens for the Trig-Free Optimization.

 

The following python code implements the equations described at "Distance from a Point to an Ellipse" and uses newton's method to find the roots and from that the closest point on the ellipse to the point.

Unfortunately, as can be seen from the example, it seems to only be accurate outside the ellipse. Within the ellipse weird things happen.

from math import sin, cos, atan2, pi, fabs


def ellipe_tan_dot(rx, ry, px, py, theta):
    '''Dot product of the equation of the line formed by the point
    with another point on the ellipse's boundary and the tangent of the ellipse
    at that point on the boundary.
    '''
    return ((rx ** 2 - ry ** 2) * cos(theta) * sin(theta) -
            px * rx * sin(theta) + py * ry * cos(theta))


def ellipe_tan_dot_derivative(rx, ry, px, py, theta):
    '''The derivative of ellipe_tan_dot.
    '''
    return ((rx ** 2 - ry ** 2) * (cos(theta) ** 2 - sin(theta) ** 2) -
            px * rx * cos(theta) - py * ry * sin(theta))


def estimate_distance(x, y, rx, ry, x0=0, y0=0, angle=0, error=1e-5):
    '''Given a point (x, y), and an ellipse with major - minor axis (rx, ry),
    its center at (x0, y0), and with a counter clockwise rotation of
    `angle` degrees, will return the distance between the ellipse and the
    closest point on the ellipses boundary.
    '''
    x -= x0
    y -= y0
    if angle:
        # rotate the points onto an ellipse whose rx, and ry lay on the x, y
        # axis
        angle = -pi / 180. * angle
        x, y = x * cos(angle) - y * sin(angle), x * sin(angle) + y * cos(angle)

    theta = atan2(rx * y, ry * x)
    while fabs(ellipe_tan_dot(rx, ry, x, y, theta)) > error:
        theta -= ellipe_tan_dot(
            rx, ry, x, y, theta) / \
            ellipe_tan_dot_derivative(rx, ry, x, y, theta)

    px, py = rx * cos(theta), ry * sin(theta)
    return ((x - px) ** 2 + (y - py) ** 2) ** .5

Here's an example:

rx, ry = 12, 35  # major, minor ellipse axis
x0 = y0 = 50  # center point of the ellipse
angle = 45  # ellipse's rotation counter clockwise
sx, sy = s = 100, 100  # size of the canvas background

dist = np.zeros(s)
for x in range(sx):
    for y in range(sy):
        dist[x, y] = estimate_distance(x, y, rx, ry, x0, y0, angle)

plt.imshow(dist.T, extent=(0, sx, 0, sy), origin="lower")
plt.colorbar()
ax = plt.gca()
ellipse = Ellipse(xy=(x0, y0), width=2 * rx, height=2 * ry, angle=angle,
                  edgecolor='r', fc='None', linestyle='dashed')
ax.add_patch(ellipse)
plt.show()

Which generates rotated ellipse an ellipse and the distance from the boundary of the ellipse as a heat map. As can be seen, at the boundary the distance is zero (deep blue).

 

You just need to calculate the intersection of the line [P1,P0] to your elipse which is S1.

If the line equeation is:

enter image description here

and the elipse equesion is:

elipse equesion

than the values of S1 will be:

enter image description here

Now you just need to calculate the distance between S1 to P1 , the formula (for A,B points) is: distance

 

Given an ellipse E in parametric form and a point P

eqn

the square of the distance between P and E(t) is

eqn

The minimum must satisfy

eqn

Using the trigonometric identities

eqn

and substituting

enter image description here

yields the following quartic equation:

enter image description here

Here's an example C function that solves the quartic directly and computes sin(t) and cos(t) for the nearest point on the ellipse:

void nearest(double a, double b, double x, double y, double *ecos_ret, double *esin_ret) {
    double ax = fabs(a*x);
    double by = fabs(b*y);
    double r  = b*b - a*a;
    double c, d;
    int switched = 0;

    if (ax <= by) {
        if (by == 0) {
            if (r >= 0) { *ecos_ret = 1; *esin_ret = 0; }
            else        { *ecos_ret = 0; *esin_ret = 1; }
            return;
        }
        c = (ax - r) / by;
        d = (ax + r) / by;
    } else {
        c = (by + r) / ax;
        d = (by - r) / ax;
        switched = 1;
    }

    double cc = c*c;
    double D0 = 12*(c*d + 1);      // *-4
    double D1 = 54*(d*d - cc);     // *4
    double D  = D1*D1 + D0*D0*D0;  // *16

    double St;
    if (D < 0) {
        double t = sqrt(-D0);             // *2
        double phi = acos(D1 / (t*t*t));
        St = 2*t*cos((1.0/3)*phi);        // *2
    } else {
        double Q = cbrt(D1 + sqrt(D));    // *2
        St = Q - D0 / Q;                  // *2
    }

    double p    = 3*cc;                          // *-2
    double SS   = (1.0/3)*(p + St);              // *4
    double S    = sqrt(SS);                      // *2
    double q    = 2*cc*c + 4*d;                  // *2
    double l    = sqrt(p - SS + q / S) - S - c;  // *2
    double ll   = l*l;                           // *4
    double ll4  = ll + 4;                        // *4
    double esin = (4*l)    / ll4;
    double ecos = (4 - ll) / ll4;

    if (switched) {
        double t = esin;
        esin = ecos;
        ecos = t;
    }

    *ecos_ret = copysign(ecos, a*x);
    *esin_ret = copysign(esin, b*y);
}

Try it online!