/*
 * Copyright (C) 2011-2015, 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.metric;

import georegression.metric.alg.DistancePointTriangle3D_F64;
import georegression.struct.line.LineParametric3D_F64;
import georegression.struct.line.LineSegment3D_F64;
import georegression.struct.plane.PlaneGeneral3D_F64;
import georegression.struct.plane.PlaneNormal3D_F64;
import georegression.struct.point.Point3D_F64;

/**
 * Functions related to finding the closest point(s) on one shape from another shape.
 *
 * @author Peter Abeles
 */
public class ClosestPoint3D_F64 {
	/**
	 * Returns the point which minimizes the distance between the two lines in 3D.  If the
	 * two lines are parallel the result is undefined.
	 *
	 * @param l0  first line. Not modified.
	 * @param l1  second line. Not modified.
	 * @param ret (Optional) Storage for the closest point. If null a new point is declared. Modified.
	 * @return Closest point between two lines.
	 */
	public static Point3D_F64 closestPoint(LineParametric3D_F64 l0,
										   LineParametric3D_F64 l1,
										   Point3D_F64 ret) {
		if( ret == null ) {
			ret = new Point3D_F64();
		}

		ret.x = l0.p.x - l1.p.x;
		ret.y = l0.p.y - l1.p.y;
		ret.z = l0.p.z - l1.p.z;

		// this solution is from: http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline3d/
		double dv01v1 = MiscOps.dot( ret, l1.slope );
		double dv1v0 = MiscOps.dot( l1.slope, l0.slope );
		double dv1v1 = MiscOps.dot( l1.slope, l1.slope );

		double t0 = dv01v1 * dv1v0 - MiscOps.dot( ret, l0.slope ) * dv1v1;
		double bottom = MiscOps.dot( l0.slope, l0.slope ) * dv1v1 - dv1v0 * dv1v0;
		if( bottom == 0 )
			return null;

		t0 /= bottom;

		// ( d1343 + mua d4321 ) / d4343
		double t1 = ( dv01v1 + t0 * dv1v0 ) / dv1v1;

		ret.x = (double) 0.5 * ( ( l0.p.x + t0 * l0.slope.x ) + ( l1.p.x + t1 * l1.slope.x ) );
		ret.y = (double) 0.5 * ( ( l0.p.y + t0 * l0.slope.y ) + ( l1.p.y + t1 * l1.slope.y ) );
		ret.z = (double) 0.5 * ( ( l0.p.z + t0 * l0.slope.z ) + ( l1.p.z + t1 * l1.slope.z ) );

		return ret;
	}

	/**
	 *

	 * Finds the closest point on line lo to l1 and on l1 to l0.  The solution is returned in
	 * 'param' as a value of 't' for each line.
	 *

	 *

	 * point on l0 = l0.a + param[0]*l0.slope

	 * point on l1 = l1.a + param[1]*l1.slope
	 *

	 * @param l0  first line. Not modified.
	 * @param l1  second line. Not modified.
	 * @param param  param[0] for line0 location and param[1] for line1 location.
	 *
	 * @return False if the lines are parallel or true if a solution was found.
	 */
	public static boolean closestPoints(LineParametric3D_F64 l0,
										LineParametric3D_F64 l1,
										double param[])
	{
		double dX = l0.p.x - l1.p.x;
		double dY = l0.p.y - l1.p.y;
		double dZ = l0.p.z - l1.p.z;

		// this solution is from: http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline3d/
		double dv01v1 = MiscOps.dot( dX , dY , dZ, l1.slope );
		double dv1v0 = MiscOps.dot( l1.slope, l0.slope );
		double dv1v1 = MiscOps.dot( l1.slope, l1.slope );

		double t0 = dv01v1 * dv1v0 - MiscOps.dot( dX , dY , dZ, l0.slope ) * dv1v1;
		double bottom = MiscOps.dot( l0.slope, l0.slope ) * dv1v1 - dv1v0 * dv1v0;
		if( bottom == 0 )
			return false;

		t0 /= bottom;

		// ( d1343 + mua d4321 ) / d4343
		double t1 = ( dv01v1 + t0 * dv1v0 ) / dv1v1;

		param[0] = t0;
		param[1] = t1;

		return true;
	}

	/**
	 * Finds the closest point on a line to the specified point.
	 *
	 * @param line Line on which the closest point is being found.  Not modified.
	 * @param pt   The point whose closest point is being looked for.  Not modified.
	 * @param ret  Storage for the solution.  Can be same as instance as 'pt'. If null is passed in a new point is created. Modified.
	 */
	public static Point3D_F64 closestPoint(LineParametric3D_F64 line, Point3D_F64 pt, Point3D_F64 ret)
	{
		if( ret == null ) {
			ret = new Point3D_F64();
		}

		double dx = pt.x - line.p.x;
		double dy = pt.y - line.p.y;
		double dz = pt.z - line.p.z;

		double n2 = line.slope.normSq();

		double d = (line.slope.x*dx + line.slope.y*dy + line.slope.z*dz);

		ret.x = line.p.x + d * line.slope.x / n2;
		ret.y = line.p.y + d * line.slope.y / n2;
		ret.z = line.p.z + d * line.slope.z / n2;

		return ret;
	}

	/**
	 * Finds the closest point on a line to the specified point as a function of distance along the line.  The 3D
	 * coordinate of the point at 'd', the returned value, is P = (x,y,z) + (slope.x,slope.y,slope.z)*d.
	 *
	 * @param line Line on which the closest point is being found.  Not modified.
	 * @param pt   The point whose closest point is being looked for.  Not modified.
	 * @return The location 'd' along the line of the closeset point
	 */
	public static double closestPoint(LineParametric3D_F64 line, Point3D_F64 pt )
	{
		double dx = pt.x - line.p.x;
		double dy = pt.y - line.p.y;
		double dz = pt.z - line.p.z;

		return (line.slope.x*dx + line.slope.y*dy + line.slope.z*dz) / line.slope.normSq();
	}

	/**
	 * Finds the closest point on the plane to the specified point.
	 *
	 * @param plane The plane
	 * @param point The point
	 * @param found (Optional) Storage for the closest point.  If null a new point is declared internally.
	 * @return The closest point
	 */
	public static Point3D_F64 closestPoint( PlaneNormal3D_F64 plane , Point3D_F64 point , Point3D_F64 found ) {
		if( found == null )
			found = new Point3D_F64();

		double A = plane.n.x;
		double B = plane.n.y;
		double C = plane.n.z;
		double D = plane.n.x*plane.p.x + plane.n.y*plane.p.y + plane.n.z*plane.p.z;

		double top = A*point.x + B*point.y + C*point.z - D;

		double n2 = A*A + B*B + C*C;

		found.x = point.x - A*top/n2;
		found.y = point.y - B*top/n2;
		found.z = point.z - C*top/n2;

		return found;
	}

	/**
	 * Finds the closest point on the plane to the specified point.
	 *
	 * @param plane The plane
	 * @param point The point
	 * @param found (Optional) Storage for the closest point.  Can be same as instance as 'pt'. If null a new point is declared internally.
	 * @return The closest point
	 */
	public static Point3D_F64 closestPoint( PlaneGeneral3D_F64 plane , Point3D_F64 point , Point3D_F64 found ) {
		if( found == null )
			found = new Point3D_F64();

		double top = plane.A*point.x + plane.B*point.y + plane.C*point.z - plane.D;

		double n2 = plane.A*plane.A + plane.B*plane.B + plane.C*plane.C;

		found.x = point.x - plane.A*top/n2;
		found.y = point.y - plane.B*top/n2;
		found.z = point.z - plane.C*top/n2;

		return found;
	}

	/**
	 * Finds the closest point on the plane to the origin.
	 *
	 * @param plane The plane
	 * @param found (Optional) Storage for the closest point.  Can be same as instance as 'pt'. If null a new point is declared internally.
	 * @return The closest point
	 */
	public static Point3D_F64 closestPointOrigin( PlaneGeneral3D_F64 plane , Point3D_F64 found ) {
		if( found == null )
			found = new Point3D_F64();

		double n2 = plane.A*plane.A + plane.B*plane.B + plane.C*plane.C;

		found.x = plane.A*plane.D/n2;
		found.y = plane.B*plane.D/n2;
		found.z = plane.C*plane.D/n2;

		return found;
	}

	/**
	 * Finds the closest point on a line segment to the specified point.
	 *
	 * @param line Line on which the closest point is being found.  Not modified.
	 * @param pt   The point whose closest point is being looked for.  Not modified.
	 * @param ret  (Optional) Storage for the solution.  Can be same as instance as 'pt'. If null is passed in a new point is created. Modified.
	 * @return The closest point
	 */
	public static Point3D_F64 closestPoint(LineSegment3D_F64 line, Point3D_F64 pt, Point3D_F64 ret) {
		if( ret == null ) {
			ret = new Point3D_F64();
		}

		double dx = pt.x - line.a.x;
		double dy = pt.y - line.a.y;
		double dz = pt.z - line.a.z;

		double slope_x = line.b.x - line.a.x;
		double slope_y = line.b.y - line.a.y;
		double slope_z = line.b.z - line.a.z;

		double n = (double) Math.sqrt(slope_x*slope_x + slope_y*slope_y + slope_z*slope_z);

		double d = (slope_x*dx + slope_y*dy + slope_z*dz) / n;

		// if it is past the end points just return one of the end points
		if( d <= 0 ) {
			ret.set(line.a);
		} else if( d >= n ) {
			ret.set(line.b);
		} else {
			ret.x = line.a.x + d * slope_x / n;
			ret.y = line.a.y + d * slope_y / n;
			ret.z = line.a.z + d * slope_z / n;
		}

		return ret;
	}

