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

import georegression.geometry.UtilEllipse_F64;
import georegression.geometry.algs.AreaIntersectionPolygon2D_F64;
import georegression.misc.GrlConstants;
import georegression.struct.curve.EllipseRotated_F64;
import georegression.struct.line.LineGeneral2D_F64;
import georegression.struct.line.LineParametric2D_F64;
import georegression.struct.line.LineSegment2D_F64;
import georegression.struct.point.Point2D_F64;
import georegression.struct.point.Point3D_F64;
import georegression.struct.shapes.Polygon2D_F64;
import georegression.struct.shapes.Quadrilateral_F64;
import georegression.struct.shapes.Rectangle2D_F64;
import georegression.struct.shapes.RectangleLength2D_F64;

/**
 * Functions relating to finding the points at which two shapes intersect with each other.
 *
 * @author Peter Abeles
 */
public class Intersection2D_F64 {

	// todo comment
	// todo how are parallel lines handled?

	/**
	 *

	 * Checks to see if the point is contained inside the convex polygon.  If the
	 * point is an the polygon's perimeter it is considered to NOT be inside.
	 *

	 *
	 *

	 * Clockwise or counter-clockwise order of the polygon does not matter.
	 *

	 *
	 * @param polygon Convex polygon. Not modified.
	 * @param pt Point. Not modified.
	 * @return True if the point is contained inside the polygon.
	 */
	// Ported from internet code 12/2011
	// http://www.ecse.rpi.edu/Homepages/wrf/Research/Short_Notes/pnpoly.html
	public static boolean containConvex( Polygon2D_F64 polygon , Point2D_F64 pt )
	{
		final int N = polygon.size();

		boolean c = false;
		for (int i = 0, j = N-1; i < N; j = i++) {
			Point2D_F64 a = polygon.vertexes.data[i];
			Point2D_F64 b = polygon.vertexes.data[j];

			if ( ((a.y>pt.y) != (b.y>pt.y)) && (pt.x < (b.x-a.x) * (pt.y-a.y) / (b.y-a.y) + a.x) )
				c = !c;
		}
		return c;
	}

	/**
	 * Checks to see if the point is contained inside the concave polygon.
	 *
	 * NOTE: Points which lie along the perimeter may or may not be considered as inside
	 *
	 * @param polygon Convex polygon. Not modified.
	 * @param pt Point. Not modified.
	 * @return True if the point is contained inside the polygon.
	 */
	public static boolean containConcave( Polygon2D_F64 polygon , Point2D_F64 pt )
	{
		final int N = polygon.size();

		int left=0;
		int right=0;
		for (int i = 0; i < N-1; i++) {
			Point2D_F64 a = polygon.vertexes.data[i];
			Point2D_F64 b = polygon.vertexes.data[i+1];

			if( (pt.y >= a.y && pt.y = b.y && pt.y < a.y) ) {
				// location of line segment along x-axis at y = pt.y
				double x = b.y==a.y ? pt.x : (pt.y-a.y)*(b.x-a.x)/(b.y-a.y) + a.x;

				if( x <= pt.x )
					left++;
				else if( x > pt.x )
					right++;
			}
		}

		Point2D_F64 a = polygon.vertexes.data[N-1];
		Point2D_F64 b = polygon.vertexes.data[0];

		if( (pt.y >= a.y && pt.y = b.y && pt.y < a.y) ) {
			// location of line segment along x-axis at y = pt.y
			double x = b.y==a.y ? pt.x : (pt.y-pt.y)*(b.x-a.x)/(b.y-a.y) + a.x;

			if( x <= pt.x )
				left++;
			else if( x > pt.x )
				right++;
		}

		return (left % 2 == 1 && right % 2 == 1);
	}

	/**
	 * True if the point is contained inside the quadrilateral.
	 *
	 * @param quad quadrilateral
	 * @param pt point
	 * @return true if the point is inside and false if it is not.
	 */
	public static boolean contains( Quadrilateral_F64 quad , Point2D_F64 pt ) {
		return containTriangle(quad.a, quad.b, quad.d, pt) ||
				containTriangle(quad.b, quad.c, quad.d, pt);

	}

	/**
	 * Returns true of the the point is inside the triangle.
	 *
	 * This function is simply an unrolled version of {@link #containConcave(Polygon2D_F64, Point2D_F64)}.
	 *
	 * @param a vertex in triangle
	 * @param b vertex in triangle
	 * @param c vertex in triangle
	 * @param pt Point which is being tested for containment inside of triangle
	 * @return true if the point is inside of triangle
	 */
	public static boolean containTriangle( Point2D_F64 a , Point2D_F64 b , Point2D_F64 c , Point2D_F64 pt )
	{
		boolean ret = false;

		if ( ((a.y>pt.y) != (b.y>pt.y)) && (pt.x < (b.x-a.x) * (pt.y-a.y) / (b.y-a.y) + a.x) )
			ret = true;

		if ( ((b.y>pt.y) != (c.y>pt.y)) && (pt.x < (c.x-b.x) * (pt.y-b.y) / (c.y-b.y) + b.x) )
			ret = !ret;

		if ( ((c.y>pt.y) != (a.y>pt.y)) && (pt.x < (a.x-c.x) * (pt.y-c.y) / (a.y-c.y) + c.x) )
			ret = !ret;

		return ret;
	}

	/**
	 * Finds the point of intersection between two lines and returns the point.
	 *
	 * @param a Line.
	 * @param b Line.
	 * @param ret storage for the point of intersection. If null a new point will be declared.
	 * @return If the two lines intersect it returns the point of intersection.  null if they don't intersect or have infinite intersections.
	 */
	public static Point2D_F64 intersection( LineParametric2D_F64 a, LineParametric2D_F64 b , Point2D_F64 ret ) {
		double t_b = a.getSlopeX() * ( b.getY() - a.getY() ) - a.getSlopeY() * ( b.getX() - a.getX() );
		double bottom = a.getSlopeY() * b.getSlopeX() - b.getSlopeY() * a.getSlopeX();

		if( bottom == 0 )
			return null;

		t_b /= bottom;

		double x = b.getSlopeX() * t_b + b.getX();
		double y = b.getSlopeY() * t_b + b.getY();

		if( ret == null )
			ret = new Point2D_F64();
		ret.set(x,y);
		return ret;
	}

	/**
	 * Finds the point of intersection between two lines.  The point of intersection is specified as a point along
	 * the parametric line 'a'.  (x,y) = (x_0,y_0) + t*(slope_x,slope_y), where 't' is the location returned.
	 *
	 * @param a Line.
	 * @param b Line.
	 * @return The location along 'target'.  If the lines do not intersect or have infinite intersections Double.NaN is returned.
	 */
	public static double intersection( LineParametric2D_F64 a, LineParametric2D_F64 b ) {
		double t_a = b.getSlopeX() * ( a.getY() - b.getY() ) - b.getSlopeY() * ( a.getX() - b.getX() );
		double bottom = b.getSlopeY() * a.getSlopeX() - a.getSlopeY() * b.getSlopeX();

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

		return t_a/bottom;
	}

	/**
	 * Finds the point of intersection between two lines segments.
	 *
	 * @param l_0 Line segment.
	 * @param l_1 line segment.
	 * @param ret storage for the point of intersection. If null a new point will be declared.
	 * @return If the two lines intersect it returns the point of intersection.  null if they don't intersect or have infinite intersections.
	 */
	public static Point2D_F64 intersection( LineSegment2D_F64 l_0, LineSegment2D_F64 l_1,
											Point2D_F64 ret ) {

		double a0 = l_0.b.x - l_0.a.x;
		double b0 = l_0.b.y - l_0.a.y;
		double a1 = l_1.b.x - l_1.a.x;
		double b1 = l_1.b.y - l_1.a.y;

		double top = b0 * ( l_1.a.x - l_0.a.x ) + a0 * ( l_0.a.y - l_1.a.y );
		double bottom = a0 * b1 - b0 * a1;

		if( bottom == 0 )
			return null;
		double t_1 = top / bottom;

		// does not intersect along the second line segment
		if( t_1 < 0 || t_1 > 1 )
			return null;

		top = b1 * ( l_0.a.x - l_1.a.x ) + a1 * ( l_1.a.y - l_0.a.y );
		bottom = a1 * b0 - b1 * a0;

		double t_0 = top / bottom;

		// does not intersect along the first line segment
		if( t_0 < 0 || t_0 > 1 )
			return null;

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

		ret.set( l_1.a.x + a1 * t_1, l_1.a.y + b1 * t_1 );

		return ret;
	}

	/**
	 *

	 * Finds the intersection of two lines as a 2D point in homogeneous coordinates.  Because the
	 * solution is found in homogeneous coordinates it can even handle parallel lines which "intersect
	 * at infinity".
	 *

	 *
	 *

	 * A 2D point in homogeneous coordinates is expressed as the triple (x,y,z), which can be converted
	 * into the standard notation as x' = x/z and y'= y/z. If the lines are parallel and intersect at
	 * infinity then z=0 and the above conversion will fail.
	 *

	 *
	 * @param a Line
	 * @param b Line
	 * @param ret Storage for point of intersection.
	 * @return Point of intersection represented in homogeneous coordinates.
	 */
	public static Point3D_F64 intersection( LineGeneral2D_F64 a , LineGeneral2D_F64 b , Point3D_F64 ret )
	{
		if( ret == null )
			ret = new Point3D_F64();

		// compute the intersection as the cross product of 'a' and 'b'
		ret.x = a.B * b.C - a.C * b.B;
		ret.y = a.C * b.A - a.A * b.C;
		ret.z = a.A * b.B - a.B * b.A;

		return ret;
	}

	/**
	 *

	 * Finds the intersection of two lines as a 2D point in coordinates. If the lines are parallel
	 * then null is returned.
	 *

	 *
	 * @param a Line
	 * @param b Line
	 * @param ret Storage for point of intersection.
	 * @return Point of intersection in 2D coordinates.  null if intersection at infinity
	 */
	public static Point2D_F64 intersection( LineGeneral2D_F64 a , LineGeneral2D_F64 b , Point2D_F64 ret )
	{
		if( ret == null )
			ret = new Point2D_F64();

		// compute the intersection as the cross product of 'a' and 'b'
		ret.x = a.B * b.C - a.C * b.B;
		ret.y = a.C * b.A - a.A * b.C;
		double z = a.A * b.B - a.B * b.A;

		if( z == 0 )
			return null;

		ret.x /= z;
		ret.y /= z;

		return ret;
	}

