/*
* Copyright (C) 2011-2016, 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.geometry;
import georegression.geometry.algs.TangentLinesTwoEllipses_F64;
import georegression.misc.GrlConstants;
import georegression.struct.point.Point2D_F64;
import georegression.struct.point.Vector2D_F64;
import georegression.struct.shapes.EllipseQuadratic_F64;
import georegression.struct.shapes.EllipseRotated_F64;
/**
* Functions for extracting information from ellipses and converting between different ellipse formats.
*
* @author Peter Abeles
*/
public class UtilEllipse_F64 {
/**
* <p>
* Convert from quadratic to rotated formats. Equations taken from [1].
* </p>
*
* <p>
* [1] David Eberly "Information About Ellipses", Geometric Tools, LLC 2011
* </p>
*
* @param input Input in quadratic format.
* @param output (Optional) Storage for converted format. Can be null.
* @return Ellipse in rotated format.
*/
public static EllipseRotated_F64 convert( EllipseQuadratic_F64 input , EllipseRotated_F64 output ) {
if( output == null )
output = new EllipseRotated_F64();
double a11 = input.a;
double a12 = input.b;
double a22 = input.c;
double b1 = 2*input.d;
double b2 = 2*input.e;
double c = input.f;
output.center.x = (a22*b1-a12*b2)/(2*(a12*a12 - a11*a22));
output.center.y = (a11*b2-a12*b1)/(2*(a12*a12 - a11*a22));
double k1 = output.center.x;
double k2 = output.center.y;
double mu = 1.0/(a11*k1*k1 + 2*a12*k1*k2 + a22*k2*k2 - c);
double m11 = mu*a11;
double m12 = mu*a12;
double m22 = mu*a22;
double inner = Math.sqrt((m11-m22)*(m11-m22) + 4*m12*m12);
double l1 = ((m11+m22) + inner)/2.0;
double l2 = ((m11+m22) - inner)/2.0;
output.b = 1/(double)Math.sqrt(l1);
output.a = 1/(double)Math.sqrt(l2);
// direction of minor axis
double dx,dy;
if( m11 >= m22 ) {
dx = l1-m22;
dy = m12;
} else {
dx = m12;
dy = l1-m11;
}
// direction of major axis
output.phi = Math.atan2(-dx,dy);
if( output.phi < -GrlConstants.PId2 ) {
output.phi += (double)Math.PI;
} else if( output.phi > GrlConstants.PId2 ) {
output.phi -= (double)Math.PI;
}
return output;
}
/**
* Convert from rotated to quadratic.
*
* @param input Input rotated format.
* @param output (Optional) Storage for quadratic format. Can be null.
* @return Ellipse in quadratic format.
*/
public static EllipseQuadratic_F64 convert( EllipseRotated_F64 input , EllipseQuadratic_F64 output ) {
if( output == null )
output = new EllipseQuadratic_F64();
double x0 = input.center.x;
double y0 = input.center.y;
double a = input.a;
double b = input.b;
double phi = input.phi;
double cphi = Math.cos(phi);
double sphi = Math.sin(phi);
double cphi2 = cphi*cphi;
double sphi2 = sphi*sphi;
double a2 = a*a;
double b2 = b*b;
double x02 = x0*x0;
double y02 = y0*y0;
// TODO simplfy using more trig identities
output.a = cphi2/a2 + sphi2/b2;
output.b = sphi*cphi/a2 - sphi*cphi/b2;
output.c = sphi2/a2 + cphi2/b2;
output.d = -x0*cphi2/a2 - y0*sphi*cphi/a2 - x0*sphi2/b2 + y0*sphi*cphi/b2;
output.e = -x0*sphi*cphi/a2 - y0*sphi2/a2 + x0*sphi*cphi/b2 - y0*cphi2/b2;
output.f = x02*cphi2/a2 + 2*x0*y0*sphi*cphi/a2 + y02*sphi2/a2 +
x02*sphi2/b2 - 2*x0*y0*sphi*cphi/b2 + y02*cphi2/b2 - 1;
return output;
}
/**
* Computes the value of the quadratic ellipse function at point (x,y). Should equal 0 if the point
* is along the ellipse.
*
* @param x x-coordinate
* @param y y-coordinate
* @param ellipse Ellipse equation being evaluated.
* @return value of ellipse equation at point (x,y)
*/
public static double evaluate( double x , double y , EllipseQuadratic_F64 ellipse ) {
return ellipse.a*x*x + 2*ellipse.b*x*y + ellipse.c*y*y + 2*ellipse.d*x + 2*ellipse.e*y + ellipse.f;
}
/**
* Computes the value of the quadratic ellipse function at point (x,y). Should equal 1 if the point is on the
* ellipse.
* @param x x-coordinate
* @param y y-coordinate
* @param ellipse Ellipse equation being evaluated.
* @return value of ellipse equation at point (x,y)
*/
public static double evaluate( double x , double y , EllipseRotated_F64 ellipse ) {
double cphi = Math.cos(ellipse.phi);
double sphi = Math.sin(ellipse.phi);
x -= ellipse.center.x;
y -= ellipse.center.y;
double left = (x*cphi + y*sphi);
double right = (-x*sphi + y*cphi);
double ll = left/ellipse.a;
double rr = right/ellipse.b;
return ll*ll + rr*rr;
}
/**
* Computes the point on the ellipse at location 't', where t is an angle in radians
*
* @param t An angle in radians from 0 to 2*PI
* @param ellipse Ellipse
* @param output (Optional) point on the ellipse . Can be null.
* @return Point on the ellipse
*/
public static Point2D_F64 computePoint( double t , EllipseRotated_F64 ellipse , Point2D_F64 output ) {
if( output == null )
output = new Point2D_F64();
double ct = Math.cos(t);
double st = Math.sin(t);
double cphi = Math.cos(ellipse.phi);
double sphi = Math.sin(ellipse.phi);
// coordinate in ellipse frame
double x = ellipse.a*ct;
double y = ellipse.b*st;
// put into global frame
output.x = ellipse.center.x + x*cphi - y*sphi;
output.y = ellipse.center.y + x*sphi + y*cphi;
return output;
}
/**
* Computes the value of 't' used to specify a point's location
*
* @param p Point on the ellipse
* @param ellipse Ellipse
* @return Angle from -pi to pi
*/
public static double computeAngle( Point2D_F64 p , EllipseRotated_F64 ellipse ) {
// put point into ellipse's reference frame
double ce = Math.cos(ellipse.phi);
double se = Math.sin(ellipse.phi);
// world into ellipse frame
double xc = p.x - ellipse.center.x;
double yc = p.y - ellipse.center.y;
double x = ce*xc + se*yc;
double y = -se*xc + ce*yc;
return Math.atan2( y/ellipse.b , x/ellipse.a );
}
/**
* Computes the tangent to the ellipse at the specified location
*
* @param t Location on the ellipse. Radians
* @param ellipse Ellipse equation
* @param output Optional storage for tangent
* @return The tangent
*/
public static Vector2D_F64 computeTangent( double t ,
EllipseRotated_F64 ellipse ,
Vector2D_F64 output ) {
if( output == null )
output = new Vector2D_F64();
double ct = Math.cos(t);
double st = Math.sin(t);
double cphi = Math.cos(ellipse.phi);
double sphi = Math.sin(ellipse.phi);
// point in ellipse frame multiplied by b^2 and a^2
double x = ellipse.a*ct*ellipse.b*ellipse.b;
double y = ellipse.b*st*ellipse.a*ellipse.a;
// rotate vector normal into world frame
double rx = x*cphi - y*sphi;
double ry = x*sphi + y*cphi;
// normalize and change into tangent
double r = Math.sqrt(rx*rx + ry*ry);
output.x = -ry/r;
output.y = rx/r;
return output;
}
/**
* <p>Finds two points on the ellipse that in combination with point 'pt' each define
* a line that is tangent to the ellipse.</p>
*
* Notes:<br>
* Point 'pt' is assumed to be outside of the ellipse.
*
* @param pt Point which the lines will pass though
* @param ellipse The ellipse which the lines will be tangent to
* @param tangentA (output) Point on the ellipse where tangent line A hits it
* @param tangentB (output) Point on the ellipse where tangent line B hits it
*/
public static boolean tangentLines(Point2D_F64 pt , EllipseRotated_F64 ellipse ,
Point2D_F64 tangentA , Point2D_F64 tangentB )
{
// Derivation:
// Compute the tangent at only point along the ellipse by computing dy/dx
// x*b^2/(y*a^2) or - x*b^2/(y*a^2) are the possible solutions for the tangent
// The slope of the line and the gradient are the same, so this is true:
// y - y' -x*b^2
// ------- = -------
// x - x' y*a^2
//
// (x,y) is point on ellipse, (x',y') is pt that lines pass through
//
// that becomes
// y^2*a^2 + x^2*b^2 = x'*x*b^2 + y'*y*a^2
// use the equation for the ellipse (centered and aligned at origin)
// a^2*b^2 = x'*x*b^2 + y'*y*a^2
//
// solve for y
// plug into ellipse equation
// solve for x, which is a quadratic equation
// translate and rotate into ellipse reference frame
double cphi = Math.cos(ellipse.phi);
double sphi = Math.sin(ellipse.phi);
double tmpx = pt.x - ellipse.center.x;
double tmpy = pt.y - ellipse.center.y;
double xt = tmpx*cphi + tmpy*sphi;
double yt = -tmpx*sphi + tmpy*cphi;
// solve
double a2 = ellipse.a*ellipse.a;
double b2 = ellipse.b*ellipse.b;
// quadratic equation for the two variants.
// solving for x
double aa0 = yt*yt/b2 + xt*xt/a2;
double bb0 = -2.0*xt;
double cc0 = a2*(1.0-yt*yt/b2);
double descriminant0 = bb0*bb0 - 4.0*aa0*cc0;
// solving for y
double aa1 = xt*xt/a2 + yt*yt/b2;
double bb1 = -2.0*yt;
double cc1 = b2*(1.0-xt*xt/a2);
double descriminant1 = bb1*bb1 - 4.0*aa1*cc1;
double x0,y0, x1,y1;
if( descriminant0 < 0 && descriminant1 < 0 ) {
return false;
} else if( descriminant0 > descriminant1 ) {
if( yt == 0 )
return false;
double right = Math.sqrt(descriminant0);
x0 = (-bb0 + right)/(2.0*aa0);
x1 = (-bb0 - right)/(2.0*aa0);
y0 = b2/yt - xt*x0*b2/(yt*a2);
y1 = b2/yt - xt*x1*b2/(yt*a2);
} else {
if( xt == 0 )
return false;
double right = Math.sqrt(descriminant1);
y0 = (-bb1 + right)/(2.0*aa1);
y1 = (-bb1 - right)/(2.0*aa1);
x0 = a2/xt - yt*y0*a2/(xt*b2);
x1 = a2/xt - yt*y1*a2/(xt*b2);
}
// convert the lines back into world space
tangentA.x = x0*cphi - y0*sphi + ellipse.center.x;
tangentA.y = x0*sphi + y0*cphi + ellipse.center.y;
tangentB.x = x1*cphi - y1*sphi + ellipse.center.x;
tangentB.y = x1*sphi + y1*cphi + ellipse.center.y;
return true;
}
/**
* <p>Finds four lines which are tangent to both ellipses. Both ellipses must not intersect. Line 0
* and line 3 will not intersect the line joining the center of the two ellipses while line 1 and 2 will.</p>
* @see TangentLinesTwoEllipses_F64
*
* @param ellipseA (Input) First ellipse
* @param ellipseB (Input) Second ellipse
* @param tangentA0 (Output) Point on ellipseA in which tangent line0 passes through
* @param tangentA1 (Output) Point on ellipseA in which tangent line1 passes through
* @param tangentA2 (Output) Point on ellipseA in which tangent line2 passes through
* @param tangentA3 (Output) Point on ellipseA in which tangent line3 passes through
* @param tangentB0 (Output) Point on ellipseB in which tangent line0 passes through
* @param tangentB1 (Output) Point on ellipseB in which tangent line1 passes through
* @param tangentB2 (Output) Point on ellipseB in which tangent line2 passes through
* @param tangentB3 (Output) Point on ellipseB in which tangent line3 passes through
*
* @return true if a solution was found or false if it failed
*/
public static boolean tangentLines( EllipseRotated_F64 ellipseA , EllipseRotated_F64 ellipseB ,
Point2D_F64 tangentA0 , Point2D_F64 tangentA1 ,
Point2D_F64 tangentA2 , Point2D_F64 tangentA3 ,
Point2D_F64 tangentB0 , Point2D_F64 tangentB1 ,
Point2D_F64 tangentB2 , Point2D_F64 tangentB3 )
{
TangentLinesTwoEllipses_F64 alg = new TangentLinesTwoEllipses_F64(GrlConstants.DOUBLE_TEST_TOL,10);
return alg.process(ellipseA, ellipseB,
tangentA0, tangentA1, tangentA2, tangentA3,
tangentB0, tangentB1, tangentB2, tangentB3) ;
}
}