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

import georegression.geometry.UtilLine2D_F64;
import georegression.metric.Intersection2D_F64;
import georegression.struct.curve.EllipseRotated_F64;
import georegression.struct.line.LineGeneral2D_F64;
import georegression.struct.line.LineSegment2D_F64;
import georegression.struct.point.Point2D_F64;

import static georegression.geometry.UtilEllipse_F64.tangentLines;

/**
 *

 * Iterative algorithm for finding the 4 pairs of tangent lines between two ellipses.  The ellipses are assumed
 * to 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.
 *

 *
 * Algorithm: While a closed form solution does exist, it is very complex and an iterative solution is used
 * here instead.
 *

 *
Initialize by finding four lines which are approximately tangent.  Two will cross the center line
 *     and two will not.  See code for details

 *
For each end point on a line find the two tangent points on the other ellipse.  Keep the line
 *     which either crosses or does not cross the center line.

 *
Repeat unit the location of each end points does not change significantly or the maximum number of
 *     iterations has been exceeded

 *

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

	// convergence parameters
	private double convergenceTol;
	private int maxIterations = 10;

	// storage
	private Point2D_F64 temp0 = new Point2D_F64();
	private Point2D_F64 temp1 = new Point2D_F64();

	// storage for the change in point positions
	double sumDifference;

	// true of the optimization converged before it ran out of iterations
	private boolean converged;

	LineSegment2D_F64 centerLine = new LineSegment2D_F64();

	// storage for local workspace
	LineSegment2D_F64 tempLine = new LineSegment2D_F64();
	LineGeneral2D_F64 lineGeneral = new LineGeneral2D_F64();
	Point2D_F64 junk = new Point2D_F64();

	/**
	 * Constructor that configures optimization parameters
	 *
	 *
	 * @param convergenceTol  Tolerance for when the iterations will stop.  Try 1e-8 for doubles and 1e-4 for floats
	 * @param maxIterations Maximum number of iterations
	 */
	public TangentLinesTwoEllipses_F64(double convergenceTol,
									   int maxIterations) {
		this.convergenceTol = convergenceTol;
		this.maxIterations = maxIterations;
	}

	/**
	 *
Selects 4 pairs of points.  Each point in the pair represents an end point in a line segment which is tangent
	 * to both ellipseA and ellipseB.  Both ellipses are assumed to not intersect each other.  If a fatal error
	 * occurs the function will return false.  However it can return true and did not converge.  To check for
	 * convergence call {@link #isConverged()}.

	 *
	 *
Line 0 and line 3 will not intersect the line joining the center of the two ellipses while line 1 and 2 will.

	 *
	 * @param ellipseA (Input) An ellipse
	 * @param ellipseB (Input) An ellipse
	 * @param tangentA0 (Output) Tangent point on A for line segment 0
	 * @param tangentA1 (Output) Tangent point on A for line segment 1
	 * @param tangentA2 (Output) Tangent point on A for line segment 2
	 * @param tangentA3 (Output) Tangent point on A for line segment 3
	 * @param tangentB0 (Output) Tangent point on B for line segment 0
	 * @param tangentB1 (Output) Tangent point on B for line segment 1
	 * @param tangentB2 (Output) Tangent point on B for line segment 2
	 * @param tangentB3 (Output) Tangent point on B for line segment 3
	 * @return true if no fatal error, false if one happened.
	 */
	public boolean process(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 )
	{
		converged = false;

		// initialize by picking an arbitrary point on A and then finding the points on B in which
		// a line is tangent to B and passes through the point on A
		if (!initialize(ellipseA, ellipseB,
				tangentA0, tangentA1, tangentA2, tangentA3,
				tangentB0, tangentB1, tangentB2, tangentB3))
			return false;

		// update the location of each point until it converges or the maximum number of iterations has been exceeded
		int iteration = 0;
		for( ;iteration < maxIterations; iteration++ ) {
			boolean allGood = false;
			sumDifference = 0;

			if( !selectTangent(tangentA0,tangentB0,ellipseB,tangentB0, false) )
				return false;
			if( !selectTangent(tangentA1,tangentB1,ellipseB,tangentB1, true) )
				return false;
			if( !selectTangent(tangentA2,tangentB2,ellipseB,tangentB2, true) )
				return false;
			if( !selectTangent(tangentA3,tangentB3,ellipseB,tangentB3, false) )
				return false;

			if( Math.sqrt(sumDifference)/4.0 <= convergenceTol ) {
				allGood = true;
			}
			sumDifference = 0;

			if( !selectTangent(tangentB0,tangentA0,ellipseA,tangentA0, false) )
				return false;
			if( !selectTangent(tangentB1,tangentA1,ellipseA,tangentA1, true) )
				return false;
			if( !selectTangent(tangentB2,tangentA2,ellipseA,tangentA2, true) )
				return false;
			if( !selectTangent(tangentB3,tangentA3,ellipseA,tangentA3, false) )
				return false;

			if( allGood && Math.sqrt(sumDifference)/4.0 <= convergenceTol ) {
				break;
			}

		}

		converged = iteration < maxIterations;

		return true;
	}

	/**
	 * Select the initial tangent points on the ellipses.  This is done by:
	 *
	 * 1) picking an arbitrary point on ellipseA.
	 * 2) Find tangent points on B using point from step 1
	 * 3) Use those two tangent points on B to find 4 points on ellipse A
	 * 4) Use those 4 points to select 4 points on B.
	 */
	boolean initialize(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) {

		centerLine.set(ellipseA.center,ellipseB.center);

		UtilLine2D_F64.convert(centerLine, lineGeneral);

		Intersection2D_F64.intersection(lineGeneral, ellipseA, temp0, temp1, -1);
		if (temp0.distance2(ellipseB.center) < temp1.distance2(ellipseB.center)) {
			tangentA0.set(temp0);
		} else {
			tangentA0.set(temp1);
		}

		// Two seed points for B.  This points will be on two different sides of center line
		if( !tangentLines(tangentA0,ellipseB,tangentB0,tangentB1) )
			return false;

		// Find initial seed of 4 points on ellipse A.  Careful which pairs of points cross or
		// don't cross the center line
		if( !selectTangent(tangentB0,tangentA0,ellipseA,tangentA0, false))
			return false;
		if( !selectTangent(tangentB0,tangentA0,ellipseA,tangentA1, true))
			return false;
		if( !selectTangent(tangentB1,tangentA0,ellipseA,tangentA2, true))
			return false;
		if( !selectTangent(tangentB1,tangentA0,ellipseA,tangentA3, false))
			return false;

		// not all of the B's have been initialized.  That's ok.  It will just have a large error
		// the first iteration

		return true;
	}

	/**
	 * Selects a tangent point on the ellipse which is closest to the original source point of A.
	 * @param a Point that the tangent lines pass through
	 * @param previousTangent Source point which generated 'a'
	 * @param ellipse Ellipse which the lines will be tangent to
	 * @param tangent (Output) Storage for the selected tangent point
	 * @return true if everything went well or false if finding tangent lines barfed
	 */
	boolean selectTangent( Point2D_F64 a , Point2D_F64 previousTangent ,
						   EllipseRotated_F64 ellipse, Point2D_F64 tangent ,
						   boolean cross )
	{
		if( !tangentLines(a,ellipse,temp0,temp1) )
			return false;

		tempLine.a = a;

		tempLine.b = temp0;
		boolean crossed0 = Intersection2D_F64.intersection(centerLine,tempLine,junk) != null;
		tempLine.b = temp1;
		boolean crossed1 = Intersection2D_F64.intersection(centerLine,tempLine,junk) != null;

		if( crossed0 == crossed1 )
			throw new RuntimeException("Well this didn't work");

		if( cross == crossed0 ) {
			sumDifference += previousTangent.distance2(temp0);
			tangent.set(temp0);
		} else {
			sumDifference += previousTangent.distance2(temp1);
			tangent.set(temp1);
		}

		return true;
	}

	public boolean isConverged() {
		return converged;
	}

	public double getConvergenceTol() {
		return convergenceTol;
	}

	public void setConvergenceTol(double convergenceTol) {
		this.convergenceTol = convergenceTol;
	}

	public int getMaxIterations() {
		return maxIterations;
	}

	public void setMaxIterations(int maxIterations) {
		this.maxIterations = maxIterations;
	}
}


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

/**
 * An axis aligned rectangle in 2D that is specified by its lower extent (x0,y0), and upper extent (x1,y1).
 * x0 ≤ x1 and y0 ≤ y1.  (x1,y1) is exclusive and not contained in the rectangle.
 */
public class Rectangle2D_I32 {
	/**
	 * Lower extent
	 */
	public int x0,y0;
	/**
	 * Upper extent
	 */
	public int x1,y1;

	public Rectangle2D_I32(int x0, int y0, int x1, int y1) {
		set(x0,y0,x1,y1);
	}

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

	public void set( Rectangle2D_I32 orig ) {
		this.x0 = orig.x0;
		this.y0 = orig.y0;
		this.x1 = orig.x1;
		this.y1 = orig.y1;
	}

	public void set(int x0, int y0, int x1, int y1) {
		this.x0 = x0;
		this.y0 = y0;
		this.x1 = x1;
		this.y1 = y1;
	}

	public Rectangle2D_I32() {
	}

	/**
	 * Makes sure x0,y0 is the lower extent and x1,y1 is the upper extent
	 */
	public void enforceExtents() {
		if( x1 < x0 ) {
			int tmp = x1;
			x1 = x0;
			x0 = tmp;
		}
		if( y1 < y0 ) {
			int tmp = y1;
			y1 = y0;
			y0 = tmp;
		}
	}

	public int getWidth() {
		return x1-x0;
	}

	public int getHeight() {
		return y1-y0;
	}

	public int area() {
		return (y1-y0)*(x1-x0);
	}

	public int getX0() {
		return x0;
	}