	/**
	 * Finds the point of intersection between the two lines defined by the set sets of points passed in.
	 * @param lineA0 Point on line A
	 * @param lineA1 Point on line A
	 * @param lineB0 Point on line B
	 * @param lineB1 Point on line B
	 * @param output (Optional) storage for point of intersection
	 * @return Point of intersection or null if the lines are parallel
	 */
	public static Point2D_F64 intersection( Point2D_F64 lineA0 , Point2D_F64 lineA1 ,
											Point2D_F64 lineB0 , Point2D_F64 lineB1 ,
											Point2D_F64 output )
	{
		if( output == null )
			output = new Point2D_F64();

		double slopeAx = lineA1.x - lineA0.x;
		double slopeAy = lineA1.y - lineA0.y;
		double slopeBx = lineB1.x - lineB0.x;
		double slopeBy = lineB1.y - lineB0.y;

		double top = slopeAy * ( lineB0.x - lineA0.x ) + slopeAx * (lineA0.y - lineB0.y );
		double bottom = slopeAx * slopeBy - slopeAy * slopeBx;

		if( bottom == 0 )
			return null;
		double t = top / bottom;

		output.x = lineB0.x + t*slopeBx;
		output.y = lineB0.y + t*slopeBy;

		return output;
	}

	/**
	 * Finds the point of intersection between a line and a line segment.  The point of intersection
	 * is specified as the distance along the parametric line.  If no intersection is found then
	 * Double.NaN is returned.
	 *
	 * @param target A line whose location along which the point of intersection is being found.  Not modified.
	 * @param l	  Line segment which is being tested for intersection. Not modified.
	 * @return The location along 'target'.  If the lines do not intersect or have infinite intersections Double.NaN is returned.
	 */
	public static double intersection( LineParametric2D_F64 target, LineSegment2D_F64 l ) {
		double a1 = l.b.x - l.a.x;
		double b1 = l.b.y - l.a.y;

		double top = target.slope.y * ( l.a.x - target.p.x ) + target.slope.x * ( target.p.y - l.a.y );
		double bottom = target.slope.x * b1 - target.slope.y * a1;

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

		double t_1 = top / bottom;

		// does not intersect along the second line segment
		if( t_1 < 0 || t_1 > 1 )
			return Double.NaN;

		top = b1 * ( target.p.x - l.a.x ) + a1 * ( l.a.y - target.p.y );
		bottom = a1 * target.slope.y - b1 * target.slope.x;

		return top / bottom;
	}

	/**
	 * Finds the area of the intersection of two polygons.
	 *
	 * @see AreaIntersectionPolygon2D_F64
	 *
	 * @param a (Input) Polygon 2D
	 * @param b (Input) Polygon 2D
	 * @return Area of intersection.
	 */
	public static double intersection( Polygon2D_F64 a , Polygon2D_F64 b ) {
		AreaIntersectionPolygon2D_F64 alg = new AreaIntersectionPolygon2D_F64();
		return Math.abs(alg.computeArea(a,b));
	}

	/**
	 *

	 * Checks to see if the specified point is inside the rectangle.  A point is inside
	 * if it is ≥ the lower extend and < the upper extent.
	 *

	 *

	 * inside = x ≥ x0 AND x < x0+width AND y ≥ y0 AND y < y0+height
	 *

	 * @param a Rectangle.
	 * @param x x-coordinate of point being tested for containment
	 * @param y y-coordinate of point being tested for containment
	 * @return true if inside and false if output
	 */
	public static boolean contains( RectangleLength2D_F64 a, double x, double y ) {
		if( a.getX() <= x && a.getX() + a.getWidth() > x ) {
			return a.getY() <= y && a.getY() + a.getHeight() > y;
		}
		return false;
	}

	/**
	 *

	 * Checks to see if the specified point is inside the rectangle.  A point is inside
	 * if it is ≥ the lower extend and ≤ the upper extent.
	 *

	 *

	 * {@code inside = x ≥ x0 AND x ≤ x0+width and y ≥ y0 AND y ≤ y0+height}
	 *

	 * @param a Rectangle.
	 * @param x x-coordinate of point being tested for containment
	 * @param y y-coordinate of point being tested for containment
	 * @return true if inside and false if output
	 */
	public static boolean contains2( RectangleLength2D_F64 a, double x, double y ) {
		if( a.getX() <= x && a.getX() + a.getWidth() >= x ) {
			return a.getY() <= y && a.getY() + a.getHeight() >= y;
		}
		return false;
	}

	/**
	 *

	 * Checks to see if the specified point is inside the rectangle.  A point is inside
	 * if it is ≥ the lower extend and < the upper extent.
	 *

	 *

	 * inside = x ≥ x0 AND x ≤ x1 AND y ≥ y0 AND y ≤ y1
	 *

	 * @param a Rectangle.
	 * @param x x-coordinate of point being tested for containment
	 * @param y y-coordinate of point being tested for containment
	 * @return true if inside and false if output
	 */
	public static boolean contains( Rectangle2D_F64 a, double x, double y ) {
		return( a.p0.x <= x && a.p1.x > x && a.p0.y <= y && a.p1.y > y );
	}

	/**
	 *

	 * Checks to see if the specified point is inside the rectangle.  A point is inside
	 * if it is ≥ the lower extend and ≤ the upper extent.
	 *

	 *

	 * inside = x ≥ x0 AND x ≤ x1 AND y ≥ y0 AND y ≤ y1
	 *

	 * @param a Rectangle.
	 * @param x x-coordinate of point being tested for containment
	 * @param y y-coordinate of point being tested for containment
	 * @return true if inside and false if output
	 */
	public static boolean contains2( Rectangle2D_F64 a, double x, double y ) {
		return( a.p0.x <= x && a.p1.x >= x && a.p0.y <= y && a.p1.y >= y );
	}

	/**
	 * Tests to see if the provided point lies on or is contained inside the ellipse
	 *
	 * @param ellipse  Ellipse
	 * @param x x-coordinate of point being tested for containment
	 * @param y y-coordinate of point being tested for containment
	 * @return true if inside and false if output
	 */
	public static boolean contains(EllipseRotated_F64 ellipse , double x , double y ) {
		return (UtilEllipse_F64.evaluate(x,y, ellipse) <= 1.0 );
	}

	public static RectangleLength2D_F64 intersection( RectangleLength2D_F64 a, RectangleLength2D_F64 b ) {
		double tl_x, tl_y, w, h;

		if( a.getX() >= b.getX() ) {
			if( a.getX() >= b.getX() + b.getWidth() )
				return null;

			tl_x = a.getX();
			w = b.getX() + b.getWidth() - a.getX();
		} else {
			if( a.getX() + a.getWidth() <= b.getX() )
				return null;

			tl_x = b.getX();
			w = a.getX() + a.getWidth() - b.getX();
		}

		if( a.getY() >= b.getY() ) {
			if( a.getY() >= b.getY() + b.getHeight() )
				return null;

			tl_y = a.getY();
			h = b.getY() + b.getHeight() - a.getY();
		} else {
			if( a.getY() + a.getHeight() <= b.getY() )
				return null;

			tl_y = b.getY();
			h = a.getY() + a.getHeight() - b.getY();
		}

		return new RectangleLength2D_F64( tl_x, tl_y, w, h );
	}

	/**
	 * Checks to see if the two rectangles intersect each other
	 *
	 * @param a Rectangle
	 * @param b Rectangle
	 * @return true if intersection
	 */
	public static boolean intersects( Rectangle2D_F64 a , Rectangle2D_F64 b ) {
		return( a.p0.x  b.p0.x && a.p0.y  b.p0.y );
	}

	/**
	 * Finds the intersection between two rectangles.  If the rectangles don't intersect then false is returned.
	 *
	 * @param a Rectangle
	 * @param b Rectangle
	 * @param result Storage for the found intersection
	 * @return true if intersection
	 */
	public static boolean intersection( Rectangle2D_F64 a , Rectangle2D_F64 b , Rectangle2D_F64 result ) {
		if( !intersects(a,b) )
			return false;

		result.p0.x = Math.max(a.p0.x,b.p0.x);
		result.p1.x = Math.min(a.p1.x,b.p1.x);
		result.p0.y = Math.max(a.p0.y,b.p0.y);
		result.p1.y = Math.min(a.p1.y,b.p1.y);

		return true;
	}

	/**
	 * Returns the area of the intersection of two rectangles.
	 *
	 * @param a Rectangle
	 * @param b Rectangle
	 * @return area of intersection
	 */
	public static double intersectionArea( Rectangle2D_F64 a , Rectangle2D_F64 b ) {
		if( !intersects(a,b) )
			return 0;

		double x0 = Math.max(a.p0.x,b.p0.x);
		double x1 = Math.min(a.p1.x,b.p1.x);
		double y0 = Math.max(a.p0.y,b.p0.y);
		double y1 = Math.min(a.p1.y,b.p1.y);

		return (x1-x0)*(y1-y0);
	}

	/**
	 * Determines the location(s) that a line and ellipse intersect.  Returns the number of intersections found.
	 * NOTE: Due to floating point errors, it's possible for a single solution to returned as two points.
	 *
	 * @param line Line
	 * @param ellipse Ellipse
	 * @param intersection0 Storage for first point of intersection.
	 * @param intersection1 Storage for second point of intersection.
	 * @param EPS Numerical precision.  Set to a negative value to use default
	 * @return Number of intersections.  Possible values are 0, 1, or 2.
	 */
	public static int intersection( LineGeneral2D_F64 line , EllipseRotated_F64 ellipse ,
									Point2D_F64 intersection0 , Point2D_F64 intersection1 , double EPS ) {

		if( EPS < 0 ) {
			EPS = GrlConstants.EPS;
		}

		// First translate the line so that coordinate origin is the same as the ellipse
		double C = line.C + (line.A*ellipse.center.x + line.B*ellipse.center.y);

		// Now rotate the line
		double cphi = Math.cos(ellipse.phi);
		double sphi = Math.sin(ellipse.phi);
		double A =  line.A*cphi + line.B*sphi;
		double B = -line.A*sphi + line.B*cphi;

		// Now solve for the intersections with the coordinate system centered and aligned to the ellipse
		// There are two different ways to solve for this.  Pick the axis with the largest slope
		// to avoid the pathological case
		double a2 = ellipse.a*ellipse.a;
		double b2 = ellipse.b*ellipse.b;

		double x0,y0;
		double x1,y1;

		int totalIntersections;

		if( Math.abs(A) > Math.abs(B) ) {
			double alpha = -C/A;
			double beta = -B/A;

			double aa = beta*beta/a2 + 1.0/b2;
			double bb = 2.0*alpha*beta/a2;
			double cc = alpha*alpha/a2 - 1.0;

			double inner = bb*bb -4.0*aa*cc;
			if( Math.abs(inner/aa) < EPS ) { // divide by aa for scale invariance
				totalIntersections = 1;
				inner = inner < 0 ? 0 : inner;
			} else if( inner < 0 ) {
				return 0;
			} else {
				totalIntersections = 2;
			}
			double right = Math.sqrt(inner);
			y0 = (-bb + right)/(2.0*aa);
			y1 = (-bb - right)/(2.0*aa);

			x0 =  -(C + B*y0)/A;
			x1 =  -(C + B*y1)/A;
		} else {
			double alpha = -C/B;
			double beta = -A/B;

			double aa = beta*beta/b2 + 1.0/a2;
			double bb = 2.0*alpha*beta/b2;
			double cc = alpha*alpha/b2-1.0;

			double inner = bb*bb -4.0*aa*cc;
			if( Math.abs(inner/aa) < EPS ) { // divide by aa for scale invariance
				totalIntersections = 1;
				inner = inner < 0 ? 0 : inner;
			} else if( inner < 0 ) {
				return 0;
			} else {
				totalIntersections = 2;
			}
			double right = Math.sqrt(inner);
			x0 = (-bb + right)/(2.0*aa);
			x1 = (-bb - right)/(2.0*aa);

			y0 = -(A*x0 + C)/B;
			y1 = -(A*x1 + C)/B;
		}

		// go back into world coordinate system
		intersection0.x = x0*cphi - y0*sphi + ellipse.center.x;
		intersection0.y = x0*sphi + y0*cphi + ellipse.center.y;

		intersection1.x = x1*cphi - y1*sphi + ellipse.center.x;
		intersection1.y = x1*sphi + y1*cphi + ellipse.center.y;

		return totalIntersections;
	}
}


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