	/**
	 * Find the point which minimizes its distance from the two line segments.
	 *
	 * @param l0 First line.  Not modified.
	 * @param l1 Second line.  Not modified.
	 * @param ret (Optional) Storage for the solution.  Can be same as instance as 'pt'. If null is passed in a new point is created. Modified.
	 * @return The closest point
	 */
	public static Point3D_F64 closestPoint( LineSegment3D_F64 l0 , LineSegment3D_F64 l1 , Point3D_F64 ret ) {
		if( ret == null ) {
			ret = new Point3D_F64();
		}

		ret.x = l0.a.x - l1.a.x;
		ret.y = l0.a.y - l1.a.y;
		ret.z = l0.a.z - l1.a.z;

		double slope0_x = l0.b.x - l0.a.x;
		double slope0_y = l0.b.y - l0.a.y;
		double slope0_z = l0.b.z - l0.a.z;

		double slope1_x = l1.b.x - l1.a.x;
		double slope1_y = l1.b.y - l1.a.y;
		double slope1_z = l1.b.z - l1.a.z;

		// normalize the slopes for easier math
		double n0 = (double) Math.sqrt(slope0_x*slope0_x + slope0_y*slope0_y + slope0_z*slope0_z);
		double n1 = (double) Math.sqrt(slope1_x*slope1_x + slope1_y*slope1_y + slope1_z*slope1_z);

		slope0_x /= n0; slope0_y /= n0; slope0_z /= n0;
		slope1_x /= n1; slope1_y /= n1; slope1_z /= n1;

		// this solution is from: http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline3d/
		double dv01v1 = ret.x*slope1_x +  ret.y*slope1_y +  ret.z*slope1_z;
		double dv01v0 = ret.x*slope0_x +  ret.y*slope0_y +  ret.z*slope0_z;
		double dv1v0 = slope1_x*slope0_x + slope1_y*slope0_y + slope1_z*slope0_z;

		double t0 = dv01v1 * dv1v0 - dv01v0;
		double bottom = 1 - dv1v0 * dv1v0;
		if( bottom == 0 )
			return null;

		t0 /= bottom;

		// restrict it to be on the line
		if( t0 < 0 )
			return closestPoint(l1,l0.a,ret);
		if( t0 > 1 )
			return closestPoint(l1,l0.b,ret);

		// ( d1343 + mua d4321 ) / d4343
		double t1 = ( dv01v1 + t0 * dv1v0 );

		if( t1 < 0 )
			return closestPoint(l0,l1.a,ret);
		if( t1 > 1 )
			return closestPoint(l0,l1.b,ret);

		ret.x = (double) 0.5 * ( ( l0.a.x + t0 * slope0_x ) + ( l1.a.x + t1 * slope1_x ) );
		ret.y = (double) 0.5 * ( ( l0.a.y + t0 * slope0_y ) + ( l1.a.y + t1 * slope1_y ) );
		ret.z = (double) 0.5 * ( ( l0.a.z + t0 * slope0_z ) + ( l1.a.z + t1 * slope1_z ) );

		return ret;
	}

	/**
	 * Closest point from a 3D triangle to a point.
	 *
	 * @see DistancePointTriangle3D_F64
	 *
	 * @param vertexA Vertex in a 3D triangle.
	 * @param vertexB Vertex in a 3D triangle.
	 * @param vertexC Vertex in a 3D triangle.
	 * @param point Point for which the closest point on the triangle is found
	 * @param ret (Optional) Storage for the solution.  If null is passed in a new point is created. Modified.
	 * @return The closest point
	 */
	public static Point3D_F64 closestPoint( Point3D_F64 vertexA, Point3D_F64 vertexB, Point3D_F64 vertexC,
											Point3D_F64 point , Point3D_F64 ret) {

		if( ret == null ) {
			ret = new Point3D_F64();
		}

		DistancePointTriangle3D_F64 alg = new DistancePointTriangle3D_F64();
		alg.setTriangle(vertexA,vertexB,vertexC);

		alg.closestPoint(point,ret);

		return ret;
	}

	/**
	 *

	 * Computes the closest point along the line to the plane as a function of 't':

	 * [x, y, z] = [x_0, y_0, z_0] + t·[slopeX, slopeY, slopZ]
	 *

	 *
	 * If there is no intersection then Double.NaN is returned.
	 *
	 * @param line The line along which the closest point is being found. Not modified.
	 * @param plane Plane being checked for intersection
	 * @return Distance as a function of 't'.  NaN if there is no intersection.
	 */
	public static double closestPointT( LineParametric3D_F64 line , PlaneNormal3D_F64 plane ) {
		double dx = plane.p.x - line.p.x;
		double dy = plane.p.y - line.p.y;
		double dz = plane.p.z - line.p.z;

		double top = dx*plane.n.x + dy*plane.n.y + dz*plane.n.z;
		double bottom = line.slope.dot(plane.n);

		if( bottom == 0 )
			return Double.NaN;

		return top/bottom;
	}
}


/*
 * Copyright (C) 2011-2015, 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.transform.homography;

import georegression.struct.homography.Homography2D_F64;
import georegression.struct.point.Point2D_F64;

/**
 * Applies homography transform to 2D points.
 *
 * @author Peter Abeles
 */
public class HomographyPointOps_F64 {

	/**
	 * Applies a 2D homography transform to the point and stores the results in another
	 * variable. b = H*a, where 'a' is the input/orig point, 'b' is the output/result point, and 'H'
	 * is a homography from a to b.
	 *
	 * @param H Homography transform
	 * @param orig   Original point being transformed. Not modified.
	 * @param result Where the results are stored.  Can be the same as orig. If null a new
	 *               instance is created. Modified.
	 * @return Transformed point.
	 */
	public static Point2D_F64 transform( Homography2D_F64 H, Point2D_F64 orig, Point2D_F64 result ) {

		if( result == null ) {
			result = new Point2D_F64();
		}

		// copy the values so that no errors happen if orig and result are the same instance
		double x = orig.x;
		double y = orig.y;

		double z = H.a31 * x + H.a32 * y + H.a33;

		result.x = (H.a11 * x + H.a12 * y + H.a13)/z;
		result.y = (H.a21 * x + H.a22 * y + H.a23)/z;

		return result;
	}

	/**
	 * Applies a 2D homography transform to the point and stores the results in another
	 * variable.  b = H*a, where 'a' is the input/orig point, 'b' is the output/result point, and 'H'
	 * is a homography from a to b.
	 *
	 * @param H Homography transform
	 * @param x  Original x-coordinate
	 * @param y  Original y-coordinate
	 * @param result Where the results are stored.  Can be the same as orig. If null a new
	 *               instance is created. Modified.
	 * @return Transformed point.
	 */
	public static Point2D_F64 transform( Homography2D_F64 H, double x , double y , Point2D_F64 result ) {

		if( result == null ) {
			result = new Point2D_F64();
		}

		double z = H.a31 * x + H.a32 * y + H.a33;

		result.x = (H.a11 * x + H.a12 * y + H.a13)/z;
		result.y = (H.a21 * x + H.a22 * y + H.a23)/z;

		return result;
	}
}


/*
 * Copyright (C) 2011-2019, 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.transform.se;

import georegression.geometry.GeometryMath_F64;
import georegression.struct.point.Point2D_F64;
import georegression.struct.point.Point3D_F64;
import georegression.struct.point.Point4D_F64;
import georegression.struct.point.Vector3D_F64;
import georegression.struct.se.Se2_F64;
import georegression.struct.se.Se3_F64;
import org.ejml.data.DMatrixRMaj;

import java.util.List;

/**
 * @author Peter Abeles
 */
public class SePointOps_F64 {

	/**
	 * Applies a 2D special euclidean transform to the point and stores the results in another
	 * variable.
	 *
	 * @param se	 The transform.
	 * @param orig   Original point being transformed. Not modified.
	 * @param result Where the results are stored.  Can be the same as orig. If null a new
	 *               instance is created. Modified.
	 * @return Transformed point.
	 */
	public static Point2D_F64 transform( Se2_F64 se, Point2D_F64 orig, Point2D_F64 result ) {

		if( result == null ) {
			result = new Point2D_F64();
		}

		final double c = se.getCosineYaw();
		final double s = se.getSineYaw();

		// copy the values so that no errors happen if orig and result are the same instance
		double x = orig.x;
		double y = orig.y;

		result.x = se.getX() + x * c - y * s;
		result.y = se.getY() + x * s + y * c;

		return result;
	}

	public static Point2D_F64 transform( Se2_F64 se, double x, double y, Point2D_F64 result ) {

		if( result == null ) {
			result = new Point2D_F64();
		}

		final double c = se.getCosineYaw();
		final double s = se.getSineYaw();

		result.x = se.getX() + x * c - y * s;
		result.y = se.getY() + x * s + y * c;

		return result;
	}

	public static Point2D_F64 transformReverse( Se2_F64 se, Point2D_F64 orig, Point2D_F64 result ) {

		if( result == null ) {
			result = new Point2D_F64();
		}

		final double c = se.getCosineYaw();
		final double s = se.getSineYaw();

		// copy the values so that no errors happen if orig and result are the same instance
		double x = orig.x - se.getX();
		double y = orig.y - se.getY();

		result.x = x * c + y * s;
		result.y = -x * s + y * c;

		return result;
	}

	/**
	 * Applies a 2D special euclidean transform to an array of points.
	 *
	 * @param se	 The transform.
	 * @param points Array of points which are to be transformed.  Modified.
	 * @param length The number of elements in the array that are to be processed.
	 */
	public static void transform( Se2_F64 se, Point2D_F64 points[], int length ) {

		double tranX = se.getX();
		double tranY = se.getY();

		final double c = se.getCosineYaw();
		final double s = se.getSineYaw();

		for( int i = 0; i < length; i++ ) {
			Point2D_F64 pt = points[i];

			double x = pt.x;
			double y = pt.y;

			pt.x = tranX + x * c - y * s;
			pt.y = tranY + x * s + y * c;
		}
	}

	/**
	 * Applies a 2D special euclidean transform to a list of points.
	 *
	 * @param se	 The transform.
	 * @param points List of points which are to be transformed.  Modified.
	 */
	public static void transform( Se2_F64 se, List<Point2D_F64> points ) {

		double tranX = se.getX();
		double tranY = se.getY();

		final double c = se.getCosineYaw();
		final double s = se.getSineYaw();

		for( Point2D_F64 pt : points ) {
			double x = pt.x;
			double y = pt.y;

			pt.x = tranX + x * c - y * s;
			pt.y = tranY + x * s + y * c;
		}
	}

	/**
	 * Applies a 3D special euclidean transform to a list of points.
	 *
	 * @param se	 The transform.
	 * @param points List of points which are to be transformed.  Modified.
	 */
	public static void transform( Se3_F64 se, Point3D_F64[] points , int start , int length ) {
		for (int i = 0; i < length; i++) {
			Point3D_F64 p = points[i+start];
			transform(se,p,p);
		}
	}