	public void setX0(int x0) {
		this.x0 = x0;
	}

	public int getY0() {
		return y0;
	}

	public void setY0(int y0) {
		this.y0 = y0;
	}

	public int getX1() {
		return x1;
	}

	public void setX1(int x1) {
		this.x1 = x1;
	}

	public int getY1() {
		return y1;
	}

	public void setY1(int y1) {
		this.y1 = y1;
	}

	@Override
	public String toString() {
		return "RectangleCorner2D_I32( "+x0+" "+y0+" "+x1+" "+y1+" )";
	}

	@Override
	public boolean equals(Object obj) {
		try {
			Rectangle2D_I32 r = (Rectangle2D_I32)obj;
			return x0==r.x0&&y0==r.y0&&x1==r.x1&&y1==r.y1;
		} catch( RuntimeException e){
			return false;
		}
	}
}


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

import java.util.Objects;

/**
 * An integer 2D point
 */
public class Point2D_I32 extends GeoTuple<Point2D_I32> {
	public int x;
	public int y;

	public Point2D_I32( int x, int y ) {
		this.x = x;
		this.y = y;
	}

	public Point2D_I32( Point2D_I32 orig ) {
		this.x = orig.x;
		this.y = orig.y;
	}

	public Point2D_I32() {
	}

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

	public void set( int x, int y ) {
		this.x = x;
		this.y = y;
	}

	public void setX( int x ) {
		this.x = x;
	}

	public void setY( int y ) {
		this.y = y;
	}

	public final int getX() {
		return x;
	}

	public final int getY() {
		return y;
	}

	public double distance( Point2D_I32 a ) {
		int dx = x-a.x;
		int dy = y-a.y;

		return Math.sqrt(dx*dx + dy*dy);
	}

	public double distance( int x , int y ) {
		int dx = this.x-x;
		int dy = this.y-y;

		return Math.sqrt(dx*dx + dy*dy);
	}

	/**
	 *

	 * Returns the Euclidean distance squared from 'this' to 'a'.  No floating point
	 * operations are used.
	 *

	 *
	 *

	 * d2 =  (this.x-a.x)2 + (this.y-a.y)2
	 *

	 *
	 * @param a Point that distance is computed from.
	 * @return Euclidean distance squared.
	 */
	public int distance2( Point2D_I32 a ) {
		int dx = x-a.x;
		int dy = y-a.y;

		return dx*dx + dy*dy;
	}

	public int distance2( int x , int y ) {
		int dx = this.x-x;
		int dy = this.y-y;

		return dx*dx + dy*dy;
	}

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

	@Override
	public String toString() {
		return "Point2D_I32{" +
				"x=" + x +
				", y=" + y +
				'}';
	}

	@Override
	public int getDimension() {
		return 2;
	}

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

	@Override
	public boolean equals(Object obj) {
		if( this.getClass() != obj.getClass() )
			return false;
		Point2D_I32 p = (Point2D_I32)obj;
		return x==p.x&&y==p.y;
	}

	@Override
	public int hashCode() {
		return Objects.hash(x, y);
	}
}


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

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_F64;

/**
 * @author Peter Abeles
 */