import georegression.misc.GrlConstants;
import georegression.struct.EulerType;
import georegression.struct.so.Quaternion_F64;
import georegression.struct.so.Rodrigues_F64;
import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.CommonOps_DDRM;
import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
import org.ejml.interfaces.decomposition.SingularValueDecomposition;

/**
 * Provides functions to convert between different parameterizations of 3D rotation matrices.
 *
 * @author Peter Abeles
 */
public class ConvertRotation3D_F64 {

	/**
	 * Converts {@link georegression.struct.so.Rodrigues_F64} into a rotation matrix.
	 *
	 * @param rodrigues rotation defined using rotation axis angle notation.
	 * @param R where the results will be stored.  If null a new matrix is declared internally.
	 * @return rotation matrix.
	 */
	public static DMatrixRMaj rodriguesToMatrix( Rodrigues_F64 rodrigues, DMatrixRMaj R ) {
		return rodriguesToMatrix(
				rodrigues.unitAxisRotation.x,
				rodrigues.unitAxisRotation.y,
				rodrigues.unitAxisRotation.z,
				rodrigues.theta, R);
	}

	/**
	 * Converts axis angle ({@link Rodrigues_F64}) into a rotation matrix with out needing to declare a storage
	 * variable.
	 *
	 * @param axisX x-component of normalized rotation vector
	 * @param axisY y-component of normalized rotation vector
	 * @param axisZ z-component of normalized rotation vector
	 * @param theta magnitude of rotation in radians
	 * @param R (Optional) storage for 3x3 rotation matrix.  If null one will be declared internally.
	 * @return Rotation matrix.
	 */
	public static DMatrixRMaj rodriguesToMatrix( double axisX , double axisY , double axisZ , double theta,
													DMatrixRMaj R ) {
		R = checkDeclare3x3( R );

		//noinspection UnnecessaryLocalVariable
		double x = axisX, y = axisY, z = axisZ;

		double c = Math.cos( theta );
		double s = Math.sin( theta );
		double oc = 1.0 - c;

		R.data[0] = c + x * x * oc;
		R.data[1] = x * y * oc - z * s;
		R.data[2] = x * z * oc + y * s;
		R.data[3] = y * x * oc + z * s;
		R.data[4] = c + y * y * oc;
		R.data[5] = y * z * oc - x * s;
		R.data[6] = z * x * oc - y * s;
		R.data[7] = z * y * oc + x * s;
		R.data[8] = c + z * z * oc;

		return R;
	}

	/**
	 *
Converts{@link georegression.struct.so.Rodrigues_F64} into an euler rotation of different types

	 *
	 * @param rodrigues rotation defined using rotation axis angle notation.
	 * @param type Type of Euler rotation
	 * @param euler (Output) Optional storage for Euler rotation
	 * @return The Euler rotation.
	 */
	public static double[] rodriguesToEuler(Rodrigues_F64 rodrigues , EulerType type , double []euler )
	{
		DMatrixRMaj R = rodriguesToMatrix(rodrigues,null);
		return matrixToEuler(R,type,euler);
	}

	/**
	 *

	 * Converts {@link georegression.struct.so.Rodrigues_F64} into a unit {@link georegression.struct.so.Quaternion_F64}.
	 *

	 *
	 * @param rodrigues The angle of rotation around the rotation axis.
	 * @param quat Storage for quaternion coordinate.  If null a new quaternion is created. Modified.
	 * @return unit quaternion coordinate.
	 */
	public static Quaternion_F64 rodriguesToQuaternion( Rodrigues_F64 rodrigues,
														Quaternion_F64 quat ) {
		if( quat == null )
			quat = new Quaternion_F64();

		quat.w = Math.cos( rodrigues.theta / 2.0 );
		double s = Math.sin( rodrigues.theta / 2.0 );

		quat.x = rodrigues.unitAxisRotation.x * s;
		quat.y = rodrigues.unitAxisRotation.y * s;
		quat.z = rodrigues.unitAxisRotation.z * s;

		return quat;
	}

	/**
	 * Converts a unit {@link Quaternion_F64} into {@link Rodrigues_F64}.
	 * @param quat (Input) Unit quaternion
	 * @param rodrigues (Optional) Storage for rodrigues coodinate.  If null a new instance is created.
	 * @return rodrigues
	 */
	public static Rodrigues_F64 quaternionToRodrigues( Quaternion_F64 quat,
													   Rodrigues_F64 rodrigues ) {
		if( rodrigues == null )
			rodrigues = new Rodrigues_F64();

		rodrigues.unitAxisRotation.set( quat.x, quat.y, quat.z);
		rodrigues.unitAxisRotation.normalize();

		rodrigues.theta = 2.0 * Math.acos( quat.w);

		return rodrigues;
	}

	/**
	 *
Converts a quaternion into an euler rotation of different types

	 *
	 * @param q (Input) Normalized quaternion. Not modified.
	 * @param type Type of Euler rotation
	 * @param euler (Output) Optional storage for Euler rotation
	 * @return The Euler rotation.
	 */
	public static double[] quaternionToEuler(Quaternion_F64 q , EulerType type , double []euler )
	{
		DMatrixRMaj R = quaternionToMatrix(q,null);
		return matrixToEuler(R,type,euler);
	}

	/**
	 *
Converts a rotation matrix into an Euler angle of different types

	 *
	 * @param R (Input) Rotation matrix. Not modified.
	 * @param type Type of Euler rotation
	 * @param euler (Output) Optional storage for Euler rotation
	 * @return The Euler rotation.
	 */
	public static double[] matrixToEuler(DMatrixRMaj R , EulerType type , double[] euler ) {
		if( euler == null )
			euler = new double[3];

		switch(type){
			case ZYX:
				TanSinTan(-2,1,  3,  -6,9,  5,-7,4,8,  R,euler);
				break;

			case ZYZ:
				TanCosTan(8,-7,  9,   6,3,  5,-7,4,8,  R,euler);
				break;

			case ZXY:
				TanSinTan(4,5,  -6,   3,9,  1,8,-2,7,  R,euler);
				break;

			case ZXZ:
				TanCosTan(7,8,   9,  3,-6,  1,8,-2,7,  R,euler);
				break;

			case YXZ:
				TanSinTan(-7,9,  8, -2,5,  1,-6,3,4,   R,euler);
				break;

			case YXY:
				TanCosTan(4,-6,  5,  2,8,  1,-6,3,4,   R,euler);
				break;

			case YZX:
				TanSinTan(3,1,  -2,  8,5,  9,4,-7,6,   R,euler);
				break;

			case YZY:
				TanCosTan(6,4,   5,  8,-2, 9,4,-7,6,   R,euler);
				break;

			case XYZ:
				TanSinTan(8,9,  -7,  4,1,  5,3,-6,2,   R,euler);
				break;

			case XYX:
				TanCosTan(2,3,   1,  4,-7, 5,3,-6,2,   R,euler);
				break;

			case XZY:
				TanSinTan(-6,5,  4, -7,1,  9,-2,8,3,   R,euler);
				break;

			case XZX:
				TanCosTan(3,-2,  1,  7,4,  9,-2,8,3,   R,euler);
				break;

			default:
				throw new IllegalArgumentException("Unknown rotation sequence");
		}

		return euler;
	}

	private static void TanSinTan( int y0 , int x0 , int sin1 , int y2 , int x2 ,
								   int cos0a , int cos0b , int sin0a , int sin0b,
								   DMatrixRMaj R , double euler[] ) {

		double val_y0 = get(R,y0);
		double val_x0 = get(R,x0);
		double val_sin1 = get(R,sin1);
		double val_y2 = get(R,y2);
		double val_x2 = get(R,x2);

		if( 1.0-Math.abs(val_sin1) <= GrlConstants.EPS ) {

			double sign = Math.signum(val_sin1);

			double sin0 = (get(R,sin0a)+sign*get(R,sin0b))/2.0;
			double cos0 = (get(R,cos0a)+sign*get(R,cos0b))/2.0;

			euler[0] = Math.atan2(sin0,cos0);
			euler[1] = sign * Math.PI/2.0;
			euler[2] = 0;
		} else {
			euler[0] = Math.atan2(val_y0,val_x0);
			euler[1] = Math.asin(val_sin1);
			euler[2] = Math.atan2(val_y2,val_x2);
		}
	}

	private static void TanCosTan( int y0 , int x0 , int cos1 , int y2 , int x2 ,
								   int cos0a , int cos0b , int sin0a , int sin0b,
								  DMatrixRMaj R , double euler[] ) {

		double val_y0 = get(R,y0);
		double val_x0 = get(R,x0);
		double val_cos1 = get(R,cos1);
		double val_y2 = get(R,y2);
		double val_x2 = get(R,x2);

		if( 1.0-Math.abs(val_cos1) <= GrlConstants.EPS ) {
			double sin0 = (get(R,sin0a)+get(R,sin0b))/2.0;
			double cos0 = (get(R,cos0a)+get(R,cos0b))/2.0;

			euler[0] = Math.atan2(sin0,cos0);
			euler[1] = 0;
			euler[2] = 0;
		} else {
			euler[0] = Math.atan2(val_y0,val_x0);
			euler[1] = Math.acos(val_cos1);
			euler[2] = Math.atan2(val_y2,val_x2);
		}
	}