	/**
	 * Applies a 3D special euclidean transform to a list of points.
	 *
	 * @param se	 The transform.
	 * @param points List of points which are to be transformed.  Modified.
	 */
	public static void transform( Se3_F64 se, List<Point3D_F64> points ) {
		for( Point3D_F64 p : points ) {
			transform(se,p,p);
		}
	}

	/**
	 *
.
	 * Applies the transform specified by SpecialEuclidean to a point.

	 *

	 * p' = R*p + T
	 *

	 *

	 * Both origPt and tranPt can be the same instance.
	 *

	 *
	 * @param se	 SpecialEuclidean transform. Not modified.
	 * @param origPt Original coordinate of the point. Not modified.
	 * @param tranPt Storage for transformed coordinate of the point. Point declared if null.  Modified.
	 * @return Transformed point.
	 */
	public static Point3D_F64 transform( Se3_F64 se, Point3D_F64 origPt, Point3D_F64 tranPt ) {
		if( tranPt == null )
			tranPt = new Point3D_F64();

		DMatrixRMaj R = se.getR();
		Vector3D_F64 T = se.getT();

		GeometryMath_F64.mult( R, origPt, tranPt );
		GeometryMath_F64.add( tranPt, T, tranPt );

		return tranPt;
	}

	/**
	 *
.
	 * Applies the transform specified by SpecialEuclidean to a homogenous point.

	 *

	 * p' = [R t]*p
	 *

	 *

	 * Both origPt and tranPt can be the same instance.
	 *

	 *
	 * @param se	 SpecialEuclidean transform. Not modified.
	 * @param origPt Original coordinate of the point. Not modified.
	 * @param tranPt Storage for transformed coordinate of the point. Point declared if null.  Modified.
	 * @return Transformed point.
	 */
	public static Point3D_F64 transform(Se3_F64 se, Point4D_F64 origPt, Point3D_F64 tranPt ) {
		if( tranPt == null )
			tranPt = new Point3D_F64();

		DMatrixRMaj R = se.getR();
		Vector3D_F64 T = se.getT();

		double P11 = R.data[0], P12 = R.data[1], P13 = R.data[2], P14 = T.x;
		double P21 = R.data[3], P22 = R.data[4], P23 = R.data[5], P24 = T.y;
		double P31 = R.data[6], P32 = R.data[7], P33 = R.data[8], P34 = T.z;

		tranPt.x = P11*origPt.x + P12*origPt.y + P13*origPt.z + P14*origPt.w;
		tranPt.y = P21*origPt.x + P22*origPt.y + P23*origPt.z + P24*origPt.w;
		tranPt.z = P31*origPt.x + P32*origPt.y + P33*origPt.z + P34*origPt.w;

		return tranPt;
	}

	/**
	 *
.
	 * Applies the transform in the reverse direction

	 *

	 * p = RT*(p'-T)
	 *

	 *

	 * Both origPt and tranPt can be the same instance.
	 *

	 *
	 * @param se	 SpecialEuclidean transform.
	 * @param origPt Original coordinate of the point.
	 * @param tranPt Transformed coordinate of the point.
	 */
	public static Point3D_F64 transformReverse( Se3_F64 se, Point3D_F64 origPt, Point3D_F64 tranPt ) {
		if( tranPt == null )
			tranPt = new Point3D_F64();

		DMatrixRMaj R = se.getR();
		Vector3D_F64 T = se.getT();

		GeometryMath_F64.sub( origPt, T, tranPt );
		GeometryMath_F64.multTran( R, tranPt, tranPt );

		return tranPt;
	}
}


/*
 * Copyright (C) 2011-2019, 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.struct.shapes;

import georegression.metric.Area2D_F64;
import georegression.struct.line.LineSegment2D_F64;
import georegression.struct.point.Point2D_F64;

import javax.annotation.Nullable;
import java.io.Serializable;
import java.util.ArrayList;
import java.util.List;

/**
 * A polygon with 4 vertices, a,b,c, and d.  The vertices are in order sequential order of a,b,c,d.
 *
 */
public class Quadrilateral_F64 implements Serializable {
	public Point2D_F64 a;
	public Point2D_F64 b;
	public Point2D_F64 c;
	public Point2D_F64 d;

	public Quadrilateral_F64() {
		a = new Point2D_F64();
		b = new Point2D_F64();
		c = new Point2D_F64();
		d = new Point2D_F64();
	}

	public Quadrilateral_F64( Quadrilateral_F64 quad ) {
		this();
		a.set(quad.a);
		b.set(quad.b);
		c.set(quad.c);
		d.set(quad.d);
	}

	public Quadrilateral_F64( double x0, double y0 , double x1, double y1 ,
							  double x2, double y2 , double x3, double y3 ) {
		a = new Point2D_F64(x0,y0);
		b = new Point2D_F64(x1,y1);
		c = new Point2D_F64(x2,y2);
		d = new Point2D_F64(x3,y3);
	}

	public Quadrilateral_F64(Point2D_F64 a, Point2D_F64 b, Point2D_F64 c, Point2D_F64 d ) {
		this(a,b,c,d,true);
	}

	public Quadrilateral_F64(Point2D_F64 a, Point2D_F64 b, Point2D_F64 c, Point2D_F64 d, boolean copy ) {
		if( copy ) {
			this.a = new Point2D_F64(a);
			this.b = new Point2D_F64(b);
			this.c = new Point2D_F64(c);
			this.d = new Point2D_F64(d);
		} else {
			this.a = a;
			this.b = b;
			this.c = c;
			this.d = d;
		}
	}

	/**
	 * Returns the area of this quadrilateral
	 * @return area
	 */
	public double area() {
		return Area2D_F64.quadrilateral(this);
	}

	public Point2D_F64 get( int index ) {
		switch( index ) {
			case 0: return a;
			case 1: return b;
			case 2: return c;
			case 3: return d;
		}
		throw new IllegalArgumentException("Requested index out of range. "+index);
	}

	public Point2D_F64 getA() {
		return a;
	}

	public void setA(Point2D_F64 a) {
		this.a = a;
	}

	public Point2D_F64 getB() {
		return b;
	}

	public void setB(Point2D_F64 b) {
		this.b = b;
	}

	public Point2D_F64 getC() {
		return c;
	}

	public void setC(Point2D_F64 c) {
		this.c = c;
	}

	public Point2D_F64 getD() {
		return d;
	}

	public void setD(Point2D_F64 d) {
		this.d = d;
	}

	public LineSegment2D_F64 getLine( int which , LineSegment2D_F64 storage ) {
		if( storage == null )
			storage = new LineSegment2D_F64();

		switch( which ) {
			case 0: storage.a.set(a);storage.b.set(b);break;
			case 1: storage.a.set(b);storage.b.set(c);break;
			case 2: storage.a.set(c);storage.b.set(d);break;
			case 3: storage.a.set(d);storage.b.set(a);break;
			default:
				throw new IllegalArgumentException("Requested index out of range. "+which);
		}
		return storage;
	}

	public double getSideLength( int which ) {
		return Math.sqrt(getSideLength2(which));
	}

	public double getSideLength2( int which ) {
		switch( which ) {
			case 0: return a.distance2(b);
			case 1: return b.distance2(c);
			case 2: return c.distance2(d);
			case 3: return d.distance2(a);
			default:
				throw new IllegalArgumentException("Requested index out of range. "+which);
		}
	}

	public void set(Quadrilateral_F64 quad) {
		this.a.set(quad.a);
		this.b.set(quad.b);
		this.c.set(quad.c);
		this.d.set(quad.d);
	}

	/**
	 * Converts the polygon into a list.
	 *
	 * @param storage (Optional) storage to put the vertexes into
	 * @param copy If points will be copied otherwise a reference of points will be returned
	 * @return List of vertexes
	 */
	public List<Point2D_F64> convert(@Nullable List<Point2D_F64> storage , boolean copy ) {
		if( storage == null )
			storage = new ArrayList<>();
		else
			storage.clear();

		if( copy ) {
			for (int i = 0; i < 4; i++) {
				storage.add( get(i).copy() );
			}
		} else {
			for (int i = 0; i < 4; i++) {
				storage.add( get(i) );
			}
		}
		return storage;
	}

	/**
	 * Sets the polygon to be the same as the list. A true copy is created and no references
	 * to points in the list are saved.
	 * @param list List which the polygon will be set to
	 */
	public void set( List<Point2D_F64> list ) {
		if( list.size() != 4 )
			throw new IllegalArgumentException("List must have size of 4");

		a.set(list.get(0));
		b.set(list.get(1));
		c.set(list.get(2));
		d.set(list.get(3));
	}

	public Quadrilateral_F64 copy() {
		return new Quadrilateral_F64(this);
	}

	/**
	 * Returns true if the two quadrilaterals are equal to each other to within tolerance.  Equality is defined
	 * by seeing if the distance between two equivalent vertexes is within tolerance.
	 *
	 * @param quad The second quadrilateral
	 * @param tol Maximum allowed distance between vertexes.
	 * @return true if equals or false if not
	 */
	public boolean isEquals( Quadrilateral_F64 quad , double tol ) {
		tol *= tol;

		if( a.distance2(quad.a) > tol )
			return false;
		if( b.distance2(quad.b) > tol )
			return false;
		if( c.distance2(quad.c) > tol )
			return false;
		return d.distance2(quad.d) <= tol;
	}

	@Override
	public String toString() {
		return getClass().getSimpleName()+"{" +
				"a=" + a +
				", b=" + b +
				", c=" + c +
				", d=" + d +
				'}';
	}
}


/*
 * Copyright (C) 2011-2018, 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.metric.UtilAngle;
import georegression.struct.line.LineGeneral2D_F64;
import georegression.struct.line.LineParametric2D_F64;
import georegression.struct.line.LinePolar2D_F64;
import georegression.struct.line.LineSegment2D_F64;
import georegression.struct.point.Point2D_F64;

/**
 * Various functions related to lines
 *
 * @author Peter Abeles
 */
public class UtilLine2D_F64 {