public class SpecialOrthogonalOps_F64 {
	/**
	 * Finds the best fit matrix in SO(3) for the input matrix.
	 *
	 * @param A 3x3 matrix. Modified.
	 */
	public static void bestFit( DMatrixRMaj A ) {

		SingularValueDecomposition_F64<DMatrixRMaj> svd = DecompositionFactory_DDRM.svd(true,true,true);

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

		CommonOps_DDRM.multTransB(svd.getU(null,false),svd.getV(null,false),A);

		// determinant should be +1
		double det = CommonOps_DDRM.det(A);

		if( det < 0 )
			CommonOps_DDRM.scale(-1,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.struct.shapes;

import georegression.struct.point.Point3D_F64;

import java.io.Serializable;

/**
 * An axis aligned box in 3D that is specified by a point p=(x0,y0,z0), and its lengthX, lengthY, and lengthZ.
 * The point 'p' has the lowest values and is also known as the lower extent..  The corner which is farthest away from 'p'
 * is (x0+lengthX , y0+lengthY, z0+lengthZ), the upper extent.
 */
public class BoxLength3D_F64 implements Serializable {
	/**
	 * Point on the box with the lowest values.  The lower extent.
	 */
	public Point3D_F64 p = new Point3D_F64();
	/**
	 * The length of each size along their respective axises
	 */
	public double lengthX,lengthY,lengthZ;

	public BoxLength3D_F64(double x0, double y0, double z0, double lengthX, double lengthY, double lengthZ) {
		this.p.set(x0,y0,z0);
		this.lengthX = lengthX;
		this.lengthY = lengthY;
		this.lengthZ = lengthZ;
	}

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

	public void set( BoxLength3D_F64 orig ) {
		set(orig.p.x,orig.p.y,orig.p.z,orig.lengthX,orig.lengthY,orig.lengthZ);
	}

	public BoxLength3D_F64() {
	}

	public void set(double x0, double y0, double z0, double lengthX, double lengthY, double lengthZ) {
		this.p.set(x0,y0,z0);
		this.lengthX = lengthX;
		this.lengthY = lengthY;
		this.lengthZ = lengthZ;
	}

	public double area() {
		return lengthX*lengthY*lengthZ;
	}

	public Point3D_F64 getP() {
		return p;
	}

	public double getLengthX() {
		return lengthX;
	}

	public double getLengthY() {
		return lengthY;
	}

	public double getLengthZ() {
		return lengthZ;
	}

	public void setP(Point3D_F64 p) {
		this.p.set(p);
	}

	public void setLengthX(double lengthX) {
		this.lengthX = lengthX;
	}

	public void setLengthY(double lengthY) {
		this.lengthY = lengthY;
	}

	public void setLengthZ(double lengthZ) {
		this.lengthZ = lengthZ;
	}

	/**
	 * Used to retrieve the corners of the box.
	 *
	 * @param index A value from 0 to 7
	 * @param corner (Optional) storage for the corner. If null a new point will be declared
	 * @return The corner.
	 */
	public Point3D_F64 getCorner(int index , Point3D_F64 corner ) {
		if( corner == null )
			corner = new Point3D_F64();

		corner.set(p);

		if( (index & 0x01) != 0 ) {
			corner.x += lengthX;
		}
		if( (index & 0x02) != 0 ) {
			corner.y += lengthY;
		}
		if( (index & 0x04) != 0 ) {
			corner.z += lengthZ;
		}
		return corner;
	}

	public String toString() {
		return getClass().getSimpleName()+"P( "+p.x+" "+p.y+" "+p.z+" ) sides ( "+lengthX+" , "+lengthY+" , "+lengthZ+" )";
	}
}


/*
 * 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.point.Point3D_F64;
import georegression.struct.shapes.Cylinder3D_F64;
import georegression.struct.shapes.Sphere3D_F64;
import georegression.struct.shapes.Triangle3D_F64;

/**
 * @author Peter Abeles
 */
public class Distance3D_F64 {
	/**
	 * Distance of the closest point between two lines.  Parallel lines are correctly
	 * handled.
	 *
	 * @param l0 First line. Not modified.
	 * @param l1 Second line. Not modified.
	 * @return Distance between the closest point on both lines.
	 */
	public static double distance( LineParametric3D_F64 l0,
								   LineParametric3D_F64 l1 ) {
		double x = l0.p.x - l1.p.x;
		double y = l0.p.y - l1.p.y;
		double z = l0.p.z - l1.p.z;

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

		double bottom = MiscOps.dot( l0.slope, l0.slope ) * dv1v1 - dv1v0 * dv1v0;
		double t0;

		if( bottom == 0 ) {
			// handle parallel lines
			t0 = 0;
		} else {
			t0 = (dv01v1 * dv1v0 - MiscOps.dot( x,y,z, l0.slope ) * dv1v1)/bottom;
		}

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

		double dx = ( l0.p.x + t0 * l0.slope.x ) - ( l1.p.x + t1 * l1.slope.x );
		double dy = ( l0.p.y + t0 * l0.slope.y ) - ( l1.p.y + t1 * l1.slope.y );
		double dz = ( l0.p.z + t0 * l0.slope.z ) - ( l1.p.z + t1 * l1.slope.z );

		// round off error can make distanceSq go negative when it is very close to zero
		double distanceSq = dx * dx + dy * dy + dz * dz;
		if( distanceSq < 0 )
			return 0;
		else
			return Math.sqrt( distanceSq );
	}

	/**
	 * Distance from the point to the closest point on the line.
	 *
	 * @param l Line. Not modified.
	 * @param p Point. Not modified.
	 * @return distance.
	 */
	public static double distance( LineParametric3D_F64 l,
								   Point3D_F64 p ) {

		double x = l.p.x - p.x;
		double y = l.p.y - p.y;
		double z = l.p.z - p.z;

		double cc = x*x + y*y + z*z;

		// could avoid a square root here by computing b*b directly
		// however that is most likely more prone to numerical overflow since the numerator will need to be squared
		// before division can reduce its "power"
		double b = MiscOps.dot(x,y,z,l.slope)/l.slope.norm();

		double distanceSq = cc-b*b;

		// round off error can make distanceSq go negative when it is very close to zero
		if( distanceSq < 0 ) {
			return 0;
		} else {
			return Math.sqrt(distanceSq);
		}
	}

	/**
	 * Distance from the point to the closest point on the line segment.
	 *
	 * @param l Line. Not modified.
	 * @param p Point. Not modified.
	 * @return distance.
	 */
	public static double distance( LineSegment3D_F64 l,
								   Point3D_F64 p ) {

		double dx = p.x - l.a.x;
		double dy = p.y - l.a.y;
		double dz = p.z - l.a.z;

		double cc = dx*dx + dy*dy + dz*dz;

		double slope_x = l.b.x - l.a.x;
		double slope_y = l.b.y - l.a.y;
		double slope_z = l.b.z - l.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;

		// check end points
		if( d <= 0 )
			return p.distance(l.a);
		else if( d >= n )
			return p.distance(l.b);

		double distanceSq = cc-d*d;

		// round off error can make distanceSq go negative when it is very close to zero
		if( distanceSq < 0 ) {
			return 0;
		} else {
			return Math.sqrt(distanceSq);
		}
	}

	/**
	 * Distance between a plane and a point. A signed distance is returned, where a positive value is returned if
	 * the point is on the same side of the plane as the normal and the opposite if it's on the other.
	 *
	 * @param plane The plane
	 * @param point The point
	 * @return Signed distance
	 */
	public static double distance( PlaneGeneral3D_F64 plane , Point3D_F64 point ) {
		double top = plane.A*point.x + plane.B*point.y + plane.C*point.z - plane.D;

		return top / Math.sqrt( plane.A*plane.A + plane.B*plane.B + plane.C*plane.C);
	}

	/**
	 * Returns the signed distance a point is from the sphere's surface.  If the point is outside of the sphere
	 * it's distance will be positive.  If it is inside it will be negative.
	 *

	 * distance = ||sphere.center - point|| - r
	 *
	 * @param sphere The sphere
	 * @param point The point
	 * @return Signed distance
	 */
	public static double distance( Sphere3D_F64 sphere , Point3D_F64 point ) {

		double r = point.distance(sphere.center);
		return r-sphere.radius;
	}

	/**
	 * Returns the signed distance a point is from the cylinder's surface.  If the point is outside of the cylinder
	 * it's distance will be positive.  If it is inside it will be negative.
	 *
	 * @param cylinder The cylinder
	 * @param point The point
	 * @return Signed distance
	 */
	public static double distance( Cylinder3D_F64 cylinder, Point3D_F64 point ) {

		double r = Distance3D_F64.distance(cylinder.line,point);

		return r - cylinder.radius;
	}

	/**
	 * Signed distance from a 3D point to 3D triangle.   The sign indicates which side of the triangle the point
	 * is on.  See {@link georegression.metric.alg.DistancePointTriangle3D_F64} for the details.
	 *
	 * @param triangle 3D triangle
	 * @param point Point for which the closest point on the triangle is found
	 * @return The closest point
	 */
	public static double distance( Triangle3D_F64 triangle, Point3D_F64 point ) {

		DistancePointTriangle3D_F64 alg = new DistancePointTriangle3D_F64();
		alg.setTriangle(triangle.v0,triangle.v1,triangle.v2);

		Point3D_F64 cp = new Point3D_F64();

		alg.closestPoint(point,cp);

		double d = point.distance(cp);

		return alg.sign(point)*d;
	}
}


/*
 * 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.struct.point.Point3D_F64;
import georegression.struct.point.Vector3D_F64;
import org.ejml.FancyPrint;

import java.io.Serializable;

/**
 *

 * 3D line parameterized using parametric equation:

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

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

 *
 * @see georegression.geometry.UtilLine3D_F64
 */
public class LineParametric3D_F64 implements Serializable {
	/**
	 * A point on the line
	 */
	public Point3D_F64 p;
	/**
	 * The line's slope
	 */
	public Vector3D_F64 slope;

	public LineParametric3D_F64( double x_0, double y_0, double z_0,
								 double slopeX, double slopeY, double slopeZ ) {
		this();
		p.set( x_0, y_0, z_0 );
		slope.set( slopeX, slopeY, slopeZ );
	}

	public LineParametric3D_F64( Point3D_F64 p, Vector3D_F64 slope ) {
		this();
		setPoint( p );
		setSlope( slope );
	}

	public LineParametric3D_F64() {
		p = new Point3D_F64();
		slope = new Vector3D_F64();
	}

	public LineParametric3D_F64( LineParametric3D_F64 l ) {
		this();
		this.p.set(l.p);
		this.slope.set(l.slope);
	}

	/**
	 * Should it declare the point and slope
	 * @param declare true means to declares the data otherwise they are left as null
	 */
	public LineParametric3D_F64( boolean declare ) {
		if( declare ) {
			p = new Point3D_F64();
			slope = new Vector3D_F64();
		}
	}

	/**
	 * where = p + t*slope.
	 */
	public void setPointOnLine( double t , Point3D_F64 where ) {
		where.x = p.x + t*slope.x;
		where.y = p.y + t*slope.y;
		where.z = p.z + t*slope.z;
	}

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

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

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

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

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

	public Point3D_F64 getPoint() {
		return p;
	}

	public Vector3D_F64 getSlope() {
		return slope;
	}

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

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

	public final double getSlopeZ() {
		return slope.z;
	}

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

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

	public final double getZ() {
		return p.z;
	}

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

	public void set( LineParametric3D_F64 o ) {
		this.p.set(o.p);
		this.slope.set(o.slope);
	}

	public Point3D_F64 getP() {
		return p;
	}

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

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

	@Override
	public String toString() {
		FancyPrint f = new FancyPrint();

		return getClass().getSimpleName()+" P( "+f.s(p.x)+" "+f.s(p.y)+" "+f.s(p.z)+
				" ) Slope( "+f.s(slope.x)+" "+f.s(slope.y)+" "+f.s(slope.z)+" )";
	}

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


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

import georegression.geometry.UtilPoint2D_F64;
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.shapes.Polygon2D_F64;
import georegression.struct.shapes.Quadrilateral_F64;

/**
 * Functions related to finding the distance of one shape from another shape.  This is often
 * closely related to finding the {@link ClosestPoint3D_F64 closest point}.
 *
 * @author Peter Abeles
 */
// TODO distance between two line segments, line lines
// handle parallel overlapping cases by returning zero
public class Distance2D_F64 {

	/**
	 *

	 * Returns the Euclidean distance of the closest point on the line from a point.
	 *

	 *
	 * @param line A line segment. Not modified.
	 * @param p The point. Not modified.
	 * @return Distance the closest point on the line is away from the point.
	 */
	public static double distance( LineParametric2D_F64 line, Point2D_F64 p ) {
		return Math.sqrt(distanceSq(line, p));
	}

	/**
	 *

	 * Returns the Euclidean distance of the closest point on the line from a point.
	 *

	 *
	 * @param line A line segment. Not modified.
	 * @param x Point's x-coordinate
	 * @param y Point's y-coordinate
	 * @return Distance the closest point on the line is away from the point.
	 */
	public static double distance( LineParametric2D_F64 line, double x , double y ) {
		return Math.sqrt(distanceSq(line, x,y));
	}

	/**
	 *

	 * Returns the Euclidean distance squared of the closest point on the line from a point.
	 *

	 *
	 * @param line A line segment. Not modified.
	 * @param p The point. Not modified.
	 * @return Euclidean distance squared to the closest point on the line is away from the point.
	 */
	public static double distanceSq( LineParametric2D_F64 line, Point2D_F64 p ) {
		double scale = Math.max(Math.abs(line.slope.x),Math.abs(line.slope.y));

		double t = ClosestPoint2D_F64.closestPointT( line, p , scale);

		double a = (line.slope.x/scale) * t + line.p.x;
		double b = (line.slope.y/scale) * t + line.p.y;

		double dx = p.x - a;
		double dy = p.y - b;

		return dx * dx + dy * dy;
	}

	/**
	 *

	 * Returns the Euclidean distance squared of the closest point on the line from a point.
	 *

	 *
	 * @param line A line segment. Not modified.
	 * @param x Point's x-coordinate
	 * @param y Point's y-coordinate
	 * @return Euclidean distance squared to the closest point on the line is away from the point.
	 */
	public static double distanceSq( LineParametric2D_F64 line, double x , double y ) {
		double scale = Math.max(Math.abs(line.slope.x),Math.abs(line.slope.y));

		double t = ClosestPoint2D_F64.closestPointT( line, x, y , scale);

		double a = (line.slope.x/scale) * t + line.p.x;
		double b = (line.slope.y/scale) * t + line.p.y;

		double dx = x - a;
		double dy = y - b;

		return dx * dx + dy * dy;
	}

	/**
	 *

	 * Returns the Euclidean distance of the closest point on a line segment to the specified point.
	 *

	 *
	 * @param line A line segment. Not modified.
	 * @param p The point. Not modified.
	 * @return Euclidean distance of the closest point on a line is away from a point.
	 */
	public static double distance( LineSegment2D_F64 line, Point2D_F64 p ) {
		return Math.sqrt(distanceSq(line, p));
	}

	/**
	 *

	 * Returns the Euclidean distance of the closest point on a line segment to the specified point.
	 *

	 *
	 * @param line A line segment. Not modified.
	 * @param x Point's x-coordinate
	 * @param y Point's y-coordinate
	 * @return Euclidean distance of the closest point on a line is away from a point.
	 */
	public static double distance( LineSegment2D_F64 line, double x , double y) {
		return Math.sqrt(distanceSq(line, x,y));
	}

	/**
	 *

	 * Returns the Euclidean distance squared of the closest point on a line segment to the specified point.
	 *

	 *
	 * @param line A line segment. Not modified.
	 * @param p The point. Not modified.
	 * @return Euclidean distance squared of the closest point on a line is away from a point.
	 */
	public static double distanceSq( LineSegment2D_F64 line, Point2D_F64 p ) {
		double a = line.b.x - line.a.x;
		double b = line.b.y - line.a.y;

		double t = a * ( p.x - line.a.x ) + b * ( p.y - line.a.y );
		t /= ( a * a + b * b );

		// if the point of intersection is past the end points return the distance
		// from the closest end point
		if( t < 0 ) {
			return UtilPoint2D_F64.distanceSq(line.a.x, line.a.y, p.x, p.y);
		} else if( t > 1.0 )
			return UtilPoint2D_F64.distanceSq(line.b.x, line.b.y, p.x, p.y);

		// return the distance of the closest point on the line
		return UtilPoint2D_F64.distanceSq(line.a.x + t * a, line.a.y + t * b, p.x, p.y);
	}

	/**
	 *

	 * Returns the Euclidean distance squared of the closest point on a line segment to the specified point.
	 *

	 *
	 * @param line Line segment. Not modified.
	 * @param x Point's x-coordinate
	 * @param y Point's y-coordinate
	 * @return Euclidean distance squared of the closest point on a line is away from a point.
	 */
	public static double distanceSq( LineSegment2D_F64 line, double x , double y ) {
		double a = line.b.x - line.a.x;
		double b = line.b.y - line.a.y;

		double t = a * ( x - line.a.x ) + b * ( y - line.a.y );
		t /= ( a * a + b * b );

		// if the point of intersection is past the end points return the distance
		// from the closest end point
		if( t < 0 ) {
			return UtilPoint2D_F64.distanceSq(line.a.x, line.a.y, x, y);
		} else if( t > 1.0 )
			return UtilPoint2D_F64.distanceSq(line.b.x, line.b.y, x, y);

		// return the distance of the closest point on the line
		return UtilPoint2D_F64.distanceSq(line.a.x + t * a, line.a.y + t * b, x, y);
	}

	/**
	 * Finds the distance between the two line segments
	 * @param segmentA Line segment. Not modified.
	 * @param segmentB Line segment. Not modified.
	 * @return Euclidean distance of the closest point between the two line segments.
	 */
	public static double distance( LineSegment2D_F64 segmentA , LineSegment2D_F64 segmentB ) {
		return Math.sqrt(distanceSq(segmentA, segmentB));
	}

	/**
	 * Finds the distance squared between the two line segments
	 * @param segmentA Line segment. Not modified.
	 * @param segmentB Line segment. Not modified.
	 * @return Euclidean distance squared of the closest point between the two line segments.
	 */
	public static double distanceSq( LineSegment2D_F64 segmentA , LineSegment2D_F64 segmentB ) {

		// intersection of the two lines relative to A
		double slopeAX = segmentA.slopeX();
		double slopeAY = segmentA.slopeY();
		double slopeBX = segmentB.slopeX();
		double slopeBY = segmentB.slopeY();

		double ta = slopeBX*( segmentA.a.y - segmentB.a.y ) - slopeBY*( segmentA.a.x - segmentB.a.x );
		double bottom = slopeBY*slopeAX - slopeAY*slopeBX;

		// see they intersect
		if( bottom != 0 ) {
			// see if the intersection is inside of lineA
			ta /= bottom;
			if( ta >= 0 && ta <= 1.0 ) {
				// see if the intersection is inside of lineB
				double tb = slopeAX*( segmentB.a.y - segmentA.a.y ) - slopeAY*( segmentB.a.x - segmentA.a.x );
				tb /= slopeAY*slopeBX - slopeBY*slopeAX;
				if( tb >= 0 && tb <= 1.0 )
					return 0;
			}
		}

		double closest = Double.MAX_VALUE;
		closest = Math.min(closest,distanceSq(segmentA, segmentB.a));
		closest = Math.min(closest,distanceSq(segmentA, segmentB.b));
		closest = Math.min(closest,distanceSq(segmentB, segmentA.a));
		closest = Math.min(closest,distanceSq(segmentB, segmentA.b));

		return closest;
	}

	/**
	 * Returns the Euclidean distance of the closest point on the quadrilateral to the provided point.
	 *
	 * @param quad Quadrilateral
	 * @param p Point
	 * @return Distance apart
	 */
	public static double distance( Quadrilateral_F64 quad , Point2D_F64 p ) {
		return Math.sqrt(distanceSq(quad,p));
	}

	/**
	 * Returns the Euclidean distance squared of the closest point on the quadrilateral to the provided point.
	 *
	 * @param quad Quadrilateral
	 * @param p Point
	 * @return Distance squared apart
	 */
	public static double distanceSq( Quadrilateral_F64 quad , Point2D_F64 p ) {
		LineSegment2D_F64 seg = LineSegment2D_F64.wrap(quad.a, quad.b);
		double a = distanceSq(seg, p);
		seg.a = quad.b;seg.b = quad.c;
		a = Math.min(a,distanceSq(seg,p));
		seg.a = quad.c;seg.b = quad.d;
		a = Math.min(a,distanceSq(seg,p));
		seg.a = quad.d;seg.b = quad.a;
		return Math.min(a, distanceSq(seg, p));
	}

	/**
	 * Returns the Euclidean distance of the closest point on the Polygon to the provided point.
	 *
	 * @param poly Polygon2D
	 * @param p Point
	 * @return Distance squared apart
	 */
	public static double distance( Polygon2D_F64 poly , Point2D_F64 p ) {
		return Math.sqrt(distanceSq(poly, p, null));
	}

	/**
	 * Returns the Euclidean distance squared of the closest point on the Polygon to the provided point.
	 *
	 * @param poly Polygon2D
	 * @param p Point
	 * @param storage Optional storage for linesegment which is used internally to compute the distance
	 * @return Distance squared apart
	 */
	public static double distanceSq( Polygon2D_F64 poly , Point2D_F64 p , LineSegment2D_F64 storage ) {
		if( storage == null )
			storage = LineSegment2D_F64.wrap(null,null);

		double minimum = Double.MAX_VALUE;
		for (int i = 0; i < poly.size(); i++) {
			int j = (i+1)%poly.size();

			storage.a = poly.vertexes.data[i];
			storage.b = poly.vertexes.data[j];

			double d = distanceSq(storage, p);
			if( d < minimum )
				minimum = d;
		}

		return minimum;
	}

	/**
	 *

	 * Returns the Euclidean distance of the closest point on the line to the specified point.
	 *

	 *
	 * @param line A line. Not modified.
	 * @param p The point. Not modified.
	 * @return Euclidean distance of the closest point on the line to the specified point.
	 */
	public static double distance( LineGeneral2D_F64 line , Point2D_F64 p ) {
		return Math.abs(line.A*p.x + line.B*p.y + line.C) / Math.sqrt( line.A*line.A + line.B*line.B );
	}

	/**
	 *

	 * Returns the signed Euclidean distance of the closest point on the line to the specified point.
	 * The line is assumed be normalized.  See {@link LineGeneral2D_F64} for details on normalization.
	 *

	 *
	 * @param line A normalized line. Not modified.
	 * @param p The point. Not modified.
	 * @return Euclidean distance of the closest point on the line to the specified point.
	 */
	public static double distanceNorm(LineGeneral2D_F64 line, Point2D_F64 p) {
		return Math.abs(line.A*p.x + line.B*p.y + line.C);
	}

	/**
	 * Returns the distance of the closest point on the line from the origin
	 * @param line Line
	 * @return Euclidean distance
	 */
	public static double distanceOrigin( LineParametric2D_F64 line ) {
		double top = line.slope.y*line.p.x - line.slope.x*line.p.y;

		return Math.abs(top)/line.slope.norm();
	}

	/**
	 * Euclidean distance of closest point on ellipse to point 'p'.
	 *
	 * @param ellipse Ellipse
	 * @param p Point
	 * @return Euclidean distance
	 */
	public static double distance(EllipseRotated_F64 ellipse , Point2D_F64 p ) {
		return Math.sqrt(distance2(ellipse, p));
	}

	/**
	 * Euclidean distance squared of closest point on ellipse to point 'p'.
	 *
	 * @param ellipse Ellipse
	 * @param p Point
	 * @return Euclidean distance squared
	 */
	public static double distance2(EllipseRotated_F64 ellipse , Point2D_F64 p ) {
		// put point into ellipse's reference frame
		double cphi = Math.cos(ellipse.phi);
		double sphi = Math.sin(ellipse.phi);

		double xc = p.x - ellipse.center.x;
		double yc = p.y - ellipse.center.y;
		double r = Math.sqrt(xc*xc + yc*yc);

		double x =  cphi*xc + sphi*yc;
		double y = -sphi*xc + cphi*yc;

		double ct = x/r;
		double st = y/r;

		x = ellipse.center.x + ellipse.a*ct*cphi - ellipse.b*st*sphi;
		y = ellipse.center.y + ellipse.a*ct*sphi + ellipse.b*st*cphi;

		return p.distance2(x,y);
	}
}


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

import georegression.struct.InvertibleTransform;

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

/**
 * <p>
 * Given a sequence of {@link georegression.struct.InvertibleTransform} transforms it will compute a single
 * equivalent transform.
 * </p>
 *
 * @author Peter Abeles
 */
public class InvertibleTransformSequence<T extends InvertibleTransform> {

	// the path
	private List<Node<T>> path = new ArrayList<Node<T>>();

	/**
	 * Adds the next transform in the sequence.
	 *
	 * @param forward Does the transform work in the forward or reverse direction.
	 * @param tran	The transform.
	 */
	public void addTransform( boolean forward, T tran ) {
		path.add( new Node<T>( tran, forward ) );
	}

	/**
	 * Clears the sequence of transform.
	 */
	public void clear() {
		path.clear();
	}

	@SuppressWarnings({"unchecked"})
	public void computeTransform( T result ) {

		if( path.size() == 0 )
			return;

		T tmp0 = (T)result.createInstance();
		T tmp1 = (T)result.createInstance();
		T inv = (T)result.createInstance();

		Node<T> n = path.get( 0 );
		T nodeTran = (T)n.tran;

		if( n.forward ) {
			tmp0.set( nodeTran );
		} else {
			nodeTran.invert( tmp0 );
		}

		for( int i = 1; i < path.size(); i++ ) {
			n = path.get( i );
			nodeTran = n.tran;

			if( n.forward ) {
				tmp0.concat( nodeTran, tmp1 );
			} else {
				nodeTran.invert( inv );
				tmp0.concat( inv, tmp1 );
			}
			T swap = tmp0;
			tmp0 = tmp1;
			tmp1 = swap;
		}
		result.set(tmp0);
	}

	public List<Node<T>> getPath() {
		return path;
	}

	public static class Node<T extends InvertibleTransform> {
		// the transform
		public T tran;
		// if the transform should be applied in the forward or reverse direction
		public boolean forward;

		public Node( T tran, boolean forward ) {
			this.tran = tran;
			this.forward = forward;
		}
	}
}


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

import georegression.fitting.curves.ClosestPointEllipseAngle_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;

/**
 * Functions related to finding the closest point(s) on one shape from another shape.
 *
 * @author Peter Abeles
 */
public class ClosestPoint2D_F64 {

	/**
	 *

	 * Finds the closest point on 'line' to the specified point.
	 *

	 *
	 * @param line Line along which the closest point is to be found.
	 * @param p Point.
	 * @param output Where the solution is stored.  If null a new instance is created. Modified.
	 * @return Closest point on the line.
	 */
	public static Point2D_F64 closestPoint( LineGeneral2D_F64 line,
											Point2D_F64 p,
											Point2D_F64 output ) {
		if( output == null )
			output = new Point2D_F64();

		double AA = line.A*line.A;
		double AB = line.A*line.B;
		double BB = line.B*line.B;

		output.y = AA*p.y - AB*p.x - line.B*line.C;
		output.y /= AA+BB;

		output.x = BB*p.x - AB*p.y - line.A*line.C;
		output.x /= AA+BB;

		return output;
	}

	/**
	 *

	 * Finds the closest point on 'line' to the specified point.
	 *

	 *
	 * @param line	Line along which the closest point is being found.
	 * @param p	   Point.
	 * @param output Where the solution is stored.  If null a new instance is created. Modified.
	 * @return Closest point on the line.
	 */
	public static Point2D_F64 closestPoint( LineParametric2D_F64 line,
											Point2D_F64 p,
											Point2D_F64 output ) {
		if( output == null )
			output = new Point2D_F64();

		double t = closestPointT( line, p );

		output.x = line.p.x + line.slope.x * t;
		output.y = line.p.y + line.slope.y * t;

		return output;
	}

	/**
	 *

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

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

	 *
	 * @param line The line along which the closest point is being found. Not modified.
	 * @param p	A point. Not modified.
	 * @return Distance as a function of 't'
	 */
	public static double closestPointT( LineParametric2D_F64 line,
										Point2D_F64 p ) {
		double t = line.slope.x * ( p.x - line.p.x ) + line.slope.y * ( p.y - line.p.y );
		t /= line.slope.x * line.slope.x + line.slope.y * line.slope.y;

		return t;
	}

	/**
	 *

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

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

	 *
	 * @param line The line along which the closest point is being found. Not modified.
	 * @param scale Scale factor. used to avoid numerical issues
	 * @param p	A point. Not modified.
	 * @return Distance as a function of 't'
	 */
	public static double closestPointT( LineParametric2D_F64 line,
										Point2D_F64 p , double scale ) {
		double sx = line.slope.x/scale;
		double sy = line.slope.y/scale;

		double t = sx * ( p.x - line.p.x ) + sy * ( p.y - line.p.y );
		t /= sx * sx + sy * sy;

		return t;
	}

	/**
	 *

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

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

	 *
	 * @param line The line along which the closest point is being found. Not modified.
	 * @param x Point's x-coordinate
	 * @param y Point's y-coordinate
	 * @return Distance as a function of 't'
	 */
	public static double closestPointT( LineParametric2D_F64 line, double x, double y) {
		double t = line.slope.x * ( x - line.p.x ) + line.slope.y * ( y - line.p.y );
		t /= line.slope.x * line.slope.x + line.slope.y * line.slope.y;

		return t;
	}

	/**
	 *

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

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

	 *
	 * @param line The line along which the closest point is being found. Not modified.
	 * @param scale Scale factor. used to avoid numerical issues
	 * @param x Point's x-coordinate
	 * @param y Point's y-coordinate
	 * @return Distance as a function of 't'
	 */
	public static double closestPointT( LineParametric2D_F64 line, double x, double y, double scale ) {
		double sx = line.slope.x/scale;
		double sy = line.slope.y/scale;

		double t = sx * ( x - line.p.x ) + sy * ( y - line.p.y );
		t /= sx * sx + sy * sy;

		return t;
	}

	/**
	 * Finds the closest point on the line segment to the provided point 'p'.
	 *
	 * @param line Line segment
	 * @param p Point`
	 * @param output Optional storage for the closet point on the line to p.  If null a new instance is created.
	 * @return Closest point on the line to the point
	 */
	public static Point2D_F64 closestPoint( LineSegment2D_F64 line,
											Point2D_F64 p,
											Point2D_F64 output ) {

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

		double slopeX = line.b.x - line.a.x;
		double slopeY = line.b.y - line.a.y;

		double t = slopeX * ( p.x - line.a.x ) + slopeY * ( p.y - line.a.y );
		t /= slopeX*slopeX + slopeY*slopeY;

		if( t < 0 )
			t = 0;
		else if( t > 1 )
			t = 1;

		output.x = line.a.x + slopeX * t;
		output.y = line.a.y + slopeY * t;

		return output;
	}

	/**
	 * Computes the closest point on an ellipse to the provided point.  If there are multiple solutions then one
	 * is arbitrarily chosen.
	 *
	 * NOTE: When optimizing consider calling {@link ClosestPointEllipseAngle_F64} directly instead.
	 *
	 * @param ellipse Ellipse
	 * @param p Point
	 * @return Closest point on the ellipse
	 */
	public static Point2D_F64 closestPoint( EllipseRotated_F64 ellipse , Point2D_F64 p ) {
		ClosestPointEllipseAngle_F64 alg = new ClosestPointEllipseAngle_F64(GrlConstants.TEST_F64,30);
		alg.setEllipse(ellipse);
		alg.process(p);
		return alg.getClosest();
	}

}


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

import georegression.struct.curve.PolynomialCurve_F64;
import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.factory.LinearSolverFactory_DDRM;
import org.ejml.interfaces.linsol.LinearSolverDense;

/**
 * Fits a polynomial using a {@link LinearSolverDense} and the equation A*X=B.
 * Different solvers can be selected to trade efficiency and robustness.
 */
public class FitPolynomialSolverTall_F64 {

	LinearSolverDense<DMatrixRMaj> solver;

	DMatrixRMaj A = new DMatrixRMaj(1,1);
	DMatrixRMaj b = new DMatrixRMaj(1,1);
	DMatrixRMaj x = new DMatrixRMaj(1,1);

	public FitPolynomialSolverTall_F64( LinearSolverDense<DMatrixRMaj> solver ) {
		this.solver = solver;
	}

	public FitPolynomialSolverTall_F64() {
		this(LinearSolverFactory_DDRM.qrp(true,false));
	}

	/**
	 * Fits the polynomial curve to the data.
	 *
	 * @param data interleaved input output
	 * @param offset first element in data
	 * @param length number of elements to read in data
	 * @param output The fitted polynomial
	 * @return true if solver worked or false if it didn't
	 */
	public boolean process(double[] data, int offset , int length , PolynomialCurve_F64 output ) {
		int N = length/2;

		int numCoefs = output.size();

		A.reshape(N,numCoefs);
		b.reshape(N,1);
		x.reshape(numCoefs,1);

		int end = offset+length;
		for (int i = offset, idxA=0; i < end; i += 2) {
			double x = data[i];
			double y = data[i+1];

			double pow = 1.0;
			for (int j = 0; j < numCoefs; j++) {
				A.data[idxA++] = pow;
				pow *= x;
			}

			b.data[i/2] = y;
		}

		if( !solver.setA(A) )
			return false;
		solver.solve(b,x);

		for (int i = 0; i < numCoefs; i++) {
			output.set(i, x.data[i]);
		}
		return true;
	}
}


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

import georegression.fitting.FitShapeToPoints_F64;
import georegression.struct.curve.ConicGeneral_F64;
import georegression.struct.point.Point2D_F64;
import org.ejml.data.DMatrix6x6;
import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.fixed.CommonOps_DDF6;
import org.ejml.dense.row.linsol.svd.SolveNullSpaceSvd_DDRM;
import org.ejml.interfaces.SolveNullSpace;
import org.ejml.ops.ConvertDMatrixStruct;

import java.util.List;

/**
 * Fit's a parabola to a set of points by finding the null space of AT*A. This is faster but less
 * numerically stable than techniques which don't explicitly square the matrix.
 *
 * @author Peter Abeles
 */
public class FitConicAtA_F64 implements FitShapeToPoints_F64<Point2D_F64,ConicGeneral_F64> {
	private SolveNullSpace<DMatrixRMaj> solver = new SolveNullSpaceSvd_DDRM();

	private DMatrix6x6 ATA = new DMatrix6x6();
	private DMatrixRMaj tmp = new DMatrixRMaj(6,6);
	private DMatrixRMaj nullspace = new DMatrixRMaj(6,1);

	/**
	 * Fits the conic to the points.  Strongly recommended that you transform the points such that they have
	 * zero mean and a standard deviation along x and y axis, independently.
	 *
	 * @param points (Input) points
	 * @param output (Output) found conic
	 * @return true if successful or false if it failed
	 */
	@Override
	public boolean process(List<Point2D_F64> points , ConicGeneral_F64 output ) {
		final int N = points.size();
		if( N < 3 )
			throw new IllegalArgumentException("At least 3 points required");

		CommonOps_DDF6.fill(ATA,0);

		for (int i = 0; i < N; i++) {
			Point2D_F64 p = points.get(i);

			double x = p.x;
			double y = p.y;
			double xx = x*x;
			double xxx = xx*x;
			double yy = y*y;
			double yyy = yy*y;

			ATA.a11 += xx*xx;
			ATA.a12 += xxx*y;
			ATA.a13 += xx*yy;
			ATA.a14 += xxx;
			ATA.a15 += xx*y;
			ATA.a16 += xx;

			ATA.a22 += xx*yy;
			ATA.a23 += x*yyy;
//			ATA.a24 += xx*y;
			ATA.a25 += x*yy;
			ATA.a26 += x*y;

			ATA.a33 += yyy*y;
//			ATA.a34 += x*yy;
			ATA.a35 += yyy;
			ATA.a36 += yy;

//			ATA.a44 += xx;
			ATA.a45 += x*y;
			ATA.a46 += x;

//			ATA.a55 += yy;
			ATA.a56 += y;

		}
		ATA.a21 = ATA.a12;
		ATA.a24 = ATA.a15;

		ATA.a31 = ATA.a13;
		ATA.a32 = ATA.a23;
		ATA.a34 = ATA.a25;

		ATA.a41 = ATA.a14;
		ATA.a42 = ATA.a24;
		ATA.a43 = ATA.a34;
		ATA.a44 = ATA.a16;

		ATA.a51 = ATA.a15;
		ATA.a52 = ATA.a25;
		ATA.a53 = ATA.a35;
		ATA.a54 = ATA.a45;
		ATA.a55 = ATA.a36;

		ATA.a61 = ATA.a16;
		ATA.a62 = ATA.a26;
		ATA.a63 = ATA.a36;
		ATA.a64 = ATA.a56;
		ATA.a65 = ATA.a56;
		ATA.a66 = N;

		ConvertDMatrixStruct.convert(ATA,tmp);

		if( !solver.process(tmp,1,nullspace) )
			return false;

		output.A = nullspace.data[0];
		output.B = nullspace.data[1];
		output.C = nullspace.data[2];
		output.D = nullspace.data[3];
		output.E = nullspace.data[4];
		output.F = nullspace.data[5];

		return true;
	}

	/**
	 * Fits the conic to the weighted set of points.  Strongly recommended that you transform the points such
	 * that they have  zero mean and a standard deviation along x and y axis, independently.
	 *
	 * @param points (Input) points
	 * @param output (Output) found conic
	 * @return true if successful or false if it failed
	 */
	@Override
	public boolean process(List<Point2D_F64> points , double weights[], ConicGeneral_F64 output ) {
		final int N = points.size();
		if( N < 3 )
			throw new IllegalArgumentException("At least 3 points required");

		CommonOps_DDF6.fill(ATA,0);

		for (int i = 0; i < N; i++) {
			Point2D_F64 p = points.get(i);

			double w = weights[i];
			w *= w;
			double x = p.x;
			double y = p.y;
			double xx = x*x;
			double xxx = xx*x;
			double yy = y*y;
			double yyy = yy*y;

			ATA.a11 += w*xx*xx;
			ATA.a12 += w*xxx*y;
			ATA.a13 += w*xx*yy;
			ATA.a14 += w*xxx;
			ATA.a15 += w*xx*y;
			ATA.a16 += w*xx;

			ATA.a22 += w*xx*yy;
			ATA.a23 += w*x*yyy;
//			ATA.a24 += w*xx*y;
			ATA.a25 += w*x*yy;
			ATA.a26 += w*x*y;

			ATA.a33 += w*yyy*y;
//			ATA.a34 += w*x*yy;
			ATA.a35 += w*yyy;
			ATA.a36 += w*yy;

//			ATA.a44 += w*xx;
			ATA.a45 += w*x*y;
			ATA.a46 += w*x;

//			ATA.a55 += w*yy;
			ATA.a56 += w*y;

			ATA.a66 += w;
		}
		ATA.a21 = ATA.a12;
		ATA.a24 = ATA.a15;

		ATA.a31 = ATA.a13;
		ATA.a32 = ATA.a23;
		ATA.a34 = ATA.a25;

		ATA.a41 = ATA.a14;
		ATA.a42 = ATA.a24;
		ATA.a43 = ATA.a34;
		ATA.a44 = ATA.a16;

		ATA.a51 = ATA.a15;
		ATA.a52 = ATA.a25;
		ATA.a53 = ATA.a35;
		ATA.a54 = ATA.a45;
		ATA.a55 = ATA.a36;

		ATA.a61 = ATA.a16;
		ATA.a62 = ATA.a26;
		ATA.a63 = ATA.a36;
		ATA.a64 = ATA.a56;
		ATA.a65 = ATA.a56;

		ConvertDMatrixStruct.convert(ATA,tmp);

		if( !solver.process(tmp,1,nullspace) )
			return false;

		output.A = nullspace.data[0];
		output.B = nullspace.data[1];
		output.C = nullspace.data[2];
		output.D = nullspace.data[3];
		output.E = nullspace.data[4];
		output.F = nullspace.data[5];

		return true;
	}

	public SolveNullSpace<DMatrixRMaj> getSolver() {
		return solver;
	}

	public void setSolver(SolveNullSpace<DMatrixRMaj> solver) {
		this.solver = solver;
	}
}


/*
 * 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.GeoTuple3D_F64;
import georegression.struct.point.Vector3D_F64;
import org.ejml.data.DMatrixRMaj;

import java.util.Random;

/**
 * @author Peter Abeles
 */

// TODO create a UtilGeoTuple3D class for this and point 3d?
public class UtilVector3D_F64 {

	/**
	 * Creates a random vector where each axis is selected from a uniform distribution.
	 *
	 * @param min  minimum value
	 * @param max  maximum value
	 * @param rand random number generator
	 * @return the new random vector
	 */
	public static Vector3D_F64 createRandom( double min, double max, Random rand ) {
		double range = max - min;

		Vector3D_F64 a = new Vector3D_F64();

		a.x = range * rand.nextDouble() + min;
		a.y = range * rand.nextDouble() + min;
		a.z = range * rand.nextDouble() + min;

		return a;
	}

	/**
	 * Selects a vector which will be perpendicular.
	 *
	 * @param A (Input) A vector
	 * @param output (Optional) storage for output
	 * @return Perpendicular vector
	 */
	public static Vector3D_F64 perpendicularCanonical( Vector3D_F64 A , Vector3D_F64 output ) {
		if( output == null )
			output = new Vector3D_F64();

		// normalize for scaling
		double scale = Math.abs(A.x)+Math.abs(A.y)+Math.abs(A.z);

		if( scale == 0 ) {
			output.set(0,0,0);
		} else {
			double x = A.x / scale;
			double y = A.y / scale;
			double z = A.z / scale;

			// For numerical stability ensure that the largest variable is swapped
			if (Math.abs(x) > Math.abs(y)) {
				output.set(z, 0, -x);
			} else {
				output.set(0, z,-y);
			}
		}

		return output;
	}

	/**
	 * Checks to see if the two vectors are identical to within tolerance. Each axis is checked
	 * individually.
	 *
	 * @param a   First vector.
	 * @param b   Second vector.
	 * @param tol Tolerance for equality.
	 * @return true if identical and false if not.
	 */
	public static boolean isIdentical( Vector3D_F64 a, Vector3D_F64 b, double tol ) {
		if( Math.abs( a.x - b.x ) > tol )
			return false;

		if( Math.abs( a.y - b.y ) > tol )
			return false;

		return Math.abs( a.z - b.z ) <= tol;

	}

	/**
	 * Rescales the vector such that its normal is equal to one.
	 *
	 * @param v Vector being normalized.
	 */
	public static void normalize( Vector3D_F64 v ) {
		double a = v.norm();

		v.x /= a;
		v.y /= a;
		v.z /= a;
	}

	/**
	 * Creates a matrix from the set of column vectors.  Each vector is a column in the new matrix.
	 *
	 * @param v Set of vectors. Not modified.
	 * @param R If not null the vectors are stored here.
	 * @return Matrix.
	 */
	public static DMatrixRMaj createMatrix( DMatrixRMaj R, Vector3D_F64... v ) {
		if( R == null ) {
			R = new DMatrixRMaj( 3, v.length );
		}

		for( int i = 0; i < v.length; i++ ) {
			R.set( 0, i, v[i].x );
			R.set( 1, i, v[i].y );
			R.set( 2, i, v[i].z );
		}

		return R;
	}

	/**
	 * Converts matrices into vectors.  All matrices must be vectors with 3 elements.
	 *
	 * @param m A 3x1 or 1x3 matrix
	 * @return Equivalent 3D vector
	 */
	public static Vector3D_F64 convert( DMatrixRMaj m ) {

		Vector3D_F64 v = new Vector3D_F64();
		v.x = (double) m.data[0];
		v.y = (double) m.data[1];
		v.z = (double) m.data[2];

		return v;
	}

	/**
	 * 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(GeoTuple3D_F64 a , GeoTuple3D_F64 b ) {
		double dot = a.x*b.x + a.y*b.y + a.z*b.z;

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


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

import org.ejml.UtilEjml;
import org.ejml.ops.MatrixIO;

import java.text.DecimalFormat;
import java.util.Objects;

/**
 * Generic Tuple for geometric objects that store (x,y,z)
 *
 * @author Peter Abeles
 */
public abstract class GeoTuple3D_F64 <T extends GeoTuple3D_F64> extends GeoTuple_F64<T> {
	public double x;
	public double y;
	public double z;

	public GeoTuple3D_F64( double x, double y, double z ) {
		this.x = x;
		this.y = y;
		this.z = z;
	}

	public GeoTuple3D_F64() {
	}

	@Override
	public int getDimension() {
		return 3;
	}

	protected void _set( GeoTuple3D_F64 a ) {
		x = a.x;
		y = a.y;
		z = a.z;
	}

	public void set( double x, double y, double z ) {
		this.x = x;
		this.y = y;
		this.z = z;
	}

	public boolean isIdentical( double x, double y, double z ) {
		return ( this.x == x && this.y == y && this.z == z );
	}

	public boolean isIdentical( double x, double y, double z, double tol ) {
		return ( Math.abs( this.x - x ) <= tol && Math.abs( this.y - y ) <= tol && Math.abs( this.z - z ) <= tol );
	}

	public boolean isIdentical( GeoTuple3D_F64 t, double tol ) {
		return ( Math.abs( this.x - t.x ) <= tol && Math.abs( this.y - t.y ) <= tol && Math.abs( this.z - t.z ) <= tol );
	}

	public double getX() {
		return x;
	}

	public double getY() {
		return y;
	}

	public double getZ() {
		return z;
	}

	public double getIdx(int index ) {
		switch( index ) {
			case 0:
				return x;

			case 1:
				return y;

			case 2:
				return z;

			default:
				throw new IllegalArgumentException( "Invalid index" );
		}
	}

	public void setIdx(int index, double value ) {
		switch( index ) {
			case 0:
				x = value;
				break;

			case 1:
				y = value;
				break;

			case 2:
				z = value;
				break;

			default:
				throw new IllegalArgumentException( "Invalid index" );
		}
	}

	/**
	 *
In-place addition

	 *
	 * this.x = this.x + a.x;
	 *
	 * @param a value which is to be added
	 */
	public void plusIP( GeoTuple3D_F64 a ) {
		x += a.x;
		y += a.y;
		z += a.z;
	}

	/**
	 *
Addition

	 *
	 * ret.x = this.x + a.x;
	 *
	 * @param a value which is to be added
	 */
	public T plus( GeoTuple3D_F64 a ) {
		T ret = createNewInstance();
		ret.x = x + a.x;
		ret.y = y + a.y;
		ret.z = z + a.z;
		return ret;
	}

	/**
	 * In-place scalar multiplication
	 * @param scalar value that it is multiplied by
	 */
	public void timesIP( double scalar ) {
		x *= scalar;
		y *= scalar;
		z *= scalar;
	}

	/**
	 * In-place scalar multiplication
	 * @param scalar value that it is multiplied by
	 */
	public void scale( double scalar ) {
		x *= scalar;
		y *= scalar;
		z *= scalar;
	}

	public void divideIP( double scalar ) {
		x /= scalar;
		y /= scalar;
		z /= scalar;
	}

	/**
	 * Scalar multiplication
	 * @param scalar value which is it multiplied by
	 * @return new matrix which is the original scaled
	 */
	public T times( double scalar ) {
		T ret = createNewInstance();
		ret.x = x*scalar;
		ret.y = y*scalar;
		ret.z = z*scalar;
		return ret;
	}

	public double norm() {
		return Math.sqrt( x * x + y * y + z * z );
	}

	public double normSq() {
		return x * x + y * y + z * z;
	}

	public double distance( double x , double y , double z ) {
		double dx = x - this.x;
		double dy = y - this.y;
		double dz = z - this.z;

		return Math.sqrt( dx * dx + dy * dy + dz * dz );
	}

	@Override
	public double distance( GeoTuple3D_F64 t ) {
		double dx = t.x - x;
		double dy = t.y - y;
		double dz = t.z - z;

		return Math.sqrt( dx * dx + dy * dy + dz * dz );
	}

	@Override
	public double distance2( GeoTuple3D_F64 t ) {
		double dx = t.x - x;
		double dy = t.y - y;
		double dz = t.z - z;

		return dx * dx + dy * dy + dz * dz;
	}

	public void print() {
		System.out.println( this );
	}

	public boolean isNaN() {
		return ( Double.isNaN( x ) || Double.isNaN( y ) || Double.isNaN( z ) );
	}

	public void setX( double x ) {
		this.x = x;
	}

	public void setY( double y ) {
		this.y = y;
	}

	public void setZ( double z ) {
		this.z = z;
	}

	@Override
	public boolean equals(Object obj) {
		if( this.getClass() != obj.getClass() )
			return false;
		GeoTuple3D_F64 p = (GeoTuple3D_F64)obj;
		return Double.compare(x,p.x)==0 && Double.compare(y,p.y)==0 && Double.compare(z,p.z)==0;
	}

	protected String toString( String name ) {
		DecimalFormat format = new DecimalFormat("#");
		String sx = UtilEjml.fancyString(x,format, MatrixIO.DEFAULT_LENGTH,4);
		String sy = UtilEjml.fancyString(y,format, MatrixIO.DEFAULT_LENGTH,4);
		String sz = UtilEjml.fancyString(z,format, MatrixIO.DEFAULT_LENGTH,4);

		return name+"( " + sx + " , " + sy + " , " + sz + " )";
	}

	@Override
	public int hashCode() {
		return Objects.hash(x, y, 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.metric;

import georegression.struct.point.Point2D_I32;
import georegression.struct.shapes.Polygon2D_I32;
import georegression.struct.shapes.Rectangle2D_I32;
import georegression.struct.shapes.RectangleLength2D_I32;

/**
 *
 *
 * @author Peter Abeles
 */
public class Intersection2D_I32 {
	/**
	 * 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_I32 a , Rectangle2D_I32 b ) {
		return( a.x0 < b.x1 && a.x1 > b.x0 && a.y0 < b.y1 && a.y1 > b.y0 );
	}

	/**
	 * 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_I32 a , Rectangle2D_I32 b , Rectangle2D_I32 result ) {
		if( !intersects(a,b) )
			return false;

		result.x0 = Math.max(a.x0,b.x0);
		result.x1 = Math.min(a.x1, b.x1);
		result.y0 = Math.max(a.y0,b.y0);
		result.y1 = Math.min(a.y1,b.y1);

		return true;
	}

	/**
	 * 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.
	 *
	 * @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_I32 polygon , Point2D_I32 pt )
	{
		final int N = polygon.size();

		boolean c = false;
		for (int i = 0, j = N-1; i < N; j = i++) {
			Point2D_I32 a = polygon.vertexes.data[i];
			Point2D_I32 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_I32 polygon , Point2D_I32 pt )
	{
		final int N = polygon.size();

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

			if( (pt.y >= a.y && pt.y < b.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)/(double)(b.y-a.y) + a.x;

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

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

		if( (pt.y >= a.y && pt.y < b.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)/(double)(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 rectangle
	 *
	 * @param a Rectangle
	 * @param x x-coordinate of point
	 * @param y y-coordinate of point
	 * @return true if the point is inside and false it is not
	 */
	public static boolean contains( RectangleLength2D_I32 a, int x, int y ) {
		if( a.getX() <= x && a.getX() + a.getWidth() > x ) {
			return a.getY() <= y && a.getY() + a.getHeight() > y;
		}
		return false;
	}

	/**
	 * True if the point is contained inside the rectangle
	 *
	 * @param a Rectangle
	 * @param x x-coordinate of point
	 * @param y y-coordinate of point
	 * @return true if the point is inside and false it is not
	 */
	public static boolean contains( Rectangle2D_I32 a, int x, int y ) {
		return( x >= a.x0 && y >= a.y0 && x < a.x1 && y < a.y1 );
	}

}


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

import org.ejml.UtilEjml;
import org.ejml.ops.MatrixIO;

import java.text.DecimalFormat;
import java.util.Objects;

/**
 * Generic Tuple for geometric objects that store (x,y)
 *
 * @author Peter Abeles
 */
public abstract class GeoTuple2D_F64 <T extends GeoTuple2D_F64> extends GeoTuple_F64<T> {
	public double x;
	public double y;

	public GeoTuple2D_F64( double x, double y ) {
		this.x = x;
		this.y = y;
	}

	public GeoTuple2D_F64() {
	}

	protected void _set( GeoTuple2D_F64 a ) {
		x = a.x;
		y = a.y;
	}

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

	public boolean isIdentical( double x, double y ) {
		return this.x == x && this.y == y;
	}

	public boolean isIdentical( double x, double y, double tol ) {
		return ( Math.abs( this.x - x ) <= tol && Math.abs( this.y - y ) <= tol );
	}

	public boolean isIdentical( T t, double tol ) {
		return ( Math.abs( this.x - t.x ) <= tol && Math.abs( this.y - t.y ) <= tol );
	}

	public void setX( double x ) {
		this.x = x;
	}

	public void setY( double y ) {
		this.y = y;
	}

	public double getX() {
		return x;
	}

	public double getY() {
		return y;
	}

	/**
	 *
In-place addition

	 *
	 * this.x = this.x + a.x;
	 *
	 * @param a value which is to be added
	 */
	public void plusIP( GeoTuple2D_F64 a ) {
		x += a.x;
		y += a.y;
	}

	/**
	 *
Addition

	 *
	 * ret.x = this.x + a.x;
	 *
	 * @param a value which is to be added
	 */
	public T plus( GeoTuple2D_F64 a ) {
		T ret = createNewInstance();
		ret.x = x + a.x;
		ret.y = y + a.y;
		return ret;
	}

	/**
	 * In-place scalar multiplication
	 * @param scalar value that it is multiplied by
	 */
	public void timesIP( double scalar ) {
		x *= scalar;
		y *= scalar;
	}

	/**
	 * In-place scalar multiplication
	 * @param scalar value that it is multiplied by
	 */
	public void scale( double scalar ) {
		x *= scalar;
		y *= scalar;
	}

	/**
	 * Scalar multiplication
	 * @param scalar value which is it multiplied by
	 * @return new matrix which is the original scaled
	 */
	public T times( double scalar ) {
		T ret = createNewInstance();
		ret.x = x*scalar;
		ret.y = y*scalar;
		return ret;
	}

	/**
	 * Euclidean distance from the point
	 * @param x x-coordinate
	 * @param y y-coordinate
	 * @return distance
	 */
	public double distance( double x , double y ) {
		double dx = x - this.x;
		double dy = y - this.y;

		return Math.sqrt(dx*dx + dy*dy);
	}

	/**
	 * Euclidean squared distance from the point
	 * @param x x-coordinate
	 * @param y y-coordinate
	 * @return distance squared
	 */
	public double distance2( double x , double y ) {
		double dx = x - this.x;
		double dy = y - this.y;

		return dx*dx + dy*dy;
	}

	@Override
	public double distance( T t ) {
		double dx = t.x - x;
		double dy = t.y - y;

		return Math.sqrt( dx * dx + dy * dy );
	}

	@Override
	public double distance2( T t ) {
		double dx = t.x - x;
		double dy = t.y - y;

		return dx * dx + dy * dy;
	}

	@Override
	public double getIdx(int index ) {
		switch( index ) {
			case 0:
				return x;

			case 1:
				return y;

			default:
				throw new IllegalArgumentException( "Invalid index" );
		}
	}

	@Override
	public void setIdx(int index, double value ) {
		switch( index ) {
			case 0:
				x = value;
				break;

			case 1:
				y = value;
				break;

			default:
				throw new IllegalArgumentException( "Invalid index" );
		}
	}

	@Override
	public double norm() {
		return Math.sqrt( x * x + y * y );
	}

	@Override
	public double normSq() {
		return x * x + y * y;
	}

	@Override
	public int getDimension() {
		return 2;
	}

	public void print() {
		System.out.println( this );
	}

	@Override
	public boolean equals(Object obj) {
		if( this.getClass() != obj.getClass() )
			return false;
		GeoTuple2D_F64 p = (GeoTuple2D_F64)obj;
		return Double.compare(x,p.x)==0 && Double.compare(y,p.y)==0;
	}

	protected String toString( String name ) {
		DecimalFormat format = new DecimalFormat("#");
		String sx = UtilEjml.fancyString(x,format, MatrixIO.DEFAULT_LENGTH,4);
		String sy = UtilEjml.fancyString(y,format, MatrixIO.DEFAULT_LENGTH,4);

		return name+"( " + sx + " " + sy + " )";
	}

	@Override
	public int hashCode() {
		return Objects.hash(x, y);
	}
}


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

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

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

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

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

	EllipseRotated_F64 ellipse;
	double ce;
	double se;

	// optimal value of parameterization
	double theta;

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

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

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

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

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

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

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

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

			theta = theta - f/d;
		}

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

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

	public Point2D_F64 getClosest() {
		return closest;
	}

	public double getTheta() {
		return theta;
	}
}


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

import java.io.Serializable;

/**
 *

 * Specifies a 3D rotation using a quaternion. q = w + x*i + y*j + z*k, where (w,x,y,z) are the parameters of
 * the quaternion and (i,j,k) are unit vectors representing the Cartesian axis.
 *

 *
 *

 * If the quaternion is a unit quaternion then the following is true:

 * q = cos(theta/2) + (x*i + y*j + z*k)*sin(theta/2)

 * where 'theta' is the angle of rotation, (x,y,z) is the unit axis of rotation.
 *

 *
 * @author Peter Abeles
 */
public class Quaternion_F64 implements Serializable {
	/**
	 * Describes the angle of rotation.  See above for how it is encoded.
	 */
	public double w;
	/**
	 * Axis of rotation
	 */
	public double x,y,z;

	public Quaternion_F64() {
		w = 1;
	}

	public Quaternion_F64(double w, double x, double y, double z) {
		set(w, x, y, z);
	}

	public void set( Quaternion_F64 quaternion ) {
		this.w = quaternion.w;
		this.x = quaternion.x;
		this.y = quaternion.y;
		this.z = quaternion.z;
	}

	public void set( double w, double x, double y, double z ) {
		this.w = w;
		this.x = x;
		this.y = y;
		this.z = z;
	}

	/**
	 * Converts the quaternion into a unit quaternion.
	 */
	public void normalize() {
		double n = Math.sqrt( w * w + x * x + y * y + z * z);

		w /= n;
		x /= n;
		y /= n;
		z /= n;
	}

   @Override
   public String toString() {
      return getClass().getSimpleName()+"{ w = "+ w +" axis( "+ x +" "+ y +" "+ z +") }";
   }
}


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

import georegression.fitting.FitShapeToPoints_F64;
import georegression.struct.curve.ConicGeneral_F64;
import georegression.struct.point.Point2D_F64;
import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.linsol.svd.SolveNullSpaceSvd_DDRM;
import org.ejml.interfaces.SolveNullSpace;

import java.util.List;

/**
 * Fit's a parabola to a set of points by finding the null space of A. More numerically stable than
 * {@link FitConicAtA_F64} but slower.
 *
 * @author Peter Abeles
 */
public class FitConicA_F64 implements FitShapeToPoints_F64<Point2D_F64,ConicGeneral_F64>
{
	private SolveNullSpace<DMatrixRMaj> solver = new SolveNullSpaceSvd_DDRM();

	private DMatrixRMaj A = new DMatrixRMaj(6,6);
	private DMatrixRMaj nullspace = new DMatrixRMaj(6,1);

	/**
	 * Fits the conic to the points.  Strongly recommended that you transform the points such that they have
	 * zero mean and a standard deviation along x and y axis, independently.
	 *
	 * @param points (Input) points
	 * @param output (Output) found conic
	 * @return true if successful or false if it failed
	 */
	@Override
	public boolean process(List<Point2D_F64> points , ConicGeneral_F64 output ) {
		final int N = points.size();
		if( N < 3 )
			throw new IllegalArgumentException("At least 3 points required");

		A.reshape(N,6);

		for (int i = 0,index=0; i < N; i++) {
			Point2D_F64 p = points.get(i);

			double x = p.x;
			double y = p.y;

			A.data[index++] = x*x;
			A.data[index++] = x*y;
			A.data[index++] = y*y;
			A.data[index++] = x;
			A.data[index++] = y;
			A.data[index++] = 1;
		}

		if( !solver.process(A,1,nullspace) )
			return false;

		output.A = nullspace.data[0];
		output.B = nullspace.data[1];
		output.C = nullspace.data[2];
		output.D = nullspace.data[3];
		output.E = nullspace.data[4];
		output.F = nullspace.data[5];

		return true;
	}

	/**
	 * Fits the conic to the weighted set of points.  Strongly recommended that you transform the points such
	 * that they have  zero mean and a standard deviation along x and y axis, independently.
	 *
	 * @param points (Input) points
	 * @param output (Output) found conic
	 * @return true if successful or false if it failed
	 */
	@Override
	public boolean process(List<Point2D_F64> points , double weights[], ConicGeneral_F64 output ) {
		final int N = points.size();
		if( N < 3 )
			throw new IllegalArgumentException("At least 3 points required");

		A.reshape(N,6);

		for (int i = 0,index=0; i < N; i++) {
			Point2D_F64 p = points.get(i);

			double w = weights[i];
			double x = p.x;
			double y = p.y;

			A.data[index++] = w*x*x;
			A.data[index++] = w*x*y;
			A.data[index++] = w*y*y;
			A.data[index++] = w*x;
			A.data[index++] = w*y;
			A.data[index++] = w;
		}

		if( !solver.process(A,1,nullspace) )
			return false;

		output.A = nullspace.data[0];
		output.B = nullspace.data[1];
		output.C = nullspace.data[2];
		output.D = nullspace.data[3];
		output.E = nullspace.data[4];
		output.F = nullspace.data[5];

		return true;
	}

	public SolveNullSpace<DMatrixRMaj> getSolver() {
		return solver;
	}

	public void setSolver(SolveNullSpace<DMatrixRMaj> solver) {
		this.solver = solver;
	}
}


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

/**
 * The area contained inside 2D shapes
 *
 * @author Peter Abeles
 */
public class Area2D_F64 {

	/**
	 * Computes the area of an arbitrary triangle from 3-vertices.
	 *
	 * area = | a.x*(b.y - c.y) + b.x*(c.y - a.y) + c.x*(a.y - b.y) | / 2
	 *
	 * @param a Corner point 1
	 * @param b Corner point 2
	 * @param c Corner point 3
	 * @return area
	 */
	public static double triangle( Point2D_F64 a, Point2D_F64 b, Point2D_F64  c ) {
		double inner = a.x*(b.y - c.y) + b.x*(c.y - a.y) + c.x*(a.y - b.y);

		return Math.abs(inner/2.0);
	}

	/**
	 * Area of a quadrilateral computed from two triangles.
	 *
	 * @param quad quadrilateral
	 * @return area
	 */
	public static double quadrilateral( Quadrilateral_F64 quad ) {
		double bx = quad.b.x-quad.a.x;
		double by = quad.b.y-quad.a.y;
		double cx = quad.c.x-quad.a.x;
		double cy = quad.c.y-quad.a.y;
		double dx = quad.d.x-quad.a.x;
		double dy = quad.d.y-quad.a.y;

		if( (bx * cy - by * cx >= 0) == (cx * dy - cy * dx >= 0)) {
			return triangle(quad.a,quad.b,quad.c) + triangle(quad.a,quad.c,quad.d);
		} else {
			return triangle(quad.a,quad.b,quad.d) + triangle(quad.b,quad.c,quad.d);
		}
	}

	/**
	 * Area of a simple polygon.  Meaning it can be concave or convex, but can't have self intersections
	 * @param poly Simple polygon
	 * @return area
	 */
	public static double polygonSimple( Polygon2D_F64 poly ) {
		double total = 0;

		Point2D_F64 v0 = poly.get(0);
		Point2D_F64 v1 = poly.get(1);
		for (int i = 2; i < poly.size(); i++) {
			Point2D_F64 v2 = poly.get(i);

			total += v1.x*( v2.y - v0.y);

			v0 = v1; v1 = v2;
		}

		Point2D_F64 v2 = poly.get(0);
		total += v1.x*( v2.y - v0.y);
		v0 = v1; v1 = v2;
		v2 = poly.get(1);
		total += v1.x*( v2.y - v0.y);

		return Math.abs(total/2.0);
	}
}