	/**
	 * If the index is negative it returns the negative of the value at -index.  Starts at 0
	 */
	private static double get( DMatrixRMaj M , int index ) {
		if( index < 0 ) {
			return -M.data[-index-1];
		} else {
			return M.data[index-1];
		}
	}

	/**
	 * Extracts quaternions from the provided rotation matrix.
	 *
	 * @param R (Input) rotation matrix
	 * @param quat (Output) Optional storage for quaternion.  If null a new class will be used.
	 * @return unit quaternion representation of the rotation matrix.
	 */
	public static Quaternion_F64 matrixToQuaternion( DMatrixRMaj R, Quaternion_F64 quat ) {

		if( quat == null )
			quat = new Quaternion_F64();

		// algorithm from:
		// http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
		//
		// Designed to minimize numerical error by not dividing by very small numbers

		double m00 = R.unsafe_get(0,0);
		double m01 = R.unsafe_get(0,1);
		double m02 = R.unsafe_get(0,2);
		double m10 = R.unsafe_get(1,0);
		double m11 = R.unsafe_get(1,1);
		double m12 = R.unsafe_get(1,2);
		double m20 = R.unsafe_get(2,0);
		double m21 = R.unsafe_get(2,1);
		double m22 = R.unsafe_get(2,2);

		double trace = m00 + m11 + m22;

		if (trace > 0) {
			double S = Math.sqrt(trace+1.0) * 2; // S=4*qw
			quat.w = 0.25 * S;
			quat.x = (m21 - m12) / S;
			quat.y = (m02 - m20) / S;
			quat.z = (m10 - m01) / S;
		} else if ((m00 > m11)&(m00 > m22)) {
			double S = Math.sqrt(1.0 + m00 - m11 - m22) * 2; // S=4*qx
			quat.w = (m21 - m12) / S;
			quat.x = 0.25 * S;
			quat.y = (m01 + m10) / S;
			quat.z = (m02 + m20) / S;
		} else if (m11 > m22) {
			double S = Math.sqrt(1.0 + m11 - m00 - m22) * 2; // S=4*qy
			quat.w = (m02 - m20) / S;
			quat.x = (m01 + m10) / S;
			quat.y = 0.25 * S;
			quat.z = (m12 + m21) / S;
		} else {
			double S = Math.sqrt(1.0 + m22 - m00 - m11) * 2; // S=4*qz
			quat.w = (m10 - m01) / S;
			quat.x = (m02 + m20) / S;
			quat.y = (m12 + m21) / S;
			quat.z = 0.25 * S;
		}

		return quat;
	}

	/**
	 * Converts a rotation matrix into {@link georegression.struct.so.Rodrigues_F64}.
	 *
	 * @param R Rotation matrix.
	 * @param rodrigues Storage used for solution.  If null a new instance is declared.
	 * @return The found axis and rotation angle.
	 */
	public static Rodrigues_F64 matrixToRodrigues( DMatrixRMaj R, Rodrigues_F64 rodrigues ) {
		if( rodrigues == null ) {
			rodrigues = new Rodrigues_F64();
		}
		// parts of this are from wikipedia
		// http://en.wikipedia.org/wiki/Rotation_representation_%28mathematics%29#Rotation_matrix_.E2.86.94_Euler_axis.2Fangle

		double diagSum = ( (R.unsafe_get( 0, 0 ) + R.unsafe_get( 1, 1 ) + R.unsafe_get( 2, 2 )) - 1.0 ) / 2.0;

		double absDiagSum = Math.abs(diagSum);

		if( absDiagSum <= 1.0 && 1.0-absDiagSum > 10.0*GrlConstants.EPS ) {
			// if numerically stable use a faster technique
			rodrigues.theta = Math.acos(diagSum);
			double bottom = 2.0 * Math.sin(rodrigues.theta);

			// in cases where bottom is close to zero that means theta is also close to zero and the vector
			// doesn't matter that much
			rodrigues.unitAxisRotation.x = (R.unsafe_get(2, 1) - R.unsafe_get(1, 2)) / bottom;
			rodrigues.unitAxisRotation.y = (R.unsafe_get(0, 2) - R.unsafe_get(2, 0)) / bottom;
			rodrigues.unitAxisRotation.z = (R.unsafe_get(1, 0) - R.unsafe_get(0, 1)) / bottom;

			// in extreme underflow situations the result can be unnormalized
			rodrigues.unitAxisRotation.normalize();

			// In theory this might be more stable
			// rotationAxis( R, rodrigues.unitAxisRotation);
		} else {

			// this handles the special case where the bottom is very very small or equal to zero
			if( diagSum >= 1.0 )
				rodrigues.theta = 0;
			else if( diagSum <= -1.0 )
				rodrigues.theta = Math.PI;
			else
				rodrigues.theta = Math.acos(diagSum);

			// compute the value of x,y,z up to a sign ambiguity
			rodrigues.unitAxisRotation.x = Math.sqrt((R.get(0, 0) + 1) / 2);
			rodrigues.unitAxisRotation.y = Math.sqrt((R.get(1, 1) + 1) / 2);
			rodrigues.unitAxisRotation.z = Math.sqrt((R.get(2, 2) + 1) / 2);

			double x = rodrigues.unitAxisRotation.x;
			double y = rodrigues.unitAxisRotation.y;
			double z = rodrigues.unitAxisRotation.z;

			if (Math.abs(R.get(1, 0) - 2 * x * y) > GrlConstants.EPS) {
				x *= -1;
			}
			if (Math.abs(R.get(2, 0) - 2 * x * z) > GrlConstants.EPS) {
				z *= -1;
			}
			if (Math.abs(R.get(2,1) - 2 * z * y) > GrlConstants.EPS) {
				y *= -1;
				x *= -1;
			}

			rodrigues.unitAxisRotation.x = x;
			rodrigues.unitAxisRotation.y = y;
			rodrigues.unitAxisRotation.z = z;
		}

		return rodrigues;
	}

	/**
	 * Creates a rotation matrix about the x-axis.
	 *
	 * @param ang the angle it rotates a point by in radians.
	 * @param R (Output) Optional storage for rotation matrix.  Modified.
	 * @return The 3 by 3 rotation matrix.
	 */
	public static DMatrixRMaj rotX( double ang, DMatrixRMaj R ) {
		if( R == null )
			R = new DMatrixRMaj( 3, 3 );

		setRotX( ang, R );

		return R;
	}

	/**
	 * Sets the values in the specified matrix to a rotation matrix about the x-axis.
	 *
	 * @param ang the angle it rotates a point by in radians.
	 * @param R (Output) Storage for rotation matrix.  Modified.
	 */
	public static void setRotX( double ang, DMatrixRMaj R ) {
		double c = Math.cos( ang );
		double s = Math.sin( ang );

		R.set( 0, 0, 1 );
		R.set( 1, 1, c );
		R.set( 1, 2, -s );
		R.set( 2, 1, s );
		R.set( 2, 2, c );
	}

	/**
	 * Creates a rotation matrix about the y-axis.
	 *
	 * @param ang the angle it rotates a point by in radians.
	 * @param R (Output) Optional storage for rotation matrix.  Modified.
	 * @return The 3 by 3 rotation matrix.
	 */
	public static DMatrixRMaj rotY( double ang, DMatrixRMaj R ) {
		R = checkDeclare3x3( R );

		setRotY( ang, R );

		return R;
	}

	/**
	 * Sets the values in the specified matrix to a rotation matrix about the y-axis.
	 *
	 * @param ang the angle it rotates a point by in radians.
	 * @param r   A 3 by 3 matrix. Is modified.
	 */
	public static void setRotY( double ang, DMatrixRMaj r ) {
		double c = Math.cos( ang );
		double s = Math.sin( ang );

		r.set( 0, 0, c );
		r.set( 0, 2, s );
		r.set( 1, 1, 1 );
		r.set( 2, 0, -s );
		r.set( 2, 2, c );
	}

	/**
	 * Creates a rotation matrix about the z-axis.
	 *
	 * @param ang the angle it rotates a point by in radians.
	 * @param R (Output) Optional storage for rotation matrix.  Modified.
	 * @return The 3 by 3 rotation matrix.
	 */
	public static DMatrixRMaj rotZ( double ang, DMatrixRMaj R ) {
		R = checkDeclare3x3( R );

		setRotZ( ang, R );

		return R;
	}

	/**
	 * Sets the values in the specified matrix to a rotation matrix about the z-axis.
	 *
	 * @param ang the angle it rotates a point by in radians.
	 * @param r   A 3 by 3 matrix. Is modified.
	 */
	public static void setRotZ( double ang, DMatrixRMaj r ) {
		double c = Math.cos( ang );
		double s = Math.sin( ang );

		r.set( 0, 0, c );
		r.set( 0, 1, -s );
		r.set( 1, 0, s );
		r.set( 1, 1, c );
		r.set( 2, 2, 1 );
	}

	/**
	 * Converts an Euler coordinate into a rotation matrix.  Different type of Euler coordinates are accepted.
	 * @param type Which Euler coordinate is the input in
	 * @param rotA Angle of rotation around axis A.  First rotation
	 * @param rotB Angle of rotation around axis B   Second rotation
	 * @param rotC Angle of rotation around axis C   Third rotation
	 * @param R (Output) Optional storage for output rotation matrix
	 * @return Rotation matrix
	 */
	public static DMatrixRMaj eulerToMatrix( EulerType type ,
												double rotA, double rotB, double rotC,
												DMatrixRMaj R ) {
		R = checkDeclare3x3( R );

		DMatrixRMaj R_a = rotationAboutAxis( type.getAxisA(), rotA, null );
		DMatrixRMaj R_b = rotationAboutAxis( type.getAxisB(), rotB, null );
		DMatrixRMaj R_c = rotationAboutAxis( type.getAxisC(), rotC, null );

		DMatrixRMaj A = new DMatrixRMaj( 3, 3 );

		CommonOps_DDRM.mult( R_b, R_a, A );
		CommonOps_DDRM.mult( R_c, A, R );

		return R;
	}

