georegression.fitting.curves.ClosestPointEllipseAngle_F64 Maven / Gradle / Ivy

Show more of this group Show more artifacts with this name
Show all versions of georegression Show documentation

GeoRegression is a free Java based geometry library for scientific computing in fields such as robotics and computer vision with a focus on 2D/3D space.

There is a newer version: 0.27.1

Show newest version

/*
 * Copyright (C) 2011-2020, Peter Abeles. All Rights Reserved.
 *
 * This file is part of Geometric Regression Library (GeoRegression).
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

package georegression.fitting.curves;

import georegression.struct.curve.EllipseRotated_F64;
import georegression.struct.point.Point2D_F64;

/**
 * Finds the closest point on an ellipse to a point.  Point is first put into the ellipse's
 * coordinate system.  Then newton's method is used to find the solution.  The following parameterization is used:
 * (x,y) = a*cos(t) + b*sin(t).  For the center a point is arbitrarily selected.
 *
 * @author Peter Abeles
 */
@SuppressWarnings("NullAway.Init")
public class ClosestPointEllipseAngle_F64 {

	// tolerance to test for solution.  Must be this close to zero
	double tol;
	// maximum number of newton steps
	int maxIterations;

	// location of the closest point
	Point2D_F64 closest = new Point2D_F64();

	EllipseRotated_F64 ellipse;
	double ce;
	double se;

	// optimal value of parameterization
	double theta;

	/**
	 * Specifies convergence criteria
	 *
	 * @param tol Convergence tolerance.  Try 1e-8
	 * @param maxIterations Maximum number of iterations.  Try 100
	 */
	public ClosestPointEllipseAngle_F64(double tol, int maxIterations) {
		this.tol = tol;
		this.maxIterations = maxIterations;
	}

	/**
	 * Specifies the ellipse which point distance is going to be found from
	 * @param ellipse Ellipse description
	 */
	public void setEllipse( EllipseRotated_F64 ellipse ) {
		this.ellipse = ellipse;
		ce = Math.cos(ellipse.phi);
		se = Math.sin(ellipse.phi);
	}

	/**
	 * Find the closest point on the ellipse to the specified point.  To get the solution call {@link #getClosest()}
	 *
	 * @param point Point which it is being fit to
	 */
	public void process( Point2D_F64 point ) {
		// put point into ellipse's coordinate system
		double xc = point.x - ellipse.center.x;
		double yc = point.y - ellipse.center.y;
//
		double x =  ce*xc + se*yc;
		double y = -se*xc + ce*yc;

		// initial guess for the angle
		theta = Math.atan2( ellipse.a*y , ellipse.b*x);

		double a2_m_b2 = ellipse.a*ellipse.a - ellipse.b*ellipse.b;

		// use Newton's Method to find the solution
		int i = 0;
		for(; i < maxIterations; i++ ) {
			double c = Math.cos(theta);
			double s = Math.sin(theta);

			double f = a2_m_b2*c*s - x*ellipse.a*s + y*ellipse.b*c;
			if( Math.abs(f) < tol )
				break;

			double d = a2_m_b2*(c*c - s*s) - x*ellipse.a*c - y*ellipse.b*s;

			theta = theta - f/d;
		}

		// compute solution in ellipse coordinate frame
		x = ellipse.a*(double)Math.cos(theta);
		y = ellipse.b*(double)Math.sin(theta);

		// put back into original coordinate system
		closest.x = ce*x - se*y + ellipse.center.x;
		closest.y = se*x + ce*y + ellipse.center.y;
	}

	public Point2D_F64 getClosest() {
		return closest;
	}

	public double getTheta() {
		return theta;
	}
}