	/**
	 * Returns the acute angle between the slope of two lines.  Lines do not need to ever
	 * intersect.  Found using the dot product.
	 *
	 * @param a (input) line
	 * @param b (input) line
	 * @return acute angle in radians
	 */
	public static double acuteAngle( LineGeneral2D_F64 a , LineGeneral2D_F64 b ) {
		double la = Math.sqrt(a.A*a.A + a.B*a.B);
		double lb = Math.sqrt(b.A*b.A + b.B*b.B);

		// numerical round off error can cause it to be barely greater than 1, which is outside the allowed
		// domain of acos()
		double value = (a.A*b.A + a.B*b.B)/(la*lb);
		if( value < -1.0 ) value = -1.0;
		else if( value > 1.0 ) value = 1.0;
		return Math.acos(value);
	}

	/**
	 * Returns the acute angle between the slope of two lines and assumes that the lines have
	 * been normalized such that A*A + B*B = 1.  This avoids the need to compute the square root
	 * twice.  Lines do not need to ever intersect.  Found using the dot product.
	 *
	 * @param a (input) normalized line
	 * @param b (input) normalized line
	 * @return acute angle in radians
	 */
	public static double acuteAngleN( LineGeneral2D_F64 a , LineGeneral2D_F64 b ) {
		double value = a.A*b.A + a.B*b.B;
		if( value < -1.0 ) value = -1.0;
		else if( value > 1.0 ) value = 1.0;
		return Math.acos(value);
	}

	/**
	 * Converts a line from polar form to parametric.
	 *
	 * @param src (input) line is polar notation
	 * @param ret (output) line in parametric notation.  If null a new instance will be created.
	 * @return Converted line in parametric notation
	 */
	public static LineParametric2D_F64 convert( LinePolar2D_F64 src , LineParametric2D_F64 ret )
	{
		if( ret == null )
			ret = new LineParametric2D_F64();

		double c = (double) Math.cos(src.angle);
		double s = (double) Math.sin(src.angle);

		ret.p.set(c*src.distance,s*src.distance);
		ret.slope.set(-s,c);

		return ret;
	}

	/**
	 * Converts a line from polar form to general.  After conversion the line will be normalized, e.g. A*A + B*B == 1.
	 *
	 * @param src (input) line is polar notation
	 * @param ret (output) line in general notation.  If null a new instance will be created.
	 * @return Converted line in general notation
	 */
	public static LineGeneral2D_F64 convert( LinePolar2D_F64 src , LineGeneral2D_F64 ret )
	{
		if( ret == null )
			ret = new LineGeneral2D_F64();

		double c = (double) Math.cos(src.angle);
		double s = (double) Math.sin(src.angle);

		ret.A = c;
		ret.B = s;
		ret.C = -src.distance;

		return ret;
	}

	/**
	 * Converts a line from general to polar.
	 *
	 * @param src (input) line is general notation
	 * @param ret (output) line in polar notation.  If null a new instance will be created.
	 * @return Converted line in polar notation
	 */
	public static LinePolar2D_F64 convert( LineGeneral2D_F64 src , LinePolar2D_F64 ret )
	{
		if( ret == null )
			ret = new LinePolar2D_F64();

		double r = Math.sqrt(src.A*src.A + src.B*src.B);

		double sign = src.C < 0 ? -1 : 1;

		ret.angle = Math.atan2(-sign*src.B/r,-sign*src.A/r);
		ret.distance = sign*src.C/r;

		return ret;
	}

	/**
	 * Converts a line segment into a parametric line.  The start point will be 'src.a' and the
	 * direction will be in the direction of 'src.b-src.a'
	 *
	 * @param src (input) line segment
	 * @param ret (output) line in parametric notation.  If null a new instance will be created.
	 * @return Converted line in parametric notation
	 */
	public static LineParametric2D_F64 convert( LineSegment2D_F64 src , LineParametric2D_F64 ret )
	{
		if( ret == null )
			ret = new LineParametric2D_F64();

		ret.p.set(src.a);
		ret.slope.set(src.slopeX(),src.slopeY());

		return ret;
	}

	/**
	 * Converts a line segment into a general line.
	 *
	 * @param src (Input) line segment
	 * @param ret (output) line in general notation. If null a new instance will be created.
	 * @return Line in general notation
	 */
	public static LineGeneral2D_F64 convert( LineSegment2D_F64 src , LineGeneral2D_F64 ret )
	{
		return convert(src.a,src.b,ret);
	}

	/**
	 * Converts a line segment into a general line. Line segment is defined by two points.
	 *
	 * @param a (Input) End point of line segment
	 * @param b (Input) End point of line segment
	 * @param ret (output) line in general notation. If null a new instance will be created.
	 * @return Line in general notation
	 */
	public static LineGeneral2D_F64 convert( Point2D_F64 a , Point2D_F64 b , LineGeneral2D_F64 ret )
	{
		if( ret == null )
			ret = new LineGeneral2D_F64();

		ret.A = a.y - b.y;
		ret.B = b.x - a.x;
		ret.C = -(ret.A*a.x + ret.B*a.y);

		return ret;
	}

	/**
	 * Converts a line segment into a line in parametric format. It will point from a to b. Point a and b must
	 * be unique.
	 *
	 * @param a (Input) End point of line segment
	 * @param b (Input) End point of line segment
	 * @param ret (output) Storage for new line.. If null a new instance will be created.
	 * @return The resulting line
	 */
	public static LineParametric2D_F64 convert( Point2D_F64 a , Point2D_F64 b , LineParametric2D_F64 ret )
	{
		if( ret == null )
			ret = new LineParametric2D_F64();

		ret.p.set(a);
		ret.slope.x = b.x-a.x;
		ret.slope.y = b.y-a.y;

		return ret;
	}

	/**
	 * Converts a line from parametric to polar.
	 *
	 * @param src (Input) line in parametric notation
	 * @param ret (output) line in polar notation. If null a new instance will be created.
	 * @return Line in polar notation
	 */
	public static LinePolar2D_F64 convert( LineParametric2D_F64 src , LinePolar2D_F64 ret )
	{
		if( ret == null )
			ret = new LinePolar2D_F64();

		double top = src.slope.y*src.p.x - src.slope.x*src.p.y;
		ret.distance = top/src.slope.norm();
		ret.angle = Math.atan2(-src.slope.x,src.slope.y);

		if( ret.distance < 0 ) {
			ret.distance = -ret.distance;
			ret.angle = UtilAngle.bound(ret.angle + Math.PI);
		}

		return ret;
	}

	/**
	 * Converts a line from parametric to general
	 *
	 * @param src (input) line in parametric notation.
	 * @param ret (output) line in general notation.  If null a new instance will be created.
	 * @return Converted line in general notation
	 */
	public static LineGeneral2D_F64 convert( LineParametric2D_F64 src , LineGeneral2D_F64 ret ) {
		if( ret == null ) {
			ret = new LineGeneral2D_F64();
		}

		ret.A = -src.slope.y;
		ret.B = src.slope.x;
		ret.C = -ret.A*src.p.x - ret.B*src.p.y;

		return ret;
	}

	/**
	 * Converts a line from general to parametric
	 *
	 * @param src (input) line in general notation.
	 * @param ret (output) line in parametric notation.  If null a new instance will be created.
	 * @return Converted line in parametric notation
	 */
	public static LineParametric2D_F64 convert( LineGeneral2D_F64 src ,  LineParametric2D_F64 ret ) {
		if( ret == null ) {
			ret = new LineParametric2D_F64();
		}

		ret.slope.x = src.B;
		ret.slope.y = -src.A;

		// find a point on the line
		if( Math.abs(src.B) > Math.abs(src.A) ) {
			ret.p.y = -src.C/src.B;
			ret.p.x = 0;
		} else {
			ret.p.x = -src.C/src.A;
			ret.p.y = 0;
		}

		return ret;
	}
}


/*
 * Copyright (C) 2011-2017, 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.metric.ClosestPoint3D_F64;
import georegression.struct.plane.PlaneGeneral3D_F64;
import georegression.struct.plane.PlaneNormal3D_F64;
import georegression.struct.plane.PlaneTangent3D_F64;
import georegression.struct.point.Point2D_F64;
import georegression.struct.point.Point3D_F64;
import georegression.struct.point.Vector3D_F64;
import georegression.struct.se.Se3_F64;
import org.ejml.data.DMatrixRMaj;

/**
 * @author Peter Abeles
 */
public class UtilPlane3D_F64 {

	/**
	 * Converts a plane in normal form into a general equation
	 *
	 * @param input Plane in normal form.
	 * @param output (Optional) Storage for output plane in general form. If null a new instance will be declared.
	 * @return The convert plane in general form.
	 */
	public static PlaneGeneral3D_F64 convert( PlaneNormal3D_F64 input , PlaneGeneral3D_F64 output ) {
		if( output == null )
			output = new PlaneGeneral3D_F64();

		Vector3D_F64 n = input.n;
		Point3D_F64 p = input.p;

		output.A = n.x;
		output.B = n.y;
		output.C = n.z;
		output.D = n.x*p.x + n.y*p.y + n.z*p.z;

		return output;
	}

	/**
	 *

	 * Converts a plane in general form into normal form.  The point on the plane in normal form will be the
	 * closest point to the origin.
	 *

	 *
	 *
NOTE: The normal is not normalized.

	 *
	 * @param input Plane in general form.
	 * @param output (Optional) Storage for output plane in normal form. If null a new instance will be declared.
	 * @return The plane in normal form.
	 */
	public static PlaneNormal3D_F64 convert( PlaneGeneral3D_F64 input , PlaneNormal3D_F64 output ) {

		if( output == null )
			output = new PlaneNormal3D_F64();

		double top = -input.D;
		double n2 = input.A*input.A + input.B*input.B + input.C*input.C;

		output.p.x = -input.A*top/n2;
		output.p.y = -input.B*top/n2;
		output.p.z = -input.C*top/n2;

		output.n.set(input.A,input.B,input.C);

		return output;
	}

	/**
	 * Converts a plane in tangent form into a plane in normal form
	 *
	 * @param input Plane in tangent form.
	 * @param output (Optional) Storage for output plane in normal form. If null a new instance will be declared.
	 * @return The convert plane in general form.
	 */
	public static PlaneNormal3D_F64 convert( PlaneTangent3D_F64 input , PlaneNormal3D_F64 output ) {
		if( output == null )
			output = new PlaneNormal3D_F64();

		// the value of input is a vector normal to the plane and a point on the plane.
		output.n.x = input.x;
		output.n.y = input.y;
		output.n.z = input.z;

		output.p.set(input);

		return output;
	}

	/**
	 * Defines a plane using a 3D rigid body transform. +z is the 3rd column the rotation matrix. The plane's
	 * point is the translation. The plane reference frame is the x-y plane.
	 * @param planeToWorld (Input) SE3 transform from the plane into the "world" frame.
	 * @param output (Output) Plane
	 * @return Plane
	 */
	public static PlaneNormal3D_F64 convert( Se3_F64 planeToWorld , PlaneNormal3D_F64 output ) {
		if( output == null )
			output = new PlaneNormal3D_F64();

		// the value of input is a vector normal to the plane and a point on the plane.
		output.n.x = planeToWorld.R.unsafe_get(0,2);
		output.n.y = planeToWorld.R.unsafe_get(1,2);
		output.n.z = planeToWorld.R.unsafe_get(2,2);

		output.p.set(planeToWorld.T.x,planeToWorld.T.y,planeToWorld.T.z);

		return output;
	}

	/**
	 * Converts the plane into Hessian normal form.   This is done by dividing each coefficient by the
	 * Euclidean norm of (A,B,C).
	 *
	 * @param plane The input plane.  Modified.
	 */
	public static void hessianNormalForm( PlaneGeneral3D_F64 plane ) {
		double n = Math.sqrt(plane.A*plane.A + plane.B*plane.B + plane.C*plane.C);
		plane.A /= n;
		plane.B /= n;
		plane.C /= n;
		plane.D /= n;
	}

	/**
	 * Applies the plane's definition to test to see if a point is one the plane
	 *
	 * @param plane Equation of the plane
	 * @param point Equation of the point.
	 * @return zero if the point is one the plane.
	 */
	public static double evaluate( PlaneGeneral3D_F64 plane , Point3D_F64 point ) {
		return plane.A*point.x + plane.B*point.y + plane.C*point.z - plane.D;
	}

	/**
	 * Applies the plane's definition to test to see if a point is one the plane
	 *
	 * @param plane Equation of the plane
	 * @param point Equation of the point.
	 * @return zero if the point is one the plane.
	 */
	public static double evaluate( PlaneNormal3D_F64 plane , Point3D_F64 point ) {
		double dx = point.x - plane.p.x;
		double dy = point.y - plane.p.y;
		double dz = point.z - plane.p.z;

		return plane.n.x*dx + plane.n.y*dy + plane.n.z*dz;
	}

	/**
	 * There are an infinite number of possible 2D coordinate axises for a plane. This selects one which
	 * will be right handed using
	 * {@link UtilVector3D_F64#perpendicularCanonical(Vector3D_F64, Vector3D_F64)} and a cross product.
	 *
	 * @param normal (Input) The plane's normal or z-axis
	 * @param axisX (Output) Selected x-axis. Normalized
	 * @param axisY (output) Selected y-axis. Normalized
	 */
	public static void selectAxis2D( Vector3D_F64 normal , Vector3D_F64 axisX , Vector3D_F64 axisY ) {
		UtilVector3D_F64.perpendicularCanonical(normal,axisX);
		axisX.normalize();
		axisY.cross(normal,axisX);
		axisY.normalize();
	}

	/**
	 * Projects the point onto the 2D coordinate system specified by the provided x-axis. If the chose of
	 * x-axis is arbitrary {@link UtilVector3D_F64#perpendicularCanonical(Vector3D_F64, Vector3D_F64)} is recommended
	 * as a way to select
	 *
	 * @param pointOnPlane (Input) A point on the plane.
	 * @param axisX (Input) Vector which defines the x-axis
	 * @param axisY (Input) Vector which defines the y-axis
	 * @param A (Input) 3D vector you wish to project. MUST BE ON THE PlANE
	 * @param output (Output) 2D projected vector.
	 */
	public static void point3Dto2D(Point3D_F64 pointOnPlane ,
								   Vector3D_F64 axisX , Vector3D_F64 axisY,
								   Point3D_F64 A , Point2D_F64 output ) {

		double x = A.x - pointOnPlane.x;
		double y = A.y - pointOnPlane.y;
		double z = A.z - pointOnPlane.z;

		output.x = x*axisX.x + y*axisX.y + z*axisX.z;
		output.y = x*axisY.x + y*axisY.y + z*axisY.z;
	}

	/**
	 * Given a point on the plane's 2D coordinate system, convert it back into a 3D point.
	 * @param origin (Input) Point that defines the 2D coordinate system's origin.
	 * @param axisX (Input) axis which defines the 2D x-axis
	 * @param axisY (Input) axis which defines the 2D y-axis
	 * @param A (Input) 2D point that is to be converted
	 * @param output (Output) 3D point
	 */
	public static void point2Dto3D(Point3D_F64 origin ,
								   Vector3D_F64 axisX , Vector3D_F64 axisY,
								   Point2D_F64 A , Point3D_F64 output ) {

		output.x = origin.x + axisX.x*A.x + axisY.y*A.y;
		output.y = origin.y + axisX.y*A.x + axisY.y*A.y;
		output.z = origin.z + axisX.z*A.x + axisY.y*A.y;
	}

	/**
	 * Returns true if the two plane equations are equal to within tolerance.  Planes are converted into
	 * generalized format and normalized to take in account scale ambiguity.
	 *
	 * @param a plane
	 * @param b plane
	 * @param tol Tolerance for equality
	 * @return true if equals and false if not
	 */
	public static boolean equals( PlaneNormal3D_F64 a , PlaneNormal3D_F64 b , double tol ) {
		PlaneGeneral3D_F64 genA = UtilPlane3D_F64.convert(a,null);
		PlaneGeneral3D_F64 genB = UtilPlane3D_F64.convert(b,null);

		double normA = Math.sqrt(genA.A*genA.A + genA.B*genA.B + genA.C*genA.C + genA.D*genA.D);
		double normB = Math.sqrt(genB.A*genB.A + genB.B*genB.B + genB.C*genB.C + genB.D*genB.D);

		genA.A /= normA; genA.B /= normA; genA.C /= normA; genA.D /= normA;
		genB.A /= normB; genB.B /= normB; genB.C /= normB; genB.D /= normB;

		// handle the sign ambiguity by checking both directions.  This is actualy a bit trickier than it would
		// see at first glance.  might be a better way
		int numMatch0 = 0;

		if( Math.abs(genA.A - genB.A) <= tol )
			numMatch0++;
		if( Math.abs(genA.B - genB.B) <= tol )
			numMatch0++;
		if( Math.abs(genA.C - genB.C) <= tol )
			numMatch0++;
		if( Math.abs(genA.D - genB.D) <= tol )
			numMatch0++;

		if( numMatch0 == 4 )
			return true;

		if( Math.abs(genA.A + genB.A) > tol )
			return false;
		if( Math.abs(genA.B + genB.B) > tol )
			return false;
		if( Math.abs(genA.C + genB.C) > tol )
			return false;
		if( Math.abs(genA.D + genB.D) > tol )
			return false;

		return true;
	}

	/**
	 * Creates a transform from the plane's reference frame into world's reference frame.  The z-axis is set to the
	 * plane's normal and the x-axis and y-axis are arbitrarily choosen.  Points which lie along the plane will
	 * lie along its x-y plane.
	 *
	 * @param plane Plane
	 * @param planeToWorld (Optional) storage for the planeToWorld transform.  Can be null.
	 * @return Transform from planeToWorld
	 */
	public static Se3_F64 planeToWorld( PlaneGeneral3D_F64 plane , Se3_F64 planeToWorld ) {
		if( planeToWorld == null )
			planeToWorld = new Se3_F64();

		Vector3D_F64 axisZ = new Vector3D_F64(plane.A,plane.B,plane.C);

		axisZ.normalize();
		Vector3D_F64 axisX = new Vector3D_F64();
		Vector3D_F64 axisY = new Vector3D_F64();
		UtilPlane3D_F64.selectAxis2D(axisZ,axisX,axisY);

		return planeToWorld(plane,axisX,axisY,axisZ,planeToWorld);
	}

	public static Se3_F64 planeToWorld( PlaneGeneral3D_F64 plane ,
										Vector3D_F64 axisX , Vector3D_F64 axisY,Vector3D_F64 axisZ,
										Se3_F64 planeToWorld ) {
		if( planeToWorld == null )
			planeToWorld = new Se3_F64();

		DMatrixRMaj R = planeToWorld.R;
		R.data[0] = axisX.x; R.data[1] = axisY.x; R.data[2] = axisZ.x;
		R.data[3] = axisX.y; R.data[4] = axisY.y; R.data[5] = axisZ.y;
		R.data[6] = axisX.z; R.data[7] = axisY.z; R.data[8] = axisZ.z;

		Point3D_F64 cp = ClosestPoint3D_F64.closestPointOrigin(plane,null);
		planeToWorld.getT().set(cp.x,cp.y,cp.z);

		return planeToWorld;
	}
}


/*
 * Copyright (C) 2011-2018, 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.struct;

/**
 * Describes geometric objects that are composed of N double values.  Where N is the dimension
 * of space the object is contained in.  Points and vectors are two examples of a GeoTuple.  Each
 * value is the value of the object along a dimension in the space it occupies.
 *
 * @author Peter Abeles
 */
public abstract class GeoTuple_F64 <T extends GeoTuple_F64> extends GeoTuple<T> {

	/**
	 * Checks to see if the two GeoTuple have values which are nearly the same.  False is always
	 * returned if the dimension is different.
	 *
	 * @param t   The GeoTuple it is being compared against.
	 * @param tol How similar each element must be for them to be considered identical.
	 * @return if they are identical or not.
	 */
	public boolean isIdentical( T t, double tol ) {
		if( t.getDimension() != getDimension() )
			return false;

		int N = getDimension();
		for( int i = 0; i < N; i++ ) {
			double diff = Math.abs( getIdx( i ) - t.getIdx( i ) );

			if( diff > tol )
				return false;
		}

		return true;
	}

	/**
	 * Generic copy routine.  It is recommended that this be overridden with a faster implementation.
	 *
	 * @return An exact copy of this GeoTuple.
	 */
	@Override
	public T copy() {
		T ret = createNewInstance();

		int N = getDimension();
		for( int i = 0; i < N; i++ ) {
			ret.setIdx( i, getIdx( i ) );
		}

		return ret;
	}

	/**
	 * Computes the  Euclidean norm.
	 *
	 * @return norm.
	 */
	public double norm() {
		return (float) Math.sqrt( normSq() );
	}

	/**
	 * Computes the square of the Euclidean norm.
	 *
	 * @return norm squared.
	 */
	public double normSq() {
		double total = 0;
		int N = getDimension();
		for( int i = 0; i < N; i++ ) {
			double a = getIdx( i );
			total += a * a;
		}

		return total;
	}

	public double distance( T t ) {
		return Math.sqrt( distance2( t ) );
	}

	public double distance2( T t ) {
		if( t.getDimension() != getDimension() )
			throw new IllegalArgumentException( "Dimension of input tuple does not match" );

		double total = 0;
		final int N = getDimension();
		for( int i = 0; i < N; i++ ) {
			double diff = Math.abs( getIdx( i ) - t.getIdx( i ) );

			total += diff * diff;
		}

		return total;
	}

	/**
	 * Returns the value of the tuple along the specified coordinate system axis.
	 *
	 * @param index Which axis in the coordinate system.
	 * @return Its value.
	 */
	public abstract double getIdx(int index );

	public abstract void setIdx(int index, double value );

	@Override
	public boolean equals(Object obj) {
		if( this.getClass() != obj.getClass() )
			return false;
		GeoTuple_F64 p = (GeoTuple_F64)obj;
		final int N = getDimension();

		if( N != p.getDimension() )
			return false;
		for( int i = 0; i < N; i++ ) {
			if( getIdx(i) != p.getIdx(i))
				return false;
		}
		return true;
	}
}


/*
 * Copyright (C) 2011-2015, 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.struct.line.LineParametric3D_F64;
import georegression.struct.line.LineSegment3D_F64;
import georegression.struct.point.Point3D_F64;

/**
 * Various utilty functions related to lines in 3D
 *
 * @author Peter Abeles
 */
public class UtilLine3D_F64 {
	/**
	 * Converts a {@link LineSegment3D_F64} into {@link LineParametric3D_F64}.
	 *
	 * @param line Line segment
	 * @param output Storage for converted line.  If null new line will be declared.
	 * @return Line in parametric format
	 */
	public static LineParametric3D_F64 convert( LineSegment3D_F64 line , LineParametric3D_F64 output ) {
		if( output == null )
			output = new LineParametric3D_F64();

		output.p.set(line.a);
		output.slope.x = line.b.x - line.a.x;
		output.slope.y = line.b.y - line.a.y;
		output.slope.z = line.b.z - line.a.z;

		return output;
	}

	/**
	 * Computes the value of T for a point on the parametric line
	 *
	 * @param line The line
	 * @param pointOnLine Point on a line
	 * @return Value of T for the point
	 */
	public static double computeT( LineParametric3D_F64 line , Point3D_F64 pointOnLine ) {

		double dx = pointOnLine.x - line.p.x;
		double dy = pointOnLine.y - line.p.y;
		double dz = pointOnLine.z - line.p.z;

		double adx = Math.abs(dx);
		double ady = Math.abs(dy);
		double adz = Math.abs(dz);

		double t;

		if( adx > ady ) {
			if( adx > adz ) {
				t = dx/line.slope.x;
			} else {
				t = dz/line.slope.z;
			}
		} else if( ady > adz ) {
			t = dy/line.slope.y;
		}  else {
			t = dz/line.slope.z;
		}
		return t;
	}
}


/*
 * Copyright (C) 2011-2018, 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.struct.curve;

import org.ejml.UtilEjml;

/**
 * Parabola is a specific type of conic that is defined below using 5 coefficients.
 *
 *
(A*x + C*y)2 + D*x + E*y + F = 0

 *
 * @author Peter Abeles
 */
public class ParabolaGeneral_F64 {

	/**
	 * Coefficients.
	 */
	public double A,C,D,E,F;

	public ParabolaGeneral_F64(double a, double c, double d, double e, double f) {
		A = a;
		C = c;
		D = d;
		E = e;
		F = f;
	}

	public ParabolaGeneral_F64( ParabolaGeneral_F64 original ) {
		this.set(original);
	}

	public ParabolaGeneral_F64(){}

	public void set( ParabolaGeneral_F64 original ) {
		this.A = original.A;
		this.C = original.C;
		this.D = original.D;
		this.E = original.E;
		this.F = original.F;
	}

	public void set(double a, double c, double d, double e, double f) {
		A = a;
		C = c;
		D = d;
		E = e;
		F = f;
	}

	public double evaluate(double x , double y ) {
		double inner = A*x + C*y;
		return inner*inner + D*x + E*y +F;
	}

	public void toArray( double[] array ) {
		array[0] = A;
		array[1] = C;
		array[2] = D;
		array[3] = E;
		array[4] = F;
	}

	public void fromArray( double[] array ) {
		A = array[0];
		C = array[1];
		D = array[2];
		E = array[3];
		F = array[4];
	}

	/**
	 * Returns true if any of its parameters have an uncountable number
	 */
	public boolean hasUncountable() {
		return UtilEjml.isUncountable(A) || UtilEjml.isUncountable(C)
				|| UtilEjml.isUncountable(D) || UtilEjml.isUncountable(E) ||
				UtilEjml.isUncountable(F);
	}

	public double relativeScale( ParabolaGeneral_F64 parabola ) {
		double scale = A/parabola.A;
		double max = Math.abs(parabola.A);
		if( max < Math.abs(parabola.C)) {
			max = Math.abs(parabola.C);
			scale = C/parabola.C;
		}
		if( max < Math.abs(parabola.D)) {
			max = Math.abs(parabola.D);
			scale = D/parabola.D;
		}
		if( max < Math.abs(parabola.E)) {
			max = Math.abs(parabola.E);
			scale = E/parabola.E;
		}
		if( max < Math.abs(parabola.F)) {
			max = Math.abs(parabola.F);
			scale = F/parabola.F;
		}

		if( max == 0 )
			scale = 0;
		return scale;
	}

	/**
	 * Determines if they are equivalent up to a scale factor
	 */
	public boolean isEquivalent( ParabolaGeneral_F64 parabola , double tol ) {
		double scale = relativeScale(parabola);

		if( Math.abs(A*scale-parabola.A) > tol )
			return false;
		if( Math.abs(C*scale-parabola.C) > tol )
			return false;
		if( Math.abs(D*scale-parabola.D) > tol )
			return false;
		if( Math.abs(E*scale-parabola.E) > tol )
			return false;
		if( Math.abs(F*scale-parabola.F) > tol )
			return false;

		return true;
	}
}


/*
 * Copyright (C) 2011-2017, 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.line;

import georegression.struct.line.LinePolar2D_F32;
import georegression.struct.line.LinePolar2D_F64;
import georegression.struct.point.Point2D_I32;

import java.util.List;

/**
 * @author Peter Abeles
 */
public class FitLine_I32 {

	/**
	 *

	 * Computes the unweighted best fit line to the set of points using the polar line equation. The solution
	 * is optimal in the Euclidean sense, see [1] for more details.
	 *

	 *
	 *

	 * [1] K. Arras, R. Siegwart, "Feature Extraction and Scene Interpretation for Map-Based Navigation
	 * and Map Building" Proc. SPIE, Mobile Robotics XIII, Vol. 3210, 1997
	 *

	 *
	 * @param points Set of points on the line.
	 * @param start First index in the list
	 * @param length number of variables used in the list
	 * @param ret Storage for the line.  If null a new line will be declared.
	 * @return Best fit line
	 */
	public static LinePolar2D_F32 polar(List<Point2D_I32> points , int start , int length , LinePolar2D_F32 ret ) {
		if( ret == null )
			ret = new LinePolar2D_F32();

		int sumX = 0;
		int sumY = 0;

		final int N = length;
		for( int i = 0; i < N; i++ ) {
			Point2D_I32 p = points.get(start+i);
			sumX += p.x;
			sumY += p.y;
		}
		float meanX = sumX / (float)N;
		float meanY = sumY / (float)N;

		float top = 0;
		float bottom = 0;

		for( int i = 0; i < N; i++ ) {
			Point2D_I32 p = points.get(start+i);
			float dx = meanX - p.x;
			float dy = meanY - p.y;

			top += dx*dy;
			bottom += dy*dy - dx*dx;
		}

		ret.angle = (float)Math.atan2(-2.0f*top , bottom)/2.0f;
		ret.distance = (float)( meanX*Math.cos(ret.angle) + meanY*Math.sin(ret.angle));

		return ret;
	}

	/**
	 *

	 * Computes the unweighted best fit line to the set of points using the polar line equation. The solution
	 * is optimal in the Euclidean sense, see [1] for more details.
	 *

	 *
	 *

	 * [1] K. Arras, R. Siegwart, "Feature Extraction and Scene Interpretation for Map-Based Navigation
	 * and Map Building" Proc. SPIE, Mobile Robotics XIII, Vol. 3210, 1997
	 *

	 *
	 * @param points Set of points on the line.
	 * @param ret Storage for the line.  If null a new line will be declared.
	 * @return Best fit line
	 */
	public static LinePolar2D_F64 polar(List<Point2D_I32> points  , int start , int length ,  LinePolar2D_F64 ret ) {
		if( ret == null )
			ret = new LinePolar2D_F64();

		int sumX = 0;
		int sumY = 0;

		final int N = length;
		for( int i = 0; i < N; i++ ) {
			Point2D_I32 p = points.get(start+i);
			sumX += p.x;
			sumY += p.y;
		}
		double meanX = sumX / (double)N;
		double meanY = sumY / (double)N;

		double top = 0;
		double bottom = 0;

		for( int i = 0; i < N; i++ ) {
			Point2D_I32 p = points.get(start+i);
			double dx = meanX - p.x;
			double dy = meanY - p.y;

			top += dx*dy;
			bottom += dy*dy - dx*dx;
		}

		ret.angle = Math.atan2(-2.0*top , bottom)/2.0;
		ret.distance = ( meanX*Math.cos(ret.angle) + meanY*Math.sin(ret.angle));

		return ret;
	}
}


/*
 * Copyright (C) 2011-2015, 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.struct.shapes;

import georegression.struct.point.Point3D_F64;

import java.io.Serializable;

/**
 * An axis aligned box in 3D that is specified by two points, p0 and p1, the lower and upper extents of the box.
 * Point p0 is less or equal to point p1, 0.x ≤ p1.x, p0.y ≤ p1.y, p0.z ≤ p1.z.
 */
public class Box3D_F64 implements Serializable {

	/**
	 * The lower point/extent.
	 */
	public Point3D_F64 p0 = new Point3D_F64();
	/**
	 * The upper point/extent
	 */
	public Point3D_F64 p1 = new Point3D_F64();

	public Box3D_F64(double x0, double y0, double z0, double x1, double y1, double z1) {
		this.p0.set(x0, y0, z0);
		this.p1.set(x1, y1, z1);
	}

	public Box3D_F64(Box3D_F64 orig) {
		set(orig);
	}

	public void set( Box3D_F64 orig ) {
		set(orig.p0.x,orig.p0.y,orig.p0.z,orig.p1.x,orig.p1.y,orig.p1.z);
	}

	public Box3D_F64() {
	}

	public void set(double x0, double y0, double z0, double x1, double y1, double z1 ) {
		this.p0.set(x0, y0, z0);
		this.p1.set(x1, y1, z1);
	}

	/**
	 * The box's area.  area = lengthX*lengthY*lengthZ
	 *
	 * @return area
	 */
	public double area() {
		return (p1.x-p0.x)*(p1.y-p0.y)*(p1.z-p0.z);
	}

	/**
	 * Length of the box along the x-axis
	 * @return length
	 */
	public double getLengthX() {
		return p1.x-p0.x;
	}

	/**
	 * Length of the box along the y-axis
	 * @return length
	 */
	public double getLengthY() {
		return p1.y-p0.y;
	}

	/**
	 * Length of the box along the z-axis
	 * @return length
	 */
	public double getLengthZ() {
		return p1.z-p0.z;
	}

	/**
	 * Returns the lower point/extend
	 */
	public Point3D_F64 getP0() {
		return p0;
	}

	public void setP1(Point3D_F64 p1) {
		this.p1.set(p1);
	}

	/**
	 * Returns the upper point/extend
	 */
	public Point3D_F64 getP1() {
		return p1;
	}

	public void setP0(Point3D_F64 p0) {
		this.p0.set(p0);
	}

	/**
	 * Computes and return the center of the cube.
	 * @param storage Optional storage for the center.  If null a new instance will be created and returned.
	 * @return The cube's center point
	 */
	public Point3D_F64 center( Point3D_F64 storage ) {
		if( storage == null )
			storage = new Point3D_F64();
		storage.x = (p0.x + p1.x)/2.0;
		storage.y = (p0.y + p1.y)/2.0;
		storage.z = (p0.z + p1.z)/2.0;
		return storage;
	}

	public String toString() {
		return getClass().getSimpleName()+"{ P0( "+ p0.x+" "+ p0.y+" "+ p0.z+" ) P1( "+ p1.x+" "+ p1.y+" "+ p1.z+" ) }";
	}
}


/*
 * Copyright (C) 2011-2015, 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.struct.line.LineSegment2D_I32;

/**
 * @author Peter Abeles
 */
public class UtilLine2D_I32 {

	/**
	 * Computes the acute angle between the two lines.  Does not check for intersection
	 *
	 * @param line0 First line
	 * @param line1 Second line
	 * @return Acute angle in radians
	 */
	public static double acuteAngle( LineSegment2D_I32 line0 , LineSegment2D_I32 line1 ) {
		int dx0 = line0.b.x - line0.a.x;
		int dy0 = line0.b.y - line0.a.y;
		int dx1 = line1.b.x - line1.a.x;
		int dy1 = line1.b.y - line1.a.y;

		double bottom = Math.sqrt(dx0*dx0 + dy0*dy0) * Math.sqrt(dx1*dx1 + dy1*dy1);
		return Math.acos((dx0*dx1 + dy0*dy1)/bottom);
	}
}


/*
 * Copyright (C) 2011-2015, 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.se;

import georegression.fitting.MotionTransformPoint;
import georegression.struct.point.Point2D_F64;
import georegression.struct.point.Point3D_F64;
import georegression.struct.se.Se2_F64;
import georegression.struct.se.Se3_F64;

import java.util.List;

/**
 * Functions related to fitting {@link georegression.struct.se.SpecialEuclidean} transform to a set
 * of corresponding points.
 *
 * @author Peter Abeles
 */
public class FitSpecialEuclideanOps_F64 {

	/**
	 * Creates a {@link MotionTransformPoint} for finding a {@link Se2_F64} from two
	 * sets of 2D points.
	 *
	 * @return {@link MotionTransformPoint}.
	 */
	public static MotionTransformPoint<Se2_F64, Point2D_F64> fitPoints2D() {
		return new MotionSe2PointSVD_F64();
	}

	/**
	 * Creates a {@link MotionTransformPoint} for finding a {@link Se3_F64} from two
	 * sets of 3D points.
	 *
	 * @return {@link MotionTransformPoint}.
	 */
	public static MotionTransformPoint<Se3_F64, Point3D_F64> fitPoints3D() {
		return new MotionSe3PointSVD_F64();
	}

	/**
	 * Given two sets of corresponding points compute the {@link Se2_F64} transform
	 * which minimizes the difference between the two sets of points.
	 *
	 * @param from List of the points original location.
	 * @param to Observed location of the points after a transform has been applied.
	 * @return Found transform.
	 */
	public static Se2_F64 fitPoints2D( List<Point2D_F64> from, List<Point2D_F64> to ) {
		MotionTransformPoint<Se2_F64, Point2D_F64> alg = fitPoints2D();

		alg.process( from, to );

		return alg.getTransformSrcToDst();
	}

	/**
	 * Given two sets of corresponding points compute the {@link Se3_F64} transform
	 * which minimizes the difference between the two sets of points.
	 *
	 * @param from List of the points original location.
	 * @param to Observed location of the points after a transform has been applied.
	 * @return Found transform.
	 */
	public static Se3_F64 fitPoints3D( List<Point3D_F64> from, List<Point3D_F64> to ) {
		MotionTransformPoint<Se3_F64, Point3D_F64> alg = fitPoints3D();

		alg.process( from, to );

		return alg.getTransformSrcToDst();
	}
}


/*
 * Copyright (C) 2011-2018, 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.struct.point.Point3D_F64;
import georegression.struct.point.Point4D_F64;

import java.util.ArrayList;
import java.util.List;
import java.util.Random;

/**
 * @author Peter Abeles
 */
public class UtilPoint4D_F64 {

	/**
	 * Checks to see if the homogenous 3D point lies on the plane at infinity
	 *
	 * @param p (Input) Homogenous point
	 * @param tol (Input) tolerance. Try EPS
	 * @return true if on plane at infinity
	 */
	public static boolean isInfiniteH(Point4D_F64 p , double tol ) {
		double n = Math.sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
		return Math.abs(p.w) <= n*tol;
	}

	/**
	 * Normally distributed random point with the mean (center) specified and each axis having the same
	 * standard deviation.
	 * @param center
	 * @param stdev
	 * @param num
	 * @param rand
	 * @return
	 */
	public static List<Point4D_F64> randomN( Point4D_F64 center , double stdev, int num, Random rand ) {
		List<Point4D_F64> ret = new ArrayList<>();

		for( int i = 0; i < num; i++ ) {
			Point4D_F64 p = new Point4D_F64();
			p.x = center.x + rand.nextGaussian() * stdev;
			p.y = center.y + rand.nextGaussian() * stdev;
			p.z = center.z + rand.nextGaussian() * stdev;
			p.w = center.w + rand.nextGaussian() * stdev;

			ret.add( p );
		}

		return ret;
	}

	/**
	 * Normally distributed homogenous 3D point. w is fixed
	 *
	 */
	public static List<Point4D_F64> randomN( Point3D_F64 center , double w, double stdev, int num, Random rand ) {
		List<Point4D_F64> ret = new ArrayList<>();

		for( int i = 0; i < num; i++ ) {
			Point4D_F64 p = new Point4D_F64();
			p.x = center.x + rand.nextGaussian() * stdev;
			p.y = center.y + rand.nextGaussian() * stdev;
			p.z = center.z + rand.nextGaussian() * stdev;
			p.w = w;

			ret.add( p );
		}

		return ret;
	}

	public static List<Point4D_F64> random(double min, double max, int num, Random rand ) {
		List<Point4D_F64> ret = new ArrayList<>();

		double d = max - min;

		for( int i = 0; i < num; i++ ) {
			Point4D_F64 p = new Point4D_F64();
			p.x = rand.nextDouble() * d + min;
			p.y = rand.nextDouble() * d + min;
			p.z = rand.nextDouble() * d + min;
			p.w = rand.nextDouble() * d + min;

			ret.add( p );
		}

		return ret;
	}

	/**
	 * Converts a point from homogenous coordinates into Euclidean
	 * @param p 3D point in homogenous coordinates
	 * @return 3D point
	 */
	public static Point3D_F64 h_to_e( Point4D_F64 p ) {
		Point3D_F64 out = new Point3D_F64();
		h_to_e(p,out);
		return out;
	}
	public static void h_to_e(Point4D_F64 p , Point3D_F64 out ) {
		out.x = p.x/p.w;
		out.y = p.y/p.w;
		out.z = p.z/p.w;
	}

}


/*
 * Copyright (C) 2011-2015, 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.transform.affine;

import georegression.struct.affine.Affine2D_F64;
import georegression.struct.point.Point2D_F64;
import georegression.struct.point.Vector2D_F64;

/**
 * @author Peter Abeles
 */
public class AffinePointOps_F64 {

	/**
	 * Applies a 2D affine transform to the point and stores the results in another
	 * variable.
	 *
	 * @param se	 The transform.
	 * @param orig   Original point being transformed. Not modified.
	 * @param result Where the results are stored.  Can be the same as orig. If null a new
	 *               instance is created. Modified.
	 * @return Transformed point.
	 */
	public static Vector2D_F64 transform( Affine2D_F64 se, Vector2D_F64 orig, Vector2D_F64 result ) {

		if( result == null ) {
			result = new Vector2D_F64();
		}

		// copy the values so that no errors happen if orig and result are the same instance
		double x = orig.x;
		double y = orig.y;

		result.x = se.a11 * x + se.a12 * y;
		result.y = se.a21 * x + se.a22 * y;

		return result;
	}

	/**
	 * Applies a 2D affine transform to the point and stores the results in another
	 * variable.
	 *
	 * @param se	 The transform.
	 * @param orig   Original point being transformed. Not modified.
	 * @param result Where the results are stored.  Can be the same as orig. If null a new
	 *               instance is created. Modified.
	 * @return Transformed point.
	 */
	public static Point2D_F64 transform( Affine2D_F64 se, Point2D_F64 orig, Point2D_F64 result ) {

		if( result == null ) {
			result = new Point2D_F64();
		}

		// copy the values so that no errors happen if orig and result are the same instance
		double x = orig.x;
		double y = orig.y;

		result.x = se.tx + se.a11 * x + se.a12 * y;
		result.y = se.ty + se.a21 * x + se.a22 * y;

		return result;
	}

	/**
	 * Applies a 2D affine transform to the point and stores the results in another
	 * variable.
	 *
	 * @param se	 The transform.
	 * @param x	  Original x-coordinate
	 * @param y	  Original y-coordinate
	 * @param result Where the results are stored.  Can be the same as orig. If null a new
	 *               instance is created. Modified.
	 * @return Transformed point.
	 */
	public static Point2D_F64 transform( Affine2D_F64 se, double x, double y, Point2D_F64 result ) {

		if( result == null ) {
			result = new Point2D_F64();
		}

		result.x = se.tx + se.a11 * x + se.a12 * y;
		result.y = se.ty + se.a21 * x + se.a22 * y;

		return result;
	}
}


/*
 * 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.struct.point.Point2D_F64;
import georegression.struct.point.Vector2D_F64;

/**
 * @author Peter Abeles
 */
public class UtilVector2D_F64 {
	/**
	 * Returns the acute angle between the two vectors.  Computed using the dot product.
	 * @param a vector
	 * @param b vector
	 * @return acute angle
	 */
	public static double acute( Vector2D_F64 a , Vector2D_F64 b ) {
		double dot = a.dot(b);

		double value = dot/(a.norm()*b.norm());
		if( value > 1.0 )
			value = 1.0;
		else if( value < -1.0 )
			value = -1.0;

		return Math.acos( value );
	}

	public static double acute( double ax , double ay, double bx , double by ) {
		double dot = ax*bx + ay*by;
		double na = Math.sqrt(ax*ax + ay*ay);
		double nb = Math.sqrt(bx*bx + by*by);

		double value = dot/(na*nb);
		if( value > 1.0 )
			value = 1.0;
		else if( value < -1.0 )
			value = -1.0;

		return Math.acos( value );
	}

	/**
	 * Sets the vector equal to 'a' - 'b'.
	 *
	 * @param a point
	 * @param b point
	 * @param output (output) optional storage for vector.
	 * @return Solution
	 */
	public static Vector2D_F64 minus( Point2D_F64 a , Point2D_F64 b , Vector2D_F64 output ) {
		if( output == null )
			output = new Vector2D_F64();

		output.x = a.x - b.x;
		output.y = a.y - b.y;

		return output;
	}

	/**
	 * Tests to see if the two vectors are identical up to a sign difference
	 * @param xa x-component of vector 'a'
	 * @param ya y-component of vector 'a'
	 * @param xb x-component of vector 'b'
	 * @param yb y-component of vector 'b'
	 * @param tol Tolerance in Euclidan distance
	 * @return true if identical to within tolerance and a sign ambiguity
	 */
	public static boolean identicalSign( double xa , double ya , double xb , double yb , double tol ) {
		double dx0 = xb-xa;
		double dy0 = yb-ya;
		double dx1 = xb+xa;
		double dy1 = yb+ya;

		double error0 = dx0*dx0 + dy0*dy0;
		double error1 = dx1*dx1 + dy1*dy1;

		if( error0 < error1 ) {
			return error0 <= tol*tol;
		} else {
			return error1 <= tol*tol;
		}

	}
}


/*
 * Copyright (C) 2011-2015, 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.struct.shapes;

import java.io.Serializable;

/**
 * <p>
 * An axis aligned rectangle in 2D that is specified by its lower extent (x0,y0), width, and height.  The three
 * other corners are (x0 + width,y0), (x0,y0 + height), (x0 + width,y0 + height).
 * </p>
 *
 * @author Peter Abeles
 */
public class RectangleLength2D_F64 implements Serializable {

	/**
	 * Lower extent x-axis
	 */
	public double x0;
	/**
	 * Lower extent y-axis
	 */
	public double y0;
	/**
	 * Rectangle's width
	 */
	public double width;
	/**
	 * Rectangle's height
	 */
	public double height;

	public RectangleLength2D_F64() {
	}

	public RectangleLength2D_F64(double x0, double y0, double width, double height) {
		this.width = width;
		this.height = height;
		this.x0 = x0;
		this.y0 = y0;
	}

	/**
	 * Sets lower extent
	 *
	 * @param x x-coordinate
	 * @param y y-coordinate
	 */
	public void setLowerExtent( double x, double y ) {
		this.x0 = x;
		this.y0 = y;
	}

	public double getWidth() {
		return width;
	}

	public void setWidth( double width ) {
		this.width = width;
	}

	public double getHeight() {
		return height;
	}

	public void setHeight( double height ) {
		this.height = height;
	}

	/**
	 * @return Lower-extent x-coordinate
	 */
	public double getX() {
		return x0;
	}

	/**
	 * Sets the lower-extent x-coordinate
	 *
	 * @param x Lower-extent x-coordinate
	 */
	public void setX( double x ) {
		this.x0 = x;
	}

	/**
	 * @return Lower-extent y-coordinate
	 */
	public double getY() {
		return y0;
	}

	/**
	 * Sets the lower-extent y-coordinate
	 *
	 * @param y Lower-extent y-coordinate
	 */
	public void setY( double y ) {
		this.y0 = y;
	}

	/**
	 * Sets this rectangle to be equal to the passed in rectangle.
	 * @param r Rectangle which this is to be set equal to
	 */
	public void set(RectangleLength2D_I32 r) {
		this.x0 = r.x0;
		this.y0 = r.y0;
		this.width = r.width;
		this.height = r.height;
	}

	@Override
	public String toString() {
		return getClass().getSimpleName()+"{" +
				"p=[ " + x0 + " , " + y0 +
				"], width=" + width + ", height=" + height + '}';
	}
}


/*
 * Copyright (C) 2011-2018, 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.struct.point;

import georegression.geometry.GeometryMath_F64;
import georegression.geometry.UtilVector3D_F64;
import georegression.struct.GeoTuple3D_F64;

/**
 * Spacial vector in 3D
 *
 */
public class Vector3D_F64 extends GeoTuple3D_F64<Vector3D_F64> {

	public Vector3D_F64( GeoTuple3D_F64 orig ) {
		this(orig.x,orig.y,orig.z);
	}

	public Vector3D_F64( double x, double y, double z ) {
		super( x, y, z );
	}

	public Vector3D_F64() {
	}

	public Vector3D_F64( Point3D_F64 a, Point3D_F64 b ) {
		x = b.getX() - a.getX();
		y = b.getY() - a.getY();
		z = b.getZ() - a.getZ();
	}

	public void set( Vector3D_F64 v ) {
		this.x = v.x;
		this.y = v.y;
		this.z = v.z;
	}

	/**
	 * In-place minus operation. this = a - b.
	 *
	 * @param a Point
	 * @param b Point
	 */
	public void minus( Point3D_F64 a , Point3D_F64 b ) {
		x = a.x - b.x;
		y = a.y - b.y;
		z = a.z - b.z;
	}

	/**
	 * In-place divide operation.  x /= value;
	 * @param value The value each component is divided by
	 */
	public void divide( double value ) {
		x /= value;
		y /= value;
		z /= value;
	}

	public Vector3D_F64 copy() {
		return new Vector3D_F64( x, y, z );
	}

	/**
	 * In-place cross product operation.  this = a cross b
	 *
	 * @param a Vector
	 * @param b Vector
	 */
	public void cross( Vector3D_F64 a, Vector3D_F64 b ) {
		GeometryMath_F64.cross( a, b, this );
	}

	public Vector3D_F64 cross( Vector3D_F64 b ) {
		Vector3D_F64 c = new Vector3D_F64();
		GeometryMath_F64.cross( this, b, c );

		return c;
	}

	@Override
	public Vector3D_F64 createNewInstance() {
		return new Vector3D_F64();
	}

	@Override
	public String toString() {
		return toString("V");
	}

	public void normalize() {
		// carefully normalize to avoid numerical overflow
		double m = Math.max(Math.max(Math.abs(x),Math.abs(y)),Math.abs(z));

		double x_n = x/m;
		double y_n = y/m;
		double z_n = z/m;
		double v = Math.sqrt(x_n*x_n + y_n*y_n + z_n*z_n);

		x = x_n/v;
		y = y_n/v;
		z = z_n/v;
	}

	/**
	 * Dot product between this and 'a' = this.x * a.x + this.y * a.y + this.z * a.z
	 *
	 * @param a A vector
	 * @return dot product.
	 */
	public double dot( Vector3D_F64 a ) {
		return x * a.x + y * a.y + z * a.z;
	}

	/**
	 * Returns the acute angle between the two vectors.  Computed using the dot product.
	 * @param a Vector
	 * @return Acute angle in radians between 'this' and 'a'.
	 */
	public double acute( Vector3D_F64 a ) {
		return UtilVector3D_F64.acute(this,a);
	}
}


/*
 * Copyright (C) 2011-2018, 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.curve.EllipseQuadratic_F64;
import georegression.struct.curve.EllipseRotated_F64;
import georegression.struct.point.Point2D_F64;
import georegression.struct.point.Vector2D_F64;

/**
 * Functions for extracting information from ellipses and converting between different ellipse formats.
 *
 * @author Peter Abeles
 */
public class UtilEllipse_F64 {

	/**
	 *

	 * Convert from quadratic to rotated formats.  Equations taken from [1].
	 *

	 *
	 *

	 * [1] David Eberly "Information About Ellipses", Geometric Tools, LLC 2011
	 *

	 *
	 * @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;
	}

	/**
	 *
Finds two points on the ellipse that in combination with point 'pt' each define
	 * a line that is tangent to the ellipse.

	 *
	 * Notes:

	 * 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;
	}

	/**
	 *
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.

	 * @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.TEST_F64,10);

		return alg.process(ellipseA, ellipseB,
				tangentA0, tangentA1, tangentA2, tangentA3,
				tangentB0, tangentB1, tangentB2, tangentB3) ;
	}
}