	@SuppressWarnings("DuplicateExpressions")
	public static Quaternion_F64 eulerToQuaternion(EulerType type ,
												   double rotA, double rotB, double rotC,
												   Quaternion_F64 q ) {
		if( q == null )
			q = new Quaternion_F64();

		double ca = Math.cos( rotA * 0.5 );
		double sa = Math.sin( rotA * 0.5 );
		double cb = Math.cos( rotB * 0.5 );
		double sb = Math.sin( rotB * 0.5 );
		double cc = Math.cos( rotC * 0.5 );
		double sc = Math.sin( rotC * 0.5 );

		double w=0,x=0,y=0,z=0;
		switch( type ) {
			case ZYX:
				w = ca*cb*cc - sa*sb*sc;
				x = cc*sa*sb + ca*cb*sc;
				y = ca*cc*sb - cb*sa*sc;
				z = cb*cc*sa + ca*sb*sc;
				break;

			case ZYZ:
				w = ca*cb*cc - cb*sa*sc;
				x = cc*sa*sb - ca*sb*sc;
				y = ca*cc*sb + sa*sb*sc;
				z = cb*cc*sa + ca*cb*sc;
				break;

			case ZXY:
				w = ca*cb*cc + sa*sb*sc;
				x = ca*cc*sb + cb*sa*sc;
				y = -cc*sa*sb + ca*cb*sc;
				z = cb*cc*sa - ca*sb*sc;
				break;

			case ZXZ:
				w = ca*cb*cc - cb*sa*sc;
				x = ca*cc*sb + sa*sb*sc;
				y = -cc*sa*sb + ca*sb*sc;
				z = cb*cc*sa + ca*cb*sc;
				break;

			case YXZ:
				w = ca*cb*cc - sa*sb*sc;
				x = ca*cc*sb - cb*sa*sc;
				y = cb*cc*sa + ca*sb*sc;
				z = cc*sa*sb + ca*cb*sc;
				break;

			case YXY:
				w = ca*cb*cc - cb*sa*sc;
				x = ca*cc*sb + sa*sb*sc;
				y = cb*cc*sa + ca*cb*sc;
				z = cc*sa*sb - ca*sb*sc;
				break;

			case YZX:
				w = ca*cb*cc + sa*sb*sc;
				x = -cc*sa*sb + ca*cb*sc;
				y = cb*cc*sa - ca*sb*sc;
				z = ca*cc*sb + cb*sa*sc;
				break;

			case YZY:
				w = ca*cb*cc - cb*sa*sc;
				x = -cc*sa*sb + ca*sb*sc;
				y = cb*cc*sa + ca*cb*sc;
				z = ca*cc*sb + sa*sb*sc;
				break;

			case XYZ:
				w = ca*cb*cc + sa*sb*sc;
				x = cb*cc*sa - ca*sb*sc;
				y = ca*cc*sb + cb*sa*sc;
				z = -cc*sa*sb + ca*cb*sc;
				break;

			case XYX:
				w = ca*cb*cc - cb*sa*sc;
				x = cb*cc*sa + ca*cb*sc;
				y = ca*cc*sb + sa*sb*sc;
				z = -cc*sa*sb + ca*sb*sc;
				break;

			case XZY:
				w = ca*cb*cc - sa*sb*sc;
				x = cb*cc*sa + ca*sb*sc;
				y = cc*sa*sb + ca*cb*sc;
				z = ca*cc*sb - cb*sa*sc;
				break;

			case XZX:
				w = ca*cb*cc - cb*sa*sc;
				x = cb*cc*sa + ca*cb*sc;
				y = cc*sa*sb - ca*sb*sc;
				z = ca*cc*sb + sa*sb*sc;
				break;

		}

		q.set(w,x,y,z);
		return q;
	}

	/**
	 * Creates a rotation matrix about the specified axis.
	 *
	 * @param axis  0 = x, 1 = y, 2 = z
	 * @param angle The angle it is rotated by in radians.
	 * @return The 3 by 3 rotation matrix.
	 */
	private static DMatrixRMaj rotationAboutAxis(int axis, double angle, DMatrixRMaj R ) {
		switch( axis ) {
			case 0:
				return ConvertRotation3D_F64.rotX( angle, R );

			case 1:
				return ConvertRotation3D_F64.rotY( angle, R );

			case 2:
				return ConvertRotation3D_F64.rotZ( angle, R );

			default:
				throw new IllegalArgumentException( "Unknown which" );
		}
	}

	/**
	 *

	 * Finds a rotation matrix which is the optimal approximation to an arbitrary 3 by 3 matrix. Optimality
	 * is specified by the equation below:

	 *

	 * min ||R-Q||2F

	 * R

	 * where R is the rotation matrix and Q is the matrix being approximated.
	 *

	 *

	 *

	 * The technique used is based on SVD and is described in Appendix C of "A Flexible New Technique for
	 * Camera Calibration" Technical Report, updated 2002.
	 *

	 *

	 *

	 * Both origin and R can be the same instance.
	 *

	 *
	 * @param orig Input approximate rotation matrix.  Not modified.
	 * @param R (Optional) Storage for the approximated rotation matrix.  Modified.
	 * @return Rotation matrix
	 */
	public static DMatrixRMaj approximateRotationMatrix( DMatrixRMaj orig, DMatrixRMaj R ) {
		R = checkDeclare3x3( R );

		SingularValueDecomposition<DMatrixRMaj> svd =
				DecompositionFactory_DDRM.svd( orig.numRows, orig.numCols ,true,true,false);

		if( !svd.decompose( orig ) )
			throw new RuntimeException( "SVD Failed" );

		CommonOps_DDRM.mult( svd.getU( null,false ), svd.getV( null,true ), R );

		// svd does not guarantee that U anv V have positive determinants.
		double det = CommonOps_DDRM.det( R );

		if( det < 0 )
			CommonOps_DDRM.scale( -1, R );

		return R;
	}

	/**
	 *
Converts a unit quaternion into a rotation matrix.

	 *
	 *

	 * Equations is taken from: Paul J. Besl and Neil D. McKay, "A Method for Registration of 3-D Shapes" IEEE
	 * Transactions on Pattern Analysis and Machine Intelligence, Vol 14, No. 2, Feb. 1992
	 *

	 *
	 * @param quat Unit quaternion.
	 * @param R Storage for rotation matrix.  If null a new matrix is created. Modified.
	 * @return Rotation matrix
	 */
	public static DMatrixRMaj quaternionToMatrix( Quaternion_F64 quat, DMatrixRMaj R ) {
		return quaternionToMatrix(quat.w,quat.x,quat.y,quat.z,R);
	}

	@SuppressWarnings("UnnecessaryLocalVariable")
	public static DMatrixRMaj quaternionToMatrix(double w, double x , double y , double z , DMatrixRMaj R )
	{
		R = checkDeclare3x3( R );

		final double q0 = w;
		final double q1 = x;
		final double q2 = y;
		final double q3 = z;

		R.data[0] = q0*q0 + q1*q1 - q2*q2 - q3*q3; // (0,0)
		R.data[1] = 2.0*( q1*q2 - q0*q3 );         // (0,1)
		R.data[2] = 2.0*( q1*q3 + q0*q2 );         // (0,2)

		R.data[3] = 2.0*( q1*q2 + q0*q3 );         // (1,0)
		R.data[4] = q0*q0 - q1*q1 + q2*q2 - q3*q3; // (1,1)
		R.data[5] = 2.0*( q2*q3 - q0*q1 );         // (1,2)

		R.data[6] = 2.0*( q1*q3 - q0*q2 );         // (2,0)
		R.data[7] = 2.0*( q2*q3 + q0*q1 );         // (2,1)
		R.data[8] = q0*q0 - q1*q1 - q2*q2 + q3*q3; // (2,2)

		return R;
	}

	private static DMatrixRMaj checkDeclare3x3( DMatrixRMaj R ) {
		if( R == null ) {
			R = new DMatrixRMaj( 3, 3 );
		} else {
			if( R.numRows != 3 || R.numCols != 3 )
				throw new IllegalArgumentException( "Expected 3 by 3 matrix." );
		}
		return R;
	}
}


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

import georegression.geometry.UtilLine2D_F64;
import georegression.struct.point.Point2D_F64;
import georegression.struct.point.Vector2D_F64;
import org.ejml.FancyPrint;

import java.io.Serializable;

/**
 *

 * 2D line parameterized using parametric equation:

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

 * where t specifies the location along the line, (x_0,y_0) is an arbitrary point on the line,
 * and (slopeX,slopeY).
 *

 *
 * @see UtilLine2D_F64
 */
public class LineParametric2D_F64 implements Serializable {
	/**
	 * A point on the line
	 */
	public Point2D_F64 p = new Point2D_F64();
	/**
	 * The line's slope
	 */
	public Vector2D_F64 slope = new Vector2D_F64();

	public LineParametric2D_F64( double x_0, double y_0, double slopeX, double slopeY ) {
		p.set( x_0, y_0 );
		slope.set( slopeX, slopeY );
	}

	public LineParametric2D_F64( Point2D_F64 p, Vector2D_F64 slope ) {
		setPoint(p);
		setSlope(slope);
	}

	/**
	 * Creates a line defined from two points. The slope will be in the direction from a to b.
	 *
	 * @see UtilLine2D_F64#convert(Point2D_F64, Point2D_F64, LineParametric2D_F64)
	 * @param a point on line
	 * @param b point on line which isn't a
	 */
	public LineParametric2D_F64( Point2D_F64 a , Point2D_F64 b ) {
		UtilLine2D_F64.convert(a,b,this);
	}

	public LineParametric2D_F64() {
	}

	public void set( LineParametric2D_F64 line ) {
		this.p.set(line.p);
		this.slope.set(line.slope);
	}

	public void setPoint( Point2D_F64 pt ) {
		this.p.set( pt );
	}

	public void setPoint( double x, double y ) {
		this.p.x = x;
		this.p.y = y;
	}

	public void setSlope( Vector2D_F64 slope ) {
		this.slope.set( slope );
	}

	public void setSlope( double slopeX, double slopeY ) {
		this.slope.x = slopeX;
		this.slope.y = slopeY;
	}

	/**
	 * Sets the slope to the unit vector specified by the provided angle.
	 *
	 * @param angle Angle of the line specified in radians.
	 */
	public void setAngle( double angle ) {
		slope.set( Math.cos( angle ), Math.sin( angle ) );
	}

	public double getAngle() {
		return Math.atan2( slope.y, slope.x );
	}

	/**
	 * Returns a point along the line.  See parametric equation in class description.
	 *
	 * @param t Location along the line.
	 * @return Point on the line.
	 */
	public Point2D_F64 getPointOnLine( double t ) {
		return new Point2D_F64( slope.x * t + p.x, slope.y * t + p.y );
	}

	public void getPointOnLine(double t , Point2D_F64 x ) {
		x.x = slope.x*t + p.x;
		x.y = slope.y*t + p.y;
	}

	public Point2D_F64 getPoint() {
		return p;
	}

	public final double getSlopeX() {
		return slope.x;
	}

	public final double getSlopeY() {
		return slope.y;
	}

	public final double getX() {
		return p.x;
	}

	public final double getY() {
		return p.y;
	}

	public Point2D_F64 getP() {
		return p;
	}

	public void setP(Point2D_F64 p) {
		this.p = p;
	}

	public Vector2D_F64 getSlope() {
		return slope;
	}

	public LineParametric2D_F64 copy() {
		return new LineParametric2D_F64( p, slope );
	}

	@Override
	public String toString() {
		FancyPrint f = new FancyPrint();
		return getClass().getSimpleName()+" P( "+f.s(p.x)+" "+f.s(p.y)+" ) Slope( "+f.s(slope.x)+" "+f.s(slope.y)+" )";
	}

	@Override
	public boolean equals(Object obj) {
		try {
			LineParametric2D_F64 o = (LineParametric2D_F64) obj;
			return p.equals(o.p) && slope.equals(o.slope);
		} catch( RuntimeException e ) {
			return super.equals(obj);
		}
	}
}


/*
 * 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.transform.twist;

import georegression.geometry.ConvertRotation3D_F64;
import georegression.geometry.GeometryMath_F64;
import georegression.misc.GrlConstants;
import georegression.struct.point.Vector3D_F64;
import georegression.struct.se.Se3_F64;
import georegression.struct.so.Rodrigues_F64;
import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.CommonOps_DDRM;
import org.ejml.dense.row.MatrixFeatures_DDRM;

/**
 * Operations related to twists.
 *
 * @author Peter Abeles
 */
public class TwistOps_F64 {

	/**
	 * Converts the {@link Se3_F64} object into homogenous notation.

	 *
	 * [ R , T; 0 , 1]
	 *
	 * @param transform transform in regular SE3 format.  If null a new matrix will be declared
	 * @param H (Optional) Storage for homogenous 4x4 matrix.  If null a new matrix is declared.
	 * @return Homogenous matrix
	 */
	public static DMatrixRMaj homogenous( Se3_F64 transform , DMatrixRMaj H ) {
		if( H == null ) {
			H = new DMatrixRMaj(4,4);
		} else {
			H.reshape(4,4);
		}
		CommonOps_DDRM.insert(transform.R,H,0,0);
		H.data[3] = transform.T.x;
		H.data[7] = transform.T.y;
		H.data[11] = transform.T.z;

		H.data[12] =0;H.data[13] =0;H.data[14] =0;H.data[15] =1;

		return H;
	}

	/**
	 * Converts the twist coordinate into homogenous format.

	 * H = [hat(w), v; 0 , 0]
	 *
	 * @param twist Twist coordinate
	 * @param H (Optional) Storage for homogenous 4x4 matrix.  If null a new matrix is declared.
	 * @return Homogenous matrix
	 */
	public static DMatrixRMaj homogenous( TwistCoordinate_F64 twist , DMatrixRMaj H ) {
		if( H == null ) {
			H = new DMatrixRMaj(4,4);
		} else {
			H.reshape(4,4);
			H.data[12] = 0; H.data[13] = 0; H.data[14] = 0; H.data[15] = 0;
		}

		H.data[0] =  0;         H.data[1] = -twist.w.z; H.data[2]  =  twist.w.y; H.data[3]  = twist.v.x;
		H.data[4] =  twist.w.z; H.data[5] = 0;          H.data[6]  = -twist.w.x; H.data[7]  = twist.v.y;
		H.data[8] = -twist.w.y; H.data[9] =  twist.w.x; H.data[10] =  0;         H.data[11] = twist.v.z;

		return H;
	}

	/**
	 *

	 *     Computes the exponential map for a twist:

	 *     exp(hat(xi)*theta) = [ SO , (I-SO)*(w cross v) + w*wTvΘ 0 , 1]

	 *     SO = exp(hat(w)*theta)
	 *

	 *
	 * See page 42 in R. Murray, et. al. "A Mathematical Introduction to ROBOTIC MANIPULATION" 1994
	 *
	 * @param twist Twist coordinate
	 * @param theta Magnitude of rotation
	 * @param motion Storage for SE(3).  If null a new instance will be returned.
	 * @return The transformation.
	 */
	public static Se3_F64 exponential(TwistCoordinate_F64 twist , double theta , Se3_F64 motion ) {
		if( motion == null ) {
			motion = new Se3_F64();
		}

		double w_norm = twist.w.norm();

		if( w_norm == 0.0 ) {
			CommonOps_DDRM.setIdentity(motion.R);
			motion.T.x = twist.v.x*theta;
			motion.T.y = twist.v.y*theta;
			motion.T.z = twist.v.z*theta;
			return motion;
		}

		DMatrixRMaj R = motion.getR();

		// First handle the SO region.  This Rodrigues equation
		double wx = twist.w.x/w_norm, wy = twist.w.y/w_norm, wz = twist.w.z/w_norm;

		ConvertRotation3D_F64.rodriguesToMatrix(wx,wy,wz, theta*w_norm, R);

		theta *= w_norm;

		// Now compute the translational component
		// (I - SO)*(w cross v) + w*w'*v*theta
		double vx = twist.v.x, vy = twist.v.y, vz = twist.v.z;

		double wv_x = wy*vz - wz*vy;
		double wv_y = wz*vx - wx*vz;
		double wv_z = wx*vy - wy*vx;

		double left_x = (1 - R.data[0])*wv_x -      R.data[1]*wv_y  -      R.data[2]*wv_z;
		double left_y =     -R.data[3]*wv_x  + (1 - R.data[4])*wv_y -      R.data[5]*wv_z;
		double left_z =     -R.data[6]*wv_x  -      R.data[7]*wv_y  + (1 - R.data[8])*wv_z;

		double right_x = (wx*wx*vx + wx*wy*vy + wx*wz*vz)*theta;
		double right_y = (wy*wx*vx + wy*wy*vy + wy*wz*vz)*theta;
		double right_z = (wz*wx*vx + wz*wy*vy + wz*wz*vz)*theta;

		motion.T.x = (double)left_x + right_x;
		motion.T.y = (double)left_y + right_y;
		motion.T.z = (double)left_z + right_z;
		motion.T.divide(w_norm);

		return motion;
	}

	/**
	 * Converts a rigid body motion into a twist coordinate.  The value of theta used to generate the motion
	 * is assumed to be one.
	 *
	 * @param motion (Input) The SE(3) transformation
	 * @param twist (Output) Storage for twist.
	 * @return magnitude of the motion
	 */
	public static TwistCoordinate_F64 twist( Se3_F64 motion , TwistCoordinate_F64 twist ) {
		if( twist == null )
			twist = new TwistCoordinate_F64();

		if(MatrixFeatures_DDRM.isIdentity(motion.R, GrlConstants.TEST_F64)) {
			twist.w.set(0,0,0);
			twist.v.set(motion.T);
		} else {
			Rodrigues_F64 rod = new Rodrigues_F64();
			ConvertRotation3D_F64.matrixToRodrigues(motion.R,rod);

			twist.w.set(rod.unitAxisRotation);
			double theta = rod.theta;

			// A = (I-SO)*hat(w) + w*w'*theta
			DMatrixRMaj A = CommonOps_DDRM.identity(3);
			CommonOps_DDRM.subtract(A,motion.R, A);

			DMatrixRMaj w_hat = GeometryMath_F64.crossMatrix(twist.w,null);
			DMatrixRMaj tmp = A.copy();
			CommonOps_DDRM.mult(tmp,w_hat,A);

			Vector3D_F64 w = twist.w;
			A.data[0] += w.x*w.x*theta; A.data[1] += w.x*w.y*theta; A.data[2] += w.x*w.z*theta;
			A.data[3] += w.y*w.x*theta; A.data[4] += w.y*w.y*theta; A.data[5] += w.y*w.z*theta;
			A.data[6] += w.z*w.x*theta; A.data[7] += w.z*w.y*theta; A.data[8] += w.z*w.z*theta;

			DMatrixRMaj y = new DMatrixRMaj(3,1);
			y.data[0] = motion.T.x;
			y.data[1] = motion.T.y;
			y.data[2] = motion.T.z;

			DMatrixRMaj x = new DMatrixRMaj(3,1);

			CommonOps_DDRM.solve(A,y,x);

			twist.w.scale(rod.theta);
			twist.v.x = (double) x.data[0];
			twist.v.y = (double) x.data[1];
			twist.v.z = (double) x.data[2];
			twist.v.scale(rod.theta);
		}
		return twist;
	}
}


/*
 * 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.transform.se;

import georegression.geometry.ConvertRotation3D_F64;
import georegression.struct.point.Vector3D_F64;
import georegression.struct.se.Se3_F64;
import georegression.struct.so.Rodrigues_F64;
import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.CommonOps_DDRM;

/**
 *

 * Linear interpolation between two {@link georegression.struct.se.Se3_F64} transforms.  It is assumed that
 * the rotation able between the two transforms is {@code < 180} degrees.  If more than 180 degrees than it will chose
 * the wrong direction to rotate.
 *

 *

 * The rotation and translation are interpolated separately.  Translation has straight forward linear interpolation
 * applied to it.  Rotations are interpolated by finding the axis of rotation and the rotation angle.  Then
 * the angle is linearly interpolated and the rotation matrix found by the angle and axis.
 *

 * @author Peter Abeles
 */
public class InterpolateLinearSe3_F64 {

	// transform at the start
	Se3_F64 initial = new Se3_F64();
	// rodrigues representation of the rotation
	Rodrigues_F64 rotation = new Rodrigues_F64();
	// magnitude of the rotation.  between initial and end
	double rotMagnitude;
	// translation between initial and end
	Vector3D_F64 translation = new Vector3D_F64();

	// difference between the initial transform and the end or the output
	DMatrixRMaj R = new DMatrixRMaj(3,3);

	/**
	 * Specify the two transforms which values are to be interpolated between
	 * @param initial first transform
	 * @param end second transform
	 */
	public void setTransforms( Se3_F64 initial , Se3_F64 end) {
		this.initial.set(initial);

		translation.x = end.T.x - initial.T.x;
		translation.y = end.T.y - initial.T.y;
		translation.z = end.T.z - initial.T.z;

		CommonOps_DDRM.multTransA(initial.getR(), end.getR(), R);

		ConvertRotation3D_F64.matrixToRodrigues(R,rotation);
		rotMagnitude = rotation.theta;
	}

	/**
	 * Interpolates a value between the first and second transform.  A value close to 0 will be
	 * more similar to the initial and 1 more similar to the end.
	 * @param where A value between 0 and 1 which specifies the interpolation location
	 * @param output Resulting transform
	 */
	public void interpolate( double where , Se3_F64 output ) {
		rotation.setTheta(where*rotMagnitude);
		ConvertRotation3D_F64.rodriguesToMatrix(rotation,R);

		output.T.x = initial.T.x + where*translation.x;
		output.T.y = initial.T.y + where*translation.y;
		output.T.z = initial.T.z + where*translation.z;

		CommonOps_DDRM.mult(initial.R,R,output.R);
	}

}


/*
 * 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.struct.shapes;

import georegression.struct.point.Point2D_F64;

import java.io.Serializable;

/**
 * An axis aligned rectangle in 2D that is specified by its lower-extent p0, and upper-extent p1.
 */
public class Rectangle2D_F64 implements Serializable {
	/**
	 * Lower extent
	 */
	public Point2D_F64 p0 = new Point2D_F64();
	/**
	 * Upper extent
	 */
	public Point2D_F64 p1 = new Point2D_F64();

	public Rectangle2D_F64(double x0, double y0, double x1, double y1) {
		set(x0,y0,x1,y1);
	}

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

	public void set( Rectangle2D_F64 orig ) {
		this.p0.set(orig.p0);
		this.p1.set(orig.p1);
	}

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

	public Rectangle2D_F64() {
	}

	/**
	 * Makes sure x0,y0 is the lower extent and x1,y1 is the upper extent
	 */
	public void enforceExtents() {
		if( p1.x < p0.x ) {
			double tmp = p1.x;
			p1.x = p0.x;
			p0.x = tmp;
		}
		if( p1.y < p0.y ) {
			double tmp = p1.y;
			p1.y = p0.y;
			p0.y = tmp;
		}
	}

	public double getWidth() {
		return p1.x-p0.x;
	}

	public double getHeight() {
		return p1.y-p0.y;
	}

	public double area() {
		return ( p1.y-p0.y)*(p1.x-p0.x);
	}

	public Point2D_F64 getP0() {
		return p0;
	}

	public void setP0(Point2D_F64 p0) {
		this.p0 = p0;
	}

	public Point2D_F64 getP1() {
		return p1;
	}

	public void setP1(Point2D_F64 p1) {
		this.p1 = p1;
	}

	@Override
	public String toString() {
		return getClass().getSimpleName()+"{ p0("+p0.x+" "+p0.y+") p1("+p1.x+" "+p1.y+") }";
	}

	@Override
	public boolean equals(Object obj) {
		try {
			Rectangle2D_F64 r = (Rectangle2D_F64)obj;
			return p0.x==r.p0.x&&p0.y==r.p0.y&&p1.x==r.p1.x&&p1.y==r.p1.y;
		} catch( RuntimeException e){
			return false;
		}
	}
}


/*
 * 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.struct.point.Point2D_F64;
import georegression.struct.point.Point3D_F64;
import georegression.struct.se.Se3_F64;
import georegression.struct.shapes.Polygon2D_F64;
import georegression.transform.se.SePointOps_F64;
import org.ddogleg.struct.FastQueue;

/**
 * @author Peter Abeles
 */
public class UtilShape3D_F64 {
	/**
	 * Converts a 2D polygon into a 3D polygon. The 2D points will lie on the x-y plane (e.g. (x,y,0)) and are
	 * converted to 3D using polyToWorld.
	 *
	 * @param polygon2D (Input) polygon
	 * @param polyToWorld (Output) transform from 2D to 3D coordinate system.
	 * @param output
	 */
	public static void polygon2Dto3D(Polygon2D_F64 polygon2D , Se3_F64  polyToWorld , FastQueue<Point3D_F64> output )
	{
		output.resize(polygon2D.size());

		for (int i = 0; i < polygon2D.size(); i++) {
			Point2D_F64 p2 = polygon2D.get(i);
			Point3D_F64 p3 = output.get(i);
			p3.set(p2.x, p2.y,0);
			SePointOps_F64.transform(polyToWorld,p3,p3);
		}
	}
}


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

import georegression.struct.line.LinePolar2D_F64;
import georegression.struct.point.Point2D_F64;

import java.util.List;

/**
 * Finds the best file line given a set of observations.
 *
 * @author Peter Abeles
 */
public class FitLine_F64 {

	/**
	 *

	 * 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_F64> points , LinePolar2D_F64 ret ) {
		if( ret == null )
			ret = new LinePolar2D_F64();

		double meanX = 0;
		double meanY = 0;

		final int N = points.size();
		for( int i = 0; i < N; i++ ) {
			Point2D_F64 p = points.get(i);
			meanX += p.x;
			meanY += p.y;
		}
		meanX /= N;
		meanY /= N;

		double top = 0;
		double bottom = 0;

		for( int i = 0; i < N; i++ ) {
			Point2D_F64 p = points.get(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 = (double)( meanX*Math.cos(ret.angle) + meanY*Math.sin(ret.angle));

		return ret;
	}

	/**
	 *

	 * Computes the weighted 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 weights Weight for each point.  weights[i] ≥ 0
	 * @param ret Storage for the line.  If null a new line will be declared.
	 * @return Best fit line. or null if sum of weights is zero
	 */
	public static LinePolar2D_F64 polar( List<Point2D_F64> points , double weights[] , LinePolar2D_F64 ret ) {

		final int N = points.size();
		double totalWeight = 0;
		for( int i = 0; i < N; i++ ) {
			totalWeight += weights[i];
		}

		if( totalWeight == 0 )
			return null;

		if( ret == null )
			ret = new LinePolar2D_F64();

		double meanX = 0;
		double meanY = 0;

		for( int i = 0; i < N; i++ ) {
			Point2D_F64 p = points.get(i);
			double w = weights[i];
			meanX += w*p.x;
			meanY += w*p.y;
		}
		meanX /= totalWeight;
		meanY /= totalWeight;

		double top = 0;
		double bottom = 0;

		for( int i = 0; i < N; i++ ) {
			Point2D_F64 p = points.get(i);
			double w = weights[i];
			double dx = meanX - p.x;
			double dy = meanY - p.y;

			top += w*dx*dy;
			bottom += w*(dy*dy - dx*dx);
		}
		top /= totalWeight;
		bottom /= totalWeight;

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

		return ret;
	}
}


/*
 * 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.fitting.plane;

import georegression.struct.point.Point3D_F64;
import georegression.struct.point.Vector3D_F64;
import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.linsol.qr.SolveNullSpaceQRP_DDRM;
import org.ejml.interfaces.SolveNullSpace;

import java.util.List;

/**
 * Various functions for fitting planes in 3D to point clouds.
 *
 * @author Peter Abeles
 */
public class FitPlane3D_F64 {

	SolveNullSpace<DMatrixRMaj> solverNull = new SolveNullSpaceQRP_DDRM();

	DMatrixRMaj A = new DMatrixRMaj(3,3);
	DMatrixRMaj nullspace = new DMatrixRMaj(3,1);

	/**
	 * SVD based method for fitting a plane to a set of points.  The plane's equation is returned
	 * as a point on the plane and the normal vector.
	 *
	 * @param points (Input) Set of points on a plane.
	 * @param outputCenter (Output) Centroid of the passed in points. Modified.
	 * @param outputNormal (Output) Vector tangent to the plane.  Normalized.  Modified.
	 * @return true if successful or false if it failed.
	 */
	public boolean svd( List<Point3D_F64> points , Point3D_F64 outputCenter , Vector3D_F64 outputNormal ) {

		final int N = points.size();

		// find the centroid
		outputCenter.set(0,0,0);
		for( int i = 0; i < N; i++ ) {
			Point3D_F64 p = points.get(i);
			outputCenter.x += p.x;
			outputCenter.y += p.y;
			outputCenter.z += p.z;
		}

		outputCenter.x /= N;
		outputCenter.y /= N;
		outputCenter.z /= N;

		return solvePoint(points,outputCenter,outputNormal);
	}

	/**
	 * SVD based method for fitting a plane to a set of points and a known point on the plane.  The plane's
	 * equation is returned as a point on the plane and the normal vector.
	 *
	 * @param points (Input)Set of points on a plane.
	 * @param pointOnPlane (Input) A known point on the plane
	 * @param outputNormal (Output) Vector tangent to the plane.  Normalized. Modified.
	 * @return true if successful or false if it failed.
	 */
	public boolean solvePoint(List<Point3D_F64> points , Point3D_F64 pointOnPlane , Vector3D_F64 outputNormal ) {

		final int N = points.size();

		// construct the matrix
		A.reshape(N,3);
		int index = 0;
		for( int i = 0; i < N; i++ ) {
			Point3D_F64 p = points.get(i);
			A.data[index++] = p.x - pointOnPlane.x;
			A.data[index++] = p.y - pointOnPlane.y;
			A.data[index++] = p.z - pointOnPlane.z;
		}

		// decompose and find the singular value
		if( !solverNull.process(A,1,nullspace) )
			return false;

		// the normal is the singular vector
		outputNormal.x = (double) nullspace.unsafe_get(0,0);
		outputNormal.y = (double) nullspace.unsafe_get(1,0);
		outputNormal.z = (double) nullspace.unsafe_get(2,0);

		return true;
	}
}


/*
 * 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.geometry.UtilPolygons2D_F64;
import georegression.metric.Area2D_F64;
import georegression.metric.Intersection2D_F64;
import georegression.struct.line.LineSegment2D_F64;
import georegression.struct.point.Point2D_F64;
import org.ddogleg.struct.FastQueue;
import org.ejml.ops.MatrixIO;

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

import static org.ejml.UtilEjml.fancyStringF;

/**
 * Describes a polygon in 2D.
 *
 * @author Peter Abeles
 */
public class Polygon2D_F64 implements Serializable {

	// vertexes in the polygon
	public FastQueue<Point2D_F64> vertexes;

	public Polygon2D_F64( double[][] a ) {
		this(a.length);

		for (int i = 0; i < a.length; i++) {
			vertexes.get(i).set(a[i][0], a[i][1]);
		}
	}

	public Polygon2D_F64( Polygon2D_F64 a ) {
		vertexes = new FastQueue<Point2D_F64>(a.size(),Point2D_F64.class,true);
		for (int i = 0; i < a.size(); i++) {
			vertexes.grow().set(a.get(i));
		}
	}

	public Polygon2D_F64( int numVertexes ) {
		vertexes = new FastQueue<Point2D_F64>(numVertexes, Point2D_F64.class,true);

		vertexes.growArray(numVertexes);
		vertexes.size = numVertexes;
	}

	public Polygon2D_F64( double... points ) {
		if( points.length % 2 == 1 )
			throw new IllegalArgumentException("Expected an even number");
		vertexes = new FastQueue<Point2D_F64>(points.length/2,Point2D_F64.class,true);
		vertexes.growArray(points.length/2);
		vertexes.size = points.length/2;

		int count = 0;
		for (int i = 0; i < points.length; i += 2) {
			vertexes.data[count++].set( points[i],points[i+1]);
		}
	}

	public Polygon2D_F64() {
		vertexes = new FastQueue<>(Point2D_F64.class,true);
	}

	public void set( Polygon2D_F64 orig ) {
		vertexes.resize(orig.size());
		for (int i = 0; i < orig.size(); i++) {
			vertexes.data[i].set( orig.vertexes.data[i]);
		}
	}

	public void set( int index , double x , double y ) {
		vertexes.data[index].set(x,y);
	}

	/**
	 * Resturns the length of the specified side that is composed of point index and index+1
	 * @return Euclidean length of the side
	 */
	public double getSideLength( int index ) {
		Point2D_F64 a = vertexes.get( index );
		Point2D_F64 b = vertexes.get( (index+1)%vertexes.size );

		return (double)a.distance(b);
	}

	public Point2D_F64 get( int index ) {
		return vertexes.data[index];
	}

	public int size() {
		return vertexes.size();
	}

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

	/**
	 * Returns the area for simply polygons.  Non-self intersecting convex or concave.
	 * @return area
	 */
	public double areaSimple() {
		return Area2D_F64.polygonSimple(this);
	}

	/**
	 * Returns true if the point is inside the polygon.  Points along the border are ambiguously considered inside
	 * or outside.
	 *
	 * @see Intersection2D_F64#containConcave(Polygon2D_F64, Point2D_F64)
	 * @see Intersection2D_F64#containConcave(Polygon2D_F64, Point2D_F64)
	 *
	 * @param p A point
	 * @return true if inside and false if outside
	 */
	public boolean isInside( Point2D_F64 p ) {
		if( isConvex() ) {
			return Intersection2D_F64.containConvex(this,p);
		} else {
			return Intersection2D_F64.containConcave(this,p);
		}
	}

	/**
	 * true if the order of vertexes is in counter clockwise order.
	 * @return true if ccw or false if cw
	 */
	public boolean isCCW() {
		return UtilPolygons2D_F64.isCCW(vertexes.toList());
	}

	public boolean isConvex() {
		return UtilPolygons2D_F64.isConvex(this);
	}

	public boolean isIdentical( Polygon2D_F64 a , double tol ) {
		return UtilPolygons2D_F64.isIdentical(this,a,tol);
	}

	public boolean isEquivalent( Polygon2D_F64 a , double tol ) {
		return UtilPolygons2D_F64.isEquivalent(this, a, tol);
	}

	public void flip() {
		UtilPolygons2D_F64.flip(this);
	}

	/**
	 * Returns the line/edge defined by vertex index and index+1.
	 * @param index Index of the line
	 * @return A new instance of the line segment.
	 */
	public LineSegment2D_F64 getLine( int index , LineSegment2D_F64 storage ) {
		if( storage == null )
			storage = new LineSegment2D_F64();

		int j = (index+1)%vertexes.size;

		storage.a.set(get(index));
		storage.b.set(get(j));

		return storage;
	}

	/**
	 * 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 < vertexes.size; i++) {
				storage.add( vertexes.get(i).copy() );
			}
		} else {
			storage.addAll(vertexes.toList());
		}
		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 ) {
		vertexes.resize(list.size());
		for (int i = 0; i < list.size(); i++) {
			vertexes.data[i].set( list.get(i));
		}
	}

	@Override
	public String toString() {
		final int length = MatrixIO.DEFAULT_LENGTH;
		DecimalFormat format = new DecimalFormat("#");
		String out = getClass().getSimpleName()+"{ order "+vertexes.size+" : vertexes [ ";

		for (int i = 0; i < vertexes.size; i++) {
			Point2D_F64 p = vertexes.get(i);
			out += "( "+ fancyStringF(p.x,format,length,4)+" , "+fancyStringF(p.y,format,length,4)+" ) ";
		}

		out += "] }";

		return out;
	}
}


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

import georegression.struct.curve.ConicGeneral_F64;
import georegression.struct.curve.ParabolaGeneral_F64;
import georegression.struct.curve.ParabolaParametric_F64;
import org.ejml.data.DMatrix3x3;
import org.ejml.data.DMatrixRMaj;

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

	/**
	 * Converts symmetric 3x3 matrix back into a conic.
	 *
	 *

	 *        [A    B/2  D/2]
	 *  dst = [B/2  C    E/2]
	 *        [D/2  E/2  F  ]
	 *

	 *
	 */
	public static DMatrixRMaj convert(ConicGeneral_F64 src , DMatrixRMaj dst )
	{
		if( dst == null )
			dst = new DMatrixRMaj(3,3);
		else
			dst.reshape(3,3);

		double B = src.B/2.0;
		double D = src.D/2.0;
		double E = src.E/2.0;

		dst.data[0] = src.A; dst.data[1] = B;     dst.data[2] = D;
		dst.data[3] = B;     dst.data[4] = src.C; dst.data[5] = E;
		dst.data[6] = D;     dst.data[7] = E;     dst.data[8] = src.F;

		return dst;
	}

	/**
	 * Converts symmetric 3x3 matrix back into a conic. Only the upper right
	 * portion of src is read.
	 *
	 *

	 *     A = DST(0,0)
	 *     B = DST(0,1)*2
	 *     D = DST(0,2)*2
	 *     C = DST(1,1)
	 *     E = DST(1,2)*2
	 *     F = DST(2,2)
	 *

	 */
	public static ConicGeneral_F64 convert(DMatrix3x3 src , ConicGeneral_F64 dst ) {
		if( dst == null )
			dst = new ConicGeneral_F64();

		dst.A = src.a11; dst.B = 2*src.a12; dst.D = 2*src.a13;
		dst.C = src.a22; dst.E = 2*src.a23;
		dst.F = src.a33;

		return dst;
	}

	/**
	 * Converts the conic into a symmetric 3x3 matrix
	 *
	 *

	 *        [A    B/2  D/2]
	 *  dst = [B/2  C    E/2]
	 *        [D/2  E/2  F  ]
	 *

	 */
	public static DMatrix3x3 convert(ConicGeneral_F64 src , DMatrix3x3 dst )
	{
		if( dst == null )
			dst = new DMatrix3x3();

		double B = src.B/2.0;
		double D = src.D/2.0;
		double E = src.E/2.0;

		dst.a11 = src.A; dst.a12 = B;     dst.a13 = D;
		dst.a21 = B;     dst.a22 = src.C; dst.a23 = E;
		dst.a31 = D;     dst.a32 = E;     dst.a33 = src.F;

		return dst;
	}

	/**
	 * Converts symmetric 3x3 matrix back into a conic. Only the upper right
	 * portion of src is read.
	 *
	 *

	 *     A = DST(0,0)
	 *     B = DST(0,1)*2
	 *     D = DST(0,2)*2
	 *     C = DST(1,1)
	 *     E = DST(1,2)*2
	 *     F = DST(2,2)
	 *

	 *
	 */
	public static ConicGeneral_F64 convert( DMatrixRMaj src , ConicGeneral_F64 dst ) {
		if( dst == null )
			dst = new ConicGeneral_F64();

		dst.A = src.data[0]; dst.B = 2*src.data[1]; dst.D = 2*src.data[2];
		dst.C = src.data[4]; dst.E = 2*src.data[5];
		dst.F = src.data[8];

		return dst;
	}

	/**
	 * Converts the conic into a parabola. If the conic isn't a parabola then it is converted into one
	 * by adjusting the value of B.
	 *
	 * @param src (Input) Conic
	 * @param dst (Output) Optional storage for converted parabola
	 * @return Parabola
	 */
	public static ParabolaGeneral_F64 convert(ConicGeneral_F64 src , ParabolaGeneral_F64 dst ) {
		if( dst == null )
			dst = new ParabolaGeneral_F64();

		// NOTE haven't put much through if this is the correct way to handle negative values of A or C
		dst.A = Math.signum(src.A) * Math.sqrt(Math.abs(src.A));
		dst.C = Math.signum(src.C) * Math.sqrt(Math.abs(src.C));
		dst.D = src.D;
		dst.E = src.E;
		dst.F = src.F;

		return dst;
	}

	/**
	 * Converts the parabola into a conic.
	 *
	 * @param src (Input) Parabola
	 * @param dst (Output) Optional storage for converted conic
	 * @return Conic
	 */
	public static ConicGeneral_F64 convert(ParabolaGeneral_F64 src , ConicGeneral_F64 dst ) {
		if( dst == null )
			dst = new ConicGeneral_F64();

		dst.A = src.A*src.A;
		dst.B = src.A*src.C*2.0;
		dst.C = src.C*src.C;
		dst.D = src.D;
		dst.E = src.E;
		dst.F = src.F;

		return dst;
	}

//	public static void axisOfSymmetry(ParabolaGeneral_F64 curve , LineGeneral2D_F64 axis ) {
//		double A = curve.A;
//		double C = curve.C;
//		double D = curve.D;
//		double E = curve.E;
//
//		axis.A = A;
//		axis.B = C;
//		axis.C = (A*D + C*E)/(2*(A*A + C*C));
//	}
//
//	public static void vertex(ParabolaGeneral_F64 curve , Point2D_F64 vertex ) {
//
//	}

	public static ParabolaParametric_F64 convert( ParabolaGeneral_F64 src , ParabolaParametric_F64 dst ) {
		if( dst == null )
			dst = new ParabolaParametric_F64();

		double A = src.A;
		double C = src.C;
		double D = src.D;
		double E = src.E;
		double F = src.F;

		double bottom = C*D-A*E;

		if( bottom == 0 ) {
			throw new RuntimeException("Not a parabola");
		} else {
			dst.A = -C / bottom;
			dst.B = E / bottom;
			dst.C = -C * F / bottom;

			dst.D = A / bottom;
			dst.E = -D / bottom;
			dst.F = A * F / bottom;
		}

		return dst;
	}

	public static ParabolaGeneral_F64 convert( ParabolaParametric_F64 src , ParabolaGeneral_F64 dst ) {
		if( dst == null )
			dst = new ParabolaGeneral_F64();

		double Z = src.A*src.E - src.B*src.D;

		dst.A = src.D/Z;
		dst.C = -src.A/Z;
		dst.D = -src.E/Z;
		dst.E = src.B/Z;

		if( Math.abs(dst.A) > Math.abs(dst.C) ) {
			dst.F = src.F/Z/dst.A;
		} else if( dst.C != 0 ) {
			dst.F = -src.C/Z/dst.C;
		} else {
			dst.F = 0;
		}

		return dst;
	}

}