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

package georegression.geometry;

import georegression.struct.GeoTuple2D_F64;
import georegression.struct.GeoTuple3D_F64;
import georegression.struct.GeoTuple4D_F64;
import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.mult.VectorVectorMult_DDRM;

/**
 * Math operations that can be applied to geometric primitives.
 *
 * @author Peter Abeles
 */
// TODO rename to PerspectiveMath?
// todo separate off fucntions that are in homogeneous coordinates into their own class?
//      alternatively indicate by the function name?
// todo make sure all functions have unit tests
@SuppressWarnings({"unchecked", "RedundantCast"})
public class GeometryMath_F64 {

	/**
	 * Creates a skew symmetric cross product matrix from the provided tuple.
	 *
	 * @param x0  Element 0.
	 * @param x1  Element 1.
	 * @param x2  Element 2.
	 * @param ret If not null the results are stored here, otherwise a new matrix is created.
	 * @return Skew symmetric cross product matrix.
	 */
	public static DMatrixRMaj crossMatrix( double x0, double x1, double x2, DMatrixRMaj ret ) {
		if( ret == null ) {
			ret = new DMatrixRMaj( 3, 3 );
		} else {
			ret.zero();
		}

		ret.set( 0, 1, -x2 );
		ret.set( 0, 2, x1 );
		ret.set( 1, 0, x2 );
		ret.set( 1, 2, -x0 );
		ret.set( 2, 0, -x1 );
		ret.set( 2, 1, x0 );

		return ret;
	}

	/**
	 * Creates a skew symmetric cross product matrix from the provided tuple.
	 *
	 * @param v Tuple. Not modified.
	 * @param ret If not null the results are stored here, otherwise a new matrix is created.
	 * @return Skew symmetric cross product matrix.
	 */
	public static DMatrixRMaj crossMatrix( GeoTuple3D_F64 v, DMatrixRMaj ret ) {
		if( ret == null ) {
			ret = new DMatrixRMaj( 3, 3 );
		} else {
			ret.zero();
		}

		double x = v.getX();
		double y = v.getY();
		double z = v.getZ();

		ret.set( 0, 1, -z );
		ret.set( 0, 2, y );
		ret.set( 1, 0, z );
		ret.set( 1, 2, -x );
		ret.set( 2, 0, -y );
		ret.set( 2, 1, x );

		return ret;
	}

	/**
	 *

	 * Computes the cross product:

	 *

	 * c = a x b
	 *

	 *
	 * @param a Not modified.
	 * @param b Not modified.
	 * @param c Modified.
	 */
	public static void cross( GeoTuple3D_F64 a, GeoTuple3D_F64 b, GeoTuple3D_F64 c ) {
		c.x = a.y * b.z - a.z * b.y;
		c.y = a.z * b.x - a.x * b.z;
		c.z = a.x * b.y - a.y * b.x;
	}

	/**
	 *

	 * Computes the cross product:

	 *

	 * c = a x b
	 *

	 *
	 * @param a_x x-coordinate of a
	 * @param a_y y-coordinate of a
	 * @param a_z z-coordinate of a
	 * @param b_x x-coordinate of b
	 * @param b_y y-coordinate of b
	 * @param b_z z-coordinate of b
	 * @param c Modified.
	 */
	public static void cross( double a_x, double a_y , double a_z ,
							  double b_x, double b_y , double b_z,
							  GeoTuple3D_F64 c ) {
		c.x = a_y * b_z - a_z * b_y;
		c.y = a_z * b_x - a_x * b_z;
		c.z = a_x * b_y - a_y * b_x;
	}

	/**
	 *

	 * Computes the cross product:

	 *

	 * c = a x b

	 * where 'a' is in homogeneous coordinates.
	 *

	 *
	 * @param a Homogeneous coordinates, z = 1 assumed. Not modified.
	 * @param b Not modified.
	 * @param c Modified.
	 */
	public static void cross( GeoTuple2D_F64 a, GeoTuple3D_F64 b, GeoTuple3D_F64 c ) {
		c.x = a.y * b.z - b.y;
		c.y = b.x - a.x * b.z;
		c.z = a.x * b.y - a.y * b.x;
	}

	/**
	 *

	 * Computes the cross product:

	 *

	 * c = a x b
	 *

	 *
	 * @param a Homogeneous coordinates, z = 1 assumed. Not modified.
	 * @param b Homogeneous coordinates, z = 1 assumed. Not modified.
	 * @param c Modified.
	 */
	public static void cross( GeoTuple2D_F64 a, GeoTuple2D_F64 b, GeoTuple3D_F64 c ) {
		c.x = a.y * 1   -       b.y;
		c.y =       b.x - a.x;
		c.z = a.x * b.y - a.y * b.x;
	}

	/**
	 *

	 * Adds two points together.

	 *

	 * c = a + b
	 *

	 *

	 *

	 * Point 'c' can be the same instance as 'a' or 'b'.
	 *

	 *
	 * @param a A point. Not modified.
	 * @param b A point. Not modified.
	 * @param c Where the results are stored. Modified.
	 */
	public static void add( GeoTuple3D_F64 a, GeoTuple3D_F64 b, GeoTuple3D_F64 c ) {
		c.x = a.x + b.x;
		c.y = a.y + b.y;
		c.z = a.z + b.z;
	}

	/**
	 *

	 * Adds two points together while scaling them.

	 *

	 * pt2 = a0 pt0 + a1 pt1
	 *

	 *

	 *

	 * Point 'c' can be the same instance as 'a' or 'b'.
	 *

	 *
	 * @param a0 Scaling factor for pt0.
	 * @param pt0 A point. Not modified.
	 * @param a1 Scaling factor for pt1.
	 * @param pt1 A point. Not modified.
	 * @param pt2 Where the results are stored. Modified.
	 */
	public static void add( double a0, GeoTuple3D_F64 pt0, double a1, GeoTuple3D_F64 pt1, GeoTuple3D_F64 pt2 ) {
		pt2.x = a0 * pt0.x + a1 * pt1.x;
		pt2.y = a0 * pt0.y + a1 * pt1.y;
		pt2.z = a0 * pt0.z + a1 * pt1.z;
	}

	/**
	 *
ret = p0 + M*p1

	 *
	 * Safe to pass in the same instance of a point more than once.
	 */
	public static <T extends GeoTuple3D_F64> T addMult( T p0, DMatrixRMaj M, T p1, T result ) {
		if( M.numRows != 3 || M.numCols != 3 )
			throw new IllegalArgumentException( "Input matrix must be 3 by 3, not " + M.numRows + " " + M.numCols );

		if( result == null ) {
			result = (T) p0.createNewInstance();
		}

		double x = p1.x;
		double y = p1.y;
		double z = p1.z;

		result.x = p0.x + (double) ( M.data[0] * x + M.data[1] * y + M.data[2] * z );
		result.y = p0.y + (double) ( M.data[3] * x + M.data[4] * y + M.data[5] * z );
		result.z = p0.z + (double) ( M.data[6] * x + M.data[7] * y + M.data[8] * z );

		return result;
	}

	/**
	 *
ret = p0 + MT*p1

	 *
	 * Safe to pass in the same instance of a point more than once.
	 */
	public static <T extends GeoTuple3D_F64> T addMultTrans( T p0, DMatrixRMaj M, T p1, T result ) {
		if( M.numRows != 3 || M.numCols != 3 )
			throw new IllegalArgumentException( "Input matrix must be 3 by 3, not " + M.numRows + " " + M.numCols );

		if( result == null ) {
			result = (T) p0.createNewInstance();
		}

		double x = p1.x;
		double y = p1.y;
		double z = p1.z;

		result.x = p0.x + (double) ( M.data[0] * x + M.data[3] * y + M.data[6] * z );
		result.y = p0.y + (double) ( M.data[1] * x + M.data[4] * y + M.data[7] * z );
		result.z = p0.z + (double) ( M.data[2] * x + M.data[5] * y + M.data[8] * z );

		return result;
	}

	/**
	 *

	 * Substracts two points from each other.

	 *

	 * c = a - b
	 *

	 *

	 *

	 * Point 'c' can be the same instance as 'a' or 'b'.
	 *

	 *
	 * @param a A point. Not modified.
	 * @param b A point. Not modified.
	 * @param c Where the results are stored. Modified.
	 */
	public static void sub( GeoTuple3D_F64 a, GeoTuple3D_F64 b, GeoTuple3D_F64 c ) {
		c.x = a.x - b.x;
		c.y = a.y - b.y;
		c.z = a.z - b.z;
	}

	/**
	 * Rotates a 2D point by the specified angle.
	 *
	 * @param theta
	 * @param pt
	 * @param solution where the solution is written to.  Can be the same point as 'pt'.
	 */
	public static void rotate( double theta, GeoTuple2D_F64 pt, GeoTuple2D_F64 solution ) {
		double c = Math.cos( theta );
		double s = Math.sin( theta );

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

		solution.x = c * x - s * y;
		solution.y = s * x + c * y;
	}

	/**
	 * Rotates a 2D point by the specified angle.
	 *
	 * @param c Cosine of theta
	 * @param s Sine of theta
	 * @param pt
	 * @param solution where the solution is written to.  Can be the same point as 'pt'.
	 */
	public static void rotate( double c , double s, GeoTuple2D_F64 pt, GeoTuple2D_F64 solution ) {

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

		solution.x = c * x - s * y;
		solution.y = s * x + c * y;
	}

	/**
	 * mod = M*pt
	 *

	 * pt and mod can be the same reference.
	 *

	 *
	 * @param M
	 * @param pt
	 * @param result Storage for output.  Can be the same instance as param 'pt'.  Modified.
	 */
	public static <T extends GeoTuple3D_F64> T mult( DMatrixRMaj M, T pt, T result ) {
		if( M.numRows != 3 || M.numCols != 3 )
			throw new IllegalArgumentException( "Input matrix must be 3 by 3, not " + M.numRows + " " + M.numCols );

		if( result == null ) {
			result = (T) pt.createNewInstance();
		}

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

		result.x = (double) ( M.data[0] * x + M.data[1] * y + M.data[2] * z );
		result.y = (double) ( M.data[3] * x + M.data[4] * y + M.data[5] * z );
		result.z = (double) ( M.data[6] * x + M.data[7] * y + M.data[8] * z );

		return (T) result;
	}

	/**
	 *

	 * mod = M*pt

	 * where mod is a 2D point that has an implicit z=1.
	 *

	 *
	 *

	 * Multiplies the 3x3 matrix against the 3D point, and normalizes the 2D point output
	 * by dividing the x and y values by the found z.
	 *

	 */
	public static <T extends GeoTuple2D_F64> T mult( DMatrixRMaj M, GeoTuple3D_F64 pt, T mod ) {
		if( M.numRows != 3 || M.numCols != 3 )
			throw new IllegalArgumentException( "Input matrix must be 3 by 3, not " + M.numRows + " " + M.numCols );

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

		mod.x = (double) ( M.unsafe_get(0, 0) * x + M.unsafe_get(0, 1) * y + M.unsafe_get(0, 2) * z );
		mod.y = (double) ( M.unsafe_get(1, 0) * x + M.unsafe_get(1, 1) * y + M.unsafe_get(1, 2) * z );
		z = (double) ( M.unsafe_get(2, 0) * x + M.unsafe_get(2, 1) * y + M.unsafe_get(2, 2) * z );

		mod.x /= z;
		mod.y /= z;

		return mod;
	}

	/**
	 *

	 * mod = M*pt

	 * where pt has z=1 implicitly.
	 *

	 *
	 *

	 * Multiplies the 3x3 matrix against the 2D point, which has an implicit z=1 value, and the output is
	 * a 3d point.
	 *

	 */
	public static void mult( DMatrixRMaj M, GeoTuple2D_F64 pt, GeoTuple3D_F64 mod ) {
		if( M.numRows != 3 || M.numCols != 3 )
			throw new IllegalArgumentException( "Input matrix must be 3 by 3, not " + M.numRows + " " + M.numCols );

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

		mod.x = (double) ( M.unsafe_get(0, 0) * x + M.unsafe_get(0, 1) * y + M.unsafe_get(0, 2) );
		mod.y = (double) ( M.unsafe_get(1, 0) * x + M.unsafe_get(1, 1) * y + M.unsafe_get(1, 2) );
		mod.z = (double) ( M.unsafe_get(2, 0) * x + M.unsafe_get(2, 1) * y + M.unsafe_get(2, 2) );
	}

	/**
	 *

	 * Computes mod =  M*pt, where both pt and mod are in homogeneous coordinates with z assumed to be
	 * equal to 1, and M is a 3x3 matrix.
	 *

	 *

	 * 'pt' and 'mod' can be the same point.
	 *

	 * @param M  3x3 matrix
	 * @param pt Homogeneous point with z=1
	 * @param mod Storage for the computation.  If null a new point is declared.  Can be same instance as pt.
	 * @return Result of computation.
	 */
	public static <T extends GeoTuple2D_F64> T mult( DMatrixRMaj M, T pt, T mod ) {
		if( M.numRows != 3 || M.numCols != 3 )
			throw new IllegalArgumentException( "Input matrix must be 3 by 3, not " + M.numRows + " " + M.numCols );

		if( mod == null ) {
			throw new IllegalArgumentException( "Must provide an instance in mod" );
		}

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

		double modz = (double) ( M.unsafe_get( 2, 0 ) * x + M.unsafe_get(2, 1) * y + M.unsafe_get(2, 2) );

		mod.x = (double) ( ( M.unsafe_get(0, 0) * x + M.unsafe_get(0, 1) * y + M.unsafe_get(0, 2) ) / modz );
		mod.y = (double) ( ( M.unsafe_get(1, 0) * x + M.unsafe_get(1, 1) * y + M.unsafe_get(1, 2) ) / modz );

		return mod;
	}

	/**
	 * x = P*X
	 *
	 * @param P Projective 3x4 matrix
	 * @param X 3D point in homogenous coordinates
	 * @param mod 2D point in homogenous coordinates
	 */
	public static void mult(DMatrixRMaj P, GeoTuple4D_F64 X, GeoTuple3D_F64 mod ) {
		if( P.numRows != 3 || P.numCols != 4 )
			throw new IllegalArgumentException( "Input matrix must be 3 by 4 not " + P.numRows + " " + P.numCols );

		mod.x = P.data[0]*X.x + P.data[1]*X.y + P.data[2]*X.z + P.data[3]*X.w;
		mod.y = P.data[4]*X.x + P.data[5]*X.y + P.data[6]*X.z + P.data[7]*X.w;
		mod.z = P.data[8]*X.x + P.data[9]*X.y + P.data[10]*X.z + P.data[11]*X.w;
	}

	/**
	 * x = (P*X)/z
	 *
	 * @param P Projective 3x4 matrix
	 * @param X 3D point in homogenous coordinates
	 * @param mod 2D point in coordinates
	 */
	public static void mult(DMatrixRMaj P, GeoTuple4D_F64 X, GeoTuple2D_F64 mod ) {
		if( P.numRows != 3 || P.numCols != 4 )
			throw new IllegalArgumentException( "Input matrix must be 3 by 4 not " + P.numRows + " " + P.numCols );

		double x = P.data[0]*X.x + P.data[1]*X.y + P.data[2]*X.z + P.data[3]*X.w;
		double y = P.data[4]*X.x + P.data[5]*X.y + P.data[6]*X.z + P.data[7]*X.w;
		double z = P.data[8]*X.x + P.data[9]*X.y + P.data[10]*X.z + P.data[11]*X.w;

		mod.x = x/z;
		mod.y = y/z;
	}

	/**
	 *

	 * Computes the following:

	 * result = cross(A)*M

	 * where M and result are 3x3 matrices, cross(A) is the cross product matrix of A.
	 *

	 *
	 * @param A 2D homogenous coordinate (implicit z = 1) that is internally converted into cross product matrix.
	 * @param M 3x3 matrix.
	 * @param result Storage for results.  Can be null.
	 * @return Results.
	 */
	public static DMatrixRMaj multCrossA( GeoTuple2D_F64 A , DMatrixRMaj M, DMatrixRMaj result ) {
		if( M.numRows != 3 || M.numCols != 3 )
			throw new IllegalArgumentException( "Input matrix must be 3 by 3, not " + M.numRows + " " + M.numCols );

		if( result == null ) {
			result = new DMatrixRMaj(3,3);
		}

		double x = A.x;
		double y = A.y;

		double a11 = M.data[0]; double a12 = M.data[1]; double a13 = M.data[2];
		double a21 = M.data[3]; double a22 = M.data[4]; double a23 = M.data[5];
		double a31 = M.data[6]; double a32 = M.data[7]; double a33 = M.data[8];

		result.data[0] = -a21 + a31*y;
		result.data[1] = -a22 + a32*y;
		result.data[2] = -a23 + a33*y;
		result.data[3] = a11 - a31*x;
		result.data[4] = a12 - a32*x;
		result.data[5] = a13 - a33*x;
		result.data[6] = -a11*y + a21*x;
		result.data[7] = -a12*y + a22*x;
		result.data[8] = -a13*y + a23*x;

		return result;
	}

	/**
	 *

	 * Computes the following:

	 * result = cross(A)T*M

	 * where M and result are 3x3 matrices, cross(A) is the cross product matrix of A.
	 *

	 *
	 * @param A 2D homogenous coordinate (implicit z = 1) that is internally converted into cross product matrix.
	 * @param M 3x3 matrix.
	 * @param result Storage for results.  Can be null.
	 * @return Results.
	 */
	public static DMatrixRMaj multCrossATransA( GeoTuple2D_F64 A , DMatrixRMaj M, DMatrixRMaj result ) {
		if( M.numRows != 3 || M.numCols != 3 )
			throw new IllegalArgumentException( "Input matrix must be 3 by 3, not " + M.numRows + " " + M.numCols );

		if( result == null ) {
			result = new DMatrixRMaj(3,3);
		}

		double x = A.x;
		double y = A.y;

		double a11 = M.data[0]; double a12 = M.data[1]; double a13 = M.data[2];
		double a21 = M.data[3]; double a22 = M.data[4]; double a23 = M.data[5];
		double a31 = M.data[6]; double a32 = M.data[7]; double a33 = M.data[8];

		result.data[0] =  a21   - a31*y;
		result.data[1] =  a22   - a32*y;
		result.data[2] =  a23   - a33*y;
		result.data[3] = -a11   + a31*x;
		result.data[4] = -a12   + a32*x;
		result.data[5] = -a13   + a33*x;
		result.data[6] =  a11*y - a21*x;
		result.data[7] =  a12*y - a22*x;
		result.data[8] =  a13*y - a23*x;

		return result;
	}

	/**
	 *

	 * Computes the following:

	 * result = cross(A)*M

	 * where M and result are 3x3 matrices, cross(A) is the cross product matrix of A.
	 *

	 *
	 * @param A 3D coordinate that is internally converted into cross product matrix.
	 * @param M 3x3 matrix.
	 * @param result Storage for results.  Can be null.
	 * @return Results.
	 */
	public static DMatrixRMaj multCrossA( GeoTuple3D_F64 A , DMatrixRMaj M, DMatrixRMaj result ) {
		if( M.numRows != 3 || M.numCols != 3 )
			throw new IllegalArgumentException( "Input matrix must be 3 by 3, not " + M.numRows + " " + M.numCols );

		if( result == null ) {
			result = new DMatrixRMaj(3,3);
		}

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

		double a11 = M.data[0]; double a12 = M.data[1]; double a13 = M.data[2];
		double a21 = M.data[3]; double a22 = M.data[4]; double a23 = M.data[5];
		double a31 = M.data[6]; double a32 = M.data[7]; double a33 = M.data[8];

		result.data[0] = -a21*z + a31*y;
		result.data[1] = -a22*z + a32*y;
		result.data[2] = -a23*z + a33*y;
		result.data[3] =  a11*z - a31*x;
		result.data[4] =  a12*z - a32*x;
		result.data[5] =  a13*z - a33*x;
		result.data[6] = -a11*y + a21*x;
		result.data[7] = -a12*y + a22*x;
		result.data[8] = -a13*y + a23*x;

		return result;
	}

	/**
	 *

	 * Computes the following:

	 * result = cross(A)T*M

	 * where M and result are 3x3 matrices, cross(A) is the cross product matrix of A.
	 *

	 *
	 * @param A 3D coordinate that is internally converted into cross product matrix.
	 * @param M 3x3 matrix.
	 * @param result Storage for results.  Can be null.
	 * @return Results.
	 */
	public static DMatrixRMaj multCrossATransA( GeoTuple3D_F64 A , DMatrixRMaj M, DMatrixRMaj result ) {
		if( M.numRows != 3 || M.numCols != 3 )
			throw new IllegalArgumentException( "Input matrix must be 3 by 3, not " + M.numRows + " " + M.numCols );

		if( result == null ) {
			result = new DMatrixRMaj(3,3);
		}

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

		double a11 = M.data[0]; double a12 = M.data[1]; double a13 = M.data[2];
		double a21 = M.data[3]; double a22 = M.data[4]; double a23 = M.data[5];
		double a31 = M.data[6]; double a32 = M.data[7]; double a33 = M.data[8];

		result.data[0] =  a21*z - a31*y;
		result.data[1] =  a22*z - a32*y;
		result.data[2] =  a23*z - a33*y;
		result.data[3] = -a11*z + a31*x;
		result.data[4] = -a12*z + a32*x;
		result.data[5] = -a13*z + a33*x;
		result.data[6] =  a11*y - a21*x;
		result.data[7] =  a12*y - a22*x;
		result.data[8] =  a13*y - a23*x;

		return result;
	}

	/**
	 * mod = MT*pt.  Both pt and mod can be the same instance.
	 *
	 *
	 */
	public static <T extends GeoTuple3D_F64> T multTran( DMatrixRMaj M, T pt, T mod ) {
		if( M.numRows != 3 || M.numCols != 3 )
			throw new IllegalArgumentException( "Rotation matrices are 3 by 3." );

		if( mod == null ) {
			mod = (T) pt.createNewInstance();
		}

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

		mod.x = (double) ( M.unsafe_get( 0, 0 ) * x + M.unsafe_get( 1, 0 ) * y + M.unsafe_get( 2, 0 ) * z );
		mod.y = (double) ( M.unsafe_get( 0, 1 ) * x + M.unsafe_get( 1, 1 ) * y + M.unsafe_get( 2, 1 ) * z );
		mod.z = (double) ( M.unsafe_get( 0, 2 ) * x + M.unsafe_get( 1, 2 ) * y + M.unsafe_get( 2, 2 ) * z );

		return (T) mod;
	}

	/**
	 * mod = MT*pt

	 * where pt.z = 1 implicitly.
	 *
	 * @param M 3 by 3 matrix.
	 * @param pt 2D point in homogeneous coordinates. Implicit z = 1
	 * @param mod 2D point in homogeneous coordinates.  Implicit z = 1
	 * @return 2D point in homogeneous coordinates.  Implicit z = 1
	 */
	public static <T extends GeoTuple3D_F64> T multTran( DMatrixRMaj M, GeoTuple2D_F64 pt, T mod ) {
		if( M.numRows != 3 || M.numCols != 3 )
			throw new IllegalArgumentException( "Rotation matrices are 3 by 3." );

		if( mod == null ) {
			throw new IllegalArgumentException( "Must provide an instance in mod" );
		}

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

		mod.x = (double) ( M.unsafe_get( 0, 0 ) * x + M.unsafe_get( 1, 0 ) * y + M.unsafe_get( 2, 0 ) );
		mod.y = (double) ( M.unsafe_get( 0, 1 ) * x + M.unsafe_get( 1, 1 ) * y + M.unsafe_get( 2, 1 ) );
		mod.z = (double) ( M.unsafe_get( 0, 2 ) * x + M.unsafe_get( 1, 2 ) * y + M.unsafe_get( 2, 2 ) );

		return mod;
	}

	/**
	 * mod = MT*pt

	 * where pt.z = 1 implicitly.
	 *
	 * @param M 3 by 3 matrix.
	 * @param pt 2D point in homogeneous coordinates. Implicit z = 1
	 * @param mod 2D point in homogeneous coordinates.  Implicit z = 1
	 * @return 2D point in homogeneous coordinates.  Implicit z = 1
	 */
	public static <T extends GeoTuple2D_F64> T multTran( DMatrixRMaj M, GeoTuple2D_F64 pt, T mod ) {
		if( M.numRows != 3 || M.numCols != 3 )
			throw new IllegalArgumentException( "Rotation matrices are 3 by 3." );

		if( mod == null ) {
			throw new IllegalArgumentException( "Must provide an instance in mod" );
		}

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

		double modZ = (double) ( M.unsafe_get( 0, 2 ) * x + M.unsafe_get( 1, 2 ) * y + M.unsafe_get( 2, 2 ) );
		mod.x = (double) ( M.unsafe_get( 0, 0 ) * x + M.unsafe_get( 1, 0 ) * y + M.unsafe_get( 2, 0 ) )/modZ;
		mod.y = (double) ( M.unsafe_get( 0, 1 ) * x + M.unsafe_get( 1, 1 ) * y + M.unsafe_get( 2, 1 ) )/modZ;

		return mod;
	}

	/**
	 *

	 * ret = aT*M*b
	 *

	 *
	 * @param a 3D point.
	 * @param M 3 by 3 matrix.
	 * @param b 3D point.
	 * @return scalar number
	 */
	public static double innerProd( GeoTuple3D_F64 a, DMatrixRMaj M, GeoTuple3D_F64 b ) {
		if( M.numRows != 3 || M.numCols != 3 )
			throw new IllegalArgumentException( "M must be 3 by 3." );

		DMatrixRMaj m1 = new DMatrixRMaj( 3, 1, true, a.x, a.y, a.z );
		DMatrixRMaj m2 = new DMatrixRMaj( 3, 1, true, b.x, b.y, b.z );

		return (double) ( VectorVectorMult_DDRM.innerProdA( m1, M, m2 ) );
	}

	/**
	 *

	 * Computes the inner matrix product:

	 * ret = xTATy
	 *

	 *
	 * @param a 3D point.
	 * @param M 3 by 3 matrix.
	 * @param b 3D point.
	 * @return scalar number
	 */
	public static double innerProdTranM( GeoTuple3D_F64 a, DMatrixRMaj M, GeoTuple3D_F64 b ) {
		if( M.numRows != 3 || M.numCols != 3 )
			throw new IllegalArgumentException( "M must be 3 by 3." );

		DMatrixRMaj m1 = new DMatrixRMaj( 3, 1, true, a.x, a.y, a.z );
		DMatrixRMaj m2 = new DMatrixRMaj( 3, 1, true, b.x, b.y, b.z );

		return (double) ( VectorVectorMult_DDRM.innerProdTranA( m1, M, m2 ) );
	}

	/**
	 * Computes the outer product of two vectors:

	 * O = a*bT
	 *
	 * @param a 3D vector
	 * @param b 3D vector
	 * @param ret 3 x 3 matrix or null.
	 * @return outer product of two 3d vectors
	 */
	public static DMatrixRMaj outerProd(GeoTuple3D_F64 a, GeoTuple3D_F64 b, DMatrixRMaj ret) {
		if( ret == null )
			ret = new DMatrixRMaj(3,3);

		ret.data[0] = a.x*b.x;
		ret.data[1] = a.x*b.y;
		ret.data[2] = a.x*b.z;
		ret.data[3] = a.y*b.x;
		ret.data[4] = a.y*b.y;
		ret.data[5] = a.y*b.z;
		ret.data[6] = a.z*b.x;
		ret.data[7] = a.z*b.y;
		ret.data[8] = a.z*b.z;

		return ret;
	}

	/**
	 * Adds the outer product of two vectors onto a matrix:

	 * ret = A + scalar*a*bT
	 *
	 * @param A 3x3 matrix
	 * @param b 3D vector
	 * @param c 3D vector
	 * @param ret 3 x 3 matrix or null.
	 * @return outer product of two 3d vectors
	 */
	public static DMatrixRMaj addOuterProd(DMatrixRMaj A , double scalar , GeoTuple3D_F64 b, GeoTuple3D_F64 c, DMatrixRMaj ret) {
		if( ret == null )
			ret = new DMatrixRMaj(3,3);

		ret.data[0] = A.data[0] + scalar*b.x*c.x;
		ret.data[1] = A.data[1] + scalar*b.x*c.y;
		ret.data[2] = A.data[2] + scalar*b.x*c.z;
		ret.data[3] = A.data[3] + scalar*b.y*c.x;
		ret.data[4] = A.data[4] + scalar*b.y*c.y;
		ret.data[5] = A.data[5] + scalar*b.y*c.z;
		ret.data[6] = A.data[6] + scalar*b.z*c.x;
		ret.data[7] = A.data[7] + scalar*b.z*c.y;
		ret.data[8] = A.data[8] + scalar*b.z*c.z;

		return ret;
	}

	/**
	 *

	 * Computes the inner matrix product: ret = a'*M*b

	 * where ret is a scalar number. 'a' and 'b' are automatically converted into homogeneous
	 * coordinates.
	 *

	 *
	 * @param a 2D point.
	 * @param M 3 by 3 matrix.
	 * @param b 2D point.
	 * @return scalar number,
	 */
	public static double innerProd( GeoTuple2D_F64 a, DMatrixRMaj M, GeoTuple2D_F64 b ) {
		if( M.numRows != 3 || M.numCols != 3 )
			throw new IllegalArgumentException( "M must be 3 by 3." );

		DMatrixRMaj m1 = new DMatrixRMaj( 3, 1, true, a.x, a.y, 1 );
		DMatrixRMaj m2 = new DMatrixRMaj( 3, 1, true, b.x, b.y, 1 );

		return (double) ( VectorVectorMult_DDRM.innerProdA( m1, M, m2 ) );
	}

	/**
	 *

	 * Dot product: ret = aTb
	 *

	 *
	 * @param a A tuple.
	 * @param b A tuple.
	 * @return scalar
	 */
	public static double dot( GeoTuple3D_F64 a, GeoTuple3D_F64 b ) {
		return a.x * b.x + a.y * b.y + a.z * b.z;
	}

	/**
	 *

	 * Multiplies each element in the tuple by 'v'.

	 * pi=pi*v
	 *

	 *
	 * @param p tuple.
	 * @param v scaling factor.
	 */
	public static void scale( GeoTuple3D_F64 p, double v ) {
		p.x *= v;
		p.y *= v;
		p.z *= v;
	}

	/**
	 * Divides each element by 'v'
	 *
	 * @param p tuple
	 * @param v divisor
	 */
	public static void divide( GeoTuple3D_F64 p , double v ) {
		p.x /= v;
		p.y /= v;
		p.z /= v;
	}

	/**
	 *

	 * Changes the sign of the vector:

	 *

	 * T = -T
	 *

	 *
	 * @param t Vector whose sign is being changed.  Modified.
	 */
	public static void changeSign( GeoTuple3D_F64 t ) {
		t.x = -t.x;
		t.y = -t.y;
		t.z = -t.z;
	}

	/**
	 * Converts a GeoTuple3D_F64 into DMatrixRMaj
	 *
	 * @param in Input vector
	 * @param out Output matrix.  If null a new matrix will be declared
	 * @return Converted matrix
	 */
	public static DMatrixRMaj toMatrix(GeoTuple3D_F64 in, DMatrixRMaj out) {
		if( out == null )
			out = new DMatrixRMaj(3,1);
		else if( out.getNumElements() != 3 )
			throw new IllegalArgumentException("Vector with 3 elements expected");

		out.data[0] = in.x;
		out.data[1] = in.y;
		out.data[2] = in.z;

		return out;
	}

	/**
	 * Converts a DMatrixRMaj into  GeoTuple3D_F64
	 *
	 * @param in Input matrix
	 * @param out Output vector.
	 */
	public static void toTuple3D(DMatrixRMaj in, GeoTuple3D_F64 out) {

		out.x = (double)in.get(0);
		out.y = (double)in.get(1);
		out.z = (double)in.get(2);
	}
}


/*
 * 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 georegression.struct.point.Vector3D_F64;

import java.io.Serializable;

/**
 *

 * Defines a 3D rotation based upon the axis of rotation and the angle of rotation in SO(3).
 *

 *
 *

 * R(θ) = ew*θ

 * R(θ) = 1 + w*sin(θ) + w2*(1-cos(θ))

 * where θ is the angle of rotation, 'w' is a skew symmetric matrix around the axis of rotation.
 *

 *
 * @author Peter Abeles
 */
public class Rodrigues_F64 implements Serializable {
	// unit vector defining the axis of rotation
	public Vector3D_F64 unitAxisRotation = new Vector3D_F64();
	// the angle it is rotated by
	public double theta;

	public Rodrigues_F64() {
	}

	/**
	 * Constructor which specifies the transform.
	 *
	 * @param theta Angle of rotation
	 * @param unitAxisRotation  Axis of rotation   Must be normalized to 1.
	 */
	public Rodrigues_F64(double theta, Vector3D_F64 unitAxisRotation) {
		this.theta = theta;
		this.unitAxisRotation.set(unitAxisRotation);
	}

	/**
	 * Constructor which specifies the transform using an unnormalized rotation axis.
	 *
	 * @param theta Angle of rotation
	 * @param x Axis of rotation. x-component.
	 * @param y Axis of rotation. y-component.
	 * @param z Axis of rotation. z-component.
	 */
	public Rodrigues_F64(double theta, double x, double y, double z) {
		this.theta = theta;
		this.unitAxisRotation.set( x, y, z );
		unitAxisRotation.normalize();
	}

	public Vector3D_F64 getUnitAxisRotation() {
		return unitAxisRotation;
	}

	public void setUnitAxisRotation(Vector3D_F64 unitAxisRotation) {
		this.unitAxisRotation.set(unitAxisRotation);
	}

	public double getTheta() {
		return theta;
	}

	public void setTheta( double theta ) {
		this.theta = theta;
	}

	/**
	 * Assign the Rodrigues coordinates using a 3 element vector.  Theta is the vector's
	 * magnitude and the axis of rotation is the unit vector.
	 *
	 * @param x x-component of 3 vector
	 * @param y y-component of 3 vector
	 * @param z z-component of 3 vector
	 */
	public void setParamVector( double  x , double y , double z ) {
		double ax = Math.abs(x);
		double ay = Math.abs(y);
		double az = Math.abs(z);

		double max = Math.max(ax,ay);
		max = Math.max(max,az);

		if( max == 0 ) {
			theta = 0;
			unitAxisRotation.set(1,0,0);
		} else {
			x /= max;
			y /= max;
			z /= max;
			theta = Math.sqrt(x*x + y*y + z*z);
			unitAxisRotation.x = x/theta;
			unitAxisRotation.y = y/theta;
			unitAxisRotation.z = z/theta;
			theta *= max;
		}
	}

	@Override
	public String toString() {
		return getClass().getSimpleName()+
				" v{ "+unitAxisRotation.x+" , "+unitAxisRotation.y+" , "+unitAxisRotation.z+" } theta = "+
				theta;
	}
}


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

import static java.lang.Math.PI;

/**
 * Utility functions relating to angles.  Unless otherwise state all angles
 * are in radians and have a domain of -pi to pi
 */
public class UtilAngle {

	public static double radianToDegree( double angleRad ) {
		return 180.0 * angleRad / Math.PI;
	}

	public static double degreeToRadian( double angleDegree ) {
		return Math.PI * angleDegree / 180.0;
	}

	public static float radianToDegree( float angleRad ) {
		return 180.0f * angleRad / GrlConstants.F_PI;
	}

	public static float degreeToRadian( float angleDegree ) {
		return GrlConstants.F_PI * angleDegree / 180.0f;
	}

	/**
	 * Converts radians into degrees
	 * @param angleDegree angle in degrees
	 * @return angle in radians
	 */
	public static double radian( double angleDegree ) {
		return degreeToRadian(angleDegree);
	}

	/**
	 * Converts degrees into radians
	 * @param angleRad angle in radians
	 * @return angle in degrees
	 */
	public static double degree( double angleRad ) {
		return radianToDegree(angleRad);
	}

	/**
	 * Converts radians into degrees
	 * @param angleDegree angle in degrees
	 * @return angle in radians
	 */
	public static float radian( float angleDegree ) {
		return degreeToRadian(angleDegree);
	}

	/**
	 * Converts degrees into radians
	 * @param angleRad angle in radians
	 * @return angle in degrees
	 */
	public static float degree( float angleRad ) {
		return radianToDegree(angleRad);
	}

	public static double atanSafe( double y , double x ) {
		if( x == 0.0 ) {
			if( y >= 0.0)
				return Math.PI / 2;
			else
				return -Math.PI / 2;
		}
		return Math.atan(y/x);
	}

	public static float atanSafe( float y , float x ) {
		if( x == 0.0 )
			if( y >= 0.0)
				return GrlConstants.F_PId2;
			else
				return -GrlConstants.F_PId2;
		return (float)Math.atan(y/x);
	}

	/**
	 * Converts an angle which is (-pi to pi) into a half circle angle (-pi/2 to pi/2).
	 *
	 * @param angle Angle between -pi and pi.
	 * @return -pi/2 to pi/2
	 */
	public static double toHalfCircle( double angle ) {
		if( angle < 0 )
			angle += Math.PI;
		if( angle > Math.PI/2.0 )
			angle -= Math.PI;
		return angle;
	}

	/**
	 * Converts an angle which is (-pi to pi) into a half circle angle (0 to pi).
	 *
	 * @param angle Angle between -pi and pi.
	 * @return 0 to pi
	 */
	public static float toHalfCircle( float angle ) {
		if( angle >= 0 )
			return angle;
		return GrlConstants.F_PI+angle;
	}

	/**
	 * Checks to see if it is between -π and π.
	 *
	 * @param ang Angle being tested
	 * @return true if it is between -π and π
	 */
	public static boolean isStandardDomain( double ang ) {
		return ( ang <= Math.PI && ang >= -Math.PI );
	}

	/**
	 * Checks to see if it is between -π/2 and π/2.
	 *
	 * @param ang Angle being tested
	 * @return true if it is between -π/2 and π/2;
	 */
	public static boolean isHalfDomain( double ang ) {
		return ( ang <= Math.PI/2 && ang >= -Math.PI/2 );
	}

	/**
	 * Returns an angle which is equivalent to the one provided, but between (inclusive) -π and π.
	 */
	public static double bound( double ang ) {
		ang %= GrlConstants.PI2;

		if( ang > PI ) {
			return ang - GrlConstants.PI2;
		} else if( ang < -PI ) {
			return ang + GrlConstants.PI2;
		}

		return ang;
	}

	/**
	 * Returns an angle which is equivalent to the one provided, but between (inclusive) -π and π.
	 */
	public static float bound( float ang ) {
		ang %= GrlConstants.F_PI2;

		if( ang > GrlConstants.F_PI ) {
			return ang - GrlConstants.F_PI2;
		} else if( ang < -GrlConstants.F_PI ) {
			return ang + GrlConstants.F_PI2;
		}

		return ang;
	}

	/**
	 * Bounds the angle between -π/2 and π/2
	 * @param angle angle in radians
	 * @return bounded angle
	 */
	public static double boundHalf( double angle ) {
		angle = bound(angle);

		if( angle > GrlConstants.PId2 ) {
			angle -= Math.PI;
		} else if( angle < -GrlConstants.PId2 ){
			angle += Math.PI;
		}
		return angle;
	}

	/**
	 * Bounds the angle between -π/2 and π/2
	 * @param angle angle in radians
	 * @return bounded angle
	 */
	public static float boundHalf( float angle ) {
		angle = bound(angle);

		if( angle > GrlConstants.F_PId2 ) {
			angle -= GrlConstants.F_PI;
		} else if( angle < -GrlConstants.F_PId2 ){
			angle += GrlConstants.F_PI;
		}
		return angle;
	}

	/**
	 * Angular distance in radians to go from angA to angB in counter clock-wise direction.
	 * The resulting angle will be from 0 to 2π.
	 *
	 * @param angA First angle. -pi to pi
	 * @param angB Second angle -pi to pi
	 * @return An angle from 0 to 2 π
	 */
	public static double distanceCCW( double angA, double angB ) {
		if( angB >= angA )
			return angB-angA;
		else
			return GrlConstants.PI2 - (angA-angB);
	}

	/**
	 * Angular distance in radians to go from angA to angB in counter clock-wise direction.
	 * The resulting angle will be from 0 to 2π.
	 *
	 * @param angA First angle. -pi to pi
	 * @param angB Second angle -pi to pi
	 * @return An angle from 0 to 2 π
	 */
	public static float distanceCCW( float angA, float angB ) {
		if( angB >= angA )
			return angB-angA;
		else
			return GrlConstants.F_PI2 - (angA-angB);
	}

	/**
	 * Angular distance in radians to go from angA to angB in clock-wise direction.
	 * The resulting angle will be from 0 to 2π.
	 *
	 * @param angA First angle. -pi to pi
	 * @param angB Second angle   -pi to pi
	 * @return An angle from 0 to 2 π
	 */
	public static double distanceCW( double angA, double angB ) {
		if( angA >= angB )
			return angA-angB;
		else
			return GrlConstants.PI2-(angB-angA);
	}

	/**
	 * Angular distance in radians to go from angA to angB in clock-wise direction.
	 * The resulting angle will be from 0 to 2π.
	 *
	 * @param angA First angle. -pi to pi
	 * @param angB Second angle   -pi to pi
	 * @return An angle from 0 to 2 π
	 */
	public static float distanceCW( float angA, float angB ) {
		if( angA >= angB )
			return angA-angB;
		else
			return GrlConstants.F_PI2-(angB-angA);
	}

	/**
	 *

	 * Returns the difference between two angles and bounds the result between -pi and pi:

	 * result = angA - angB

	 * and takes in account boundary conditions.
	 *

	 *
	 * @param angA first angle.  Must be between -pi and pi.
	 * @param angB second angle. Must be between -pi and pi.
	 * @return an angle between -pi and pi
	 */
	public static double minus( double angA, double angB ) {
		double diff = angA - angB;

		if( diff > Math.PI ) {
			return GrlConstants.PI2 - diff;
		} else if( diff < -Math.PI )
			return -GrlConstants.PI2 - diff;

		return diff;
	}

	/**
	 *

	 * Returns the difference between two angles and bounds the result between -pi and pi:

	 * result = angA - angB

	 * and takes in account boundary conditions.
	 *

	 *
	 * @param angA first angle.  Must be between -pi and pi.
	 * @param angB second angle. Must be between -pi and pi.
	 * @return an angle between -pi and pi
	 */
	public static float minus( float angA, float angB ) {
		float diff = angA - angB;

		if( diff > GrlConstants.F_PI ) {
			return GrlConstants.F_PI2 - diff;
		} else if( diff < -GrlConstants.F_PI )
			return -GrlConstants.F_PI2 - diff;

		return diff;
	}

	/**
	 *

	 * Returns the number of radians two angles are apart.  This is equivalent to
	 * Math.abs(UtilAngle.minus(angA,angB)).
	 *

	 *
	 * @param angA first angle.  Must be between -pi and pi.
	 * @param angB second angle. Must be between -pi and pi.
	 * @return an angle between 0 and pi
	 */
	public static double dist( double angA, double angB ) {
		return Math.abs(minus(angA,angB));
	}

	/**
	 *

	 * Returns the number of radians two angles are apart.  This is equivalent to
	 * Math.abs(UtilAngle.minus(angA,angB)).
	 *

	 *
	 * @param angA first angle.  Must be between -pi and pi.
	 * @param angB second angle. Must be between -pi and pi.
	 * @return an angle between 0 and pi
	 */
	public static float dist( float angA, float angB ) {
		return Math.abs(minus(angA,angB));
	}

	/**
	 * Angular distance between two half circle angles.
	 *
	 * @param angA Angle between -pi/2 and pi/2.
	 * @param angB Angle between -pi/2 and pi/2.
	 * @return Acute angle between the two input angles.
	 */
	public static double distHalf( double angA , double angB ) {
		double a = Math.abs(angA-angB);
		if( a <= Math.PI/2 )
			return a;
		else
			return Math.PI-a;
	}

	/**
	 * Converts an angle from -pi to pi into 0 to 2*pi domain
	 * @param angle angle from -pi to pi radians
	 * @return angle from 0 to 2*pi radians
	 */
	public static double domain2PI( double angle ) {
		if( angle < 0 ) {
			return angle + 2*Math.PI;
		} else {
			return angle;
		}
	}

	/**
	 * Ensures a wrapping circular bound so that the numbers from 0 to 1, where 0 is inclusive and 1 is exclusive.
	 *
Examples:
	 * 1.5   = 0.5
	 * -0.25 = 0.75
	 * 0     = 0
	 * 1     = 0
	 * 0.999 = 0.999
	 * 2     = 0
	 * -1    = 0
	 *

	 *
	 * @return A value from 0 to 1. [0,1)
	 */
	public static double wrapZeroToOne(double value ) {
		if( value >= 0 )
			return value % 1.0;
		else {
			return (1.0+(value%1.0))%1.0; // last bit is to ensure that 1.0 is returned as 0.0
		}
	}

	/**
	 * Ensures a wrapping circular bound so that the numbers from 0 to 1, where 0 is inclusive and 1 is exclusive.
	 *
Examples:
	 * 1.5   = 0.5
	 * -0.25 = 0.75
	 * 0     = 0
	 * 1     = 0
	 * 0.999 = 0.999
	 * 2     = 0
	 * -1    = 0
	 *

	 *
	 * @return A value from 0 to 1. [0,1)
	 */
	public static float wrapZeroToOne(float value ) {
		if( value >= 0 )
			return value % 1.0f;
		else {
			return (1.0f+(value%1.0f))%1.0f; // last bit is to ensure that 1.0 is returned as 0.0
		}
	}

	/**
	 * Ensures a reflective bound so that the numbers from 0 to 1, where 0 is inclusive and 1 is inclusive.
	 *
Examples:
	 * 1.5   = 0.5
	 * -0.25 = 0.25
	 * -0.75 = 0.75
	 * 0     = 0
	 * 1     = 1
	 * 0.999 = 0.999
	 * 2     = 0
	 * -1    = 1
	 *

	 *
	 * @return A value from 0 to 1. [0,1]
	 */
	public static double reflectZeroToOne(double value ) {
		if( value < 0 )
			value = -value;
		value = value%2.0;
		if( value > 1.0 )
			return 2.0 - value;
		return value;
	}

	/**
	 * Ensures a reflective bound so that the numbers from 0 to 1, where 0 is inclusive and 1 is inclusive.
	 *
Examples:
	 * 1.5   = 0.5
	 * -0.25 = 0.25
	 * -0.75 = 0.75
	 * 0     = 0
	 * 1     = 1
	 * 0.999 = 0.999
	 * 2     = 0
	 * -1    = 1
	 *

	 *
	 * @return A value from 0 to 1. [0,1]
	 */
	public static float reflectZeroToOne(float value ) {
		if( value < 0 )
			value = -value;
		value = value%2.0f;
		if( value > 1.0f )
			return 2.0f - value;
		return 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.metric;

import georegression.geometry.GeometryMath_F64;
import georegression.misc.GrlConstants;
import georegression.struct.line.LineParametric3D_F64;
import georegression.struct.line.LineSegment3D_F64;
import georegression.struct.plane.PlaneGeneral3D_F64;
import georegression.struct.plane.PlaneNormal3D_F64;
import georegression.struct.point.Point3D_F64;
import georegression.struct.point.Vector3D_F64;
import georegression.struct.shapes.Box3D_F64;
import georegression.struct.shapes.BoxLength3D_F64;
import georegression.struct.shapes.Sphere3D_F64;
import georegression.struct.shapes.Triangle3D_F64;
import org.ddogleg.struct.FastQueue;

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

	/**
	 * Finds the intersection of a line and a plane.  Returns true if they intersect at a unique point or false if
	 * there is no intersection or an infinite number of intersections.
	 *
	 * @param plane (Input) Plane
	 * @param line (Input) Line
	 * @param intersection (Output) Where the intersection is written to
	 * @return True if the intersection is at a unique point.  If false then no intersection or infinite.
	 */
	public static boolean intersect( PlaneNormal3D_F64 plane , LineParametric3D_F64 line , Point3D_F64 intersection ) {
		double dx = plane.p.x - line.p.x;
		double dy = plane.p.y - line.p.y;
		double dz = plane.p.z - line.p.z;

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

		if( bottom == 0 )
			return false;

		double d = top/bottom;

		intersection.x = line.p.x + d*line.slope.x;
		intersection.y = line.p.y + d*line.slope.y;
		intersection.z = line.p.z + d*line.slope.z;

		return true;
	}

	/**
	 * Finds the intersection of a line and a plane.  Returns true if they intersect at a unique point or false if
	 * there is no intersection or an infinite number of intersections.
	 *
	 * @param plane (Input) Plane
	 * @param line (Input) Line
	 * @param intersection (Output) Where the intersection is written to
	 * @return True if the intersection is at a unique point.  If false then no intersection or infinite.
	 */
	public static boolean intersect( PlaneGeneral3D_F64 plane , LineParametric3D_F64 line , Point3D_F64 intersection ) {

		double top = plane.D - plane.A*line.p.x - plane.B*line.p.y - plane.C*line.p.z;
		double bottom = plane.A*line.slope.x + plane.B*line.slope.y + plane.C*line.slope.z;

		if( bottom == 0 )
			return false;

		double d = top/bottom;

		intersection.x = line.p.x + d*line.slope.x;
		intersection.y = line.p.y + d*line.slope.y;
		intersection.z = line.p.z + d*line.slope.z;

		return true;
	}

	/**
	 * Finds the line which is the intersection between the two planes.  For a valid solution to be returned
	 * the planes must not be parallel to each other.  If the planes are parallel then the slope of the returned line
	 * will have a value of zero for each element.
	 *
	 * @param a (Input) Plane
	 * @param b (Input) Plane
	 * @param line (Output) Intersection.
	 * @return true if they intersect ata line or false if not
	 */
	public static boolean intersect( PlaneGeneral3D_F64 a , PlaneGeneral3D_F64 b , LineParametric3D_F64 line ) {

		// Line's slope is the cross product of the two normal vectors
		GeometryMath_F64.cross(a.A,a.B,a.C,b.A,b.B,b.C,line.slope);

		if( line.slope.normSq() == 0 )
			return false;

		// Closest point on plane 'a' to origin (0,0,0)
		double n2 = a.A*a.A + a.B*a.B + a.C*a.C;

		double closestX = a.A*a.D/n2;
		double closestY = a.B*a.D/n2;
		double closestZ = a.C*a.D/n2;

		// Cross product between normal of 'a' and the line's slope.  This points towards the intersection
		double slopeX = a.B * line.slope.z - a.C * line.slope.y;
		double slopeY = a.C * line.slope.x - a.A * line.slope.z;
		double slopeZ = a.A * line.slope.y - a.B * line.slope.x;

		// Now find the intersection of the plane and a line containing point 'closest' and pointing towards the
		// the intersection.
		double top = b.D - b.A*closestX - b.B*closestY - b.C*closestZ;
		double bottom = b.A*slopeX + b.B*slopeY + b.C*slopeZ;

		double d = top/bottom;

		line.p.x = closestX + d*slopeX;
		line.p.y = closestY + d*slopeY;
		line.p.z = closestZ + d*slopeZ;

		return true;
	}

	/**
	 *
Finds the intersection between a 3D triangle and a line-segment.  Code ported from [1].

	 *
	 *

	 * [1] http://geomalgorithms.com/a06-_intersect-2.html
	 *

	 *
	 * @param T (Input) Triangle in 3D space
	 * @param R (Input) Line segment in 3D space.
	 * @param output (Output) Storage for the intersection, if there is one
	 * @return -1 = triangle is degenerate (a segment or point)

	 *          0 =  disjoint (no intersect)

	 *          1 =  intersect in unique point I1

	 *          2 =  are in the same plane
	 **/
	public static int intersect(Triangle3D_F64 T , LineSegment3D_F64 R , Point3D_F64 output ) {
		return intersect(T,R,output,
				new Vector3D_F64(),new Vector3D_F64(),new Vector3D_F64(),new Vector3D_F64(),new Vector3D_F64());
	}

	/**
	 *
Finds the intersection between a 3D triangle and a line-segment.  Code ported from [1].  Internal
	 * working variables are provided in this interface to reduce memory creation/destruction.

	 *
	 *

	 * [1] http://geomalgorithms.com/a06-_intersect-2.html
	 *

	 *
	 * @param T (Input) Triangle in 3D space
	 * @param R (Input) Line segment in 3D space.
	 * @param output (Output) Storage for the intersection, if there is one
	 * @param u   (internal use) triangle vectors
	 * @param v   (internal use) triangle vectors
	 * @param n   (internal use) triangle vectors
	 * @param dir (internal use) ray vector
	 * @param w0  (internal use) ray vector
	 * @return -1 = triangle is degenerate (a segment or point)
	 *          0 =  disjoint (no intersect)
	 *          1 =  intersect in unique point I1
	 *          2 =  are in the same plane
	 **/
	public static int intersect(Triangle3D_F64 T , LineSegment3D_F64 R , Point3D_F64 output ,
								Vector3D_F64 u , Vector3D_F64 v , Vector3D_F64 n,
								Vector3D_F64 dir , Vector3D_F64 w0 ) {
		double r, a, b;              // params to calc ray-plane intersect

		// get triangle edge vectors and plane normal
		u.minus(T.v1,T.v0);   // NOTE: these could be precomputed
		v.minus(T.v2,T.v0);
		n.cross(u,v);

		if ( n.normSq() == 0 )        // triangle is degenerate
			return -1;                  // do not deal with this case

		dir.minus(R.b,R.a);             // ray direction vector
		w0.minus(R.a,T.v0);
		a = -n.dot(w0);
		b = n.dot(dir);
		if (Math.abs(b) < GrlConstants.EPS) { // ray is  parallel to triangle plane
			if (a == 0)                       // ray lies in triangle plane
				return 2;
			else return 0;                    // ray disjoint from plane
		}

		// get intersect point of ray with triangle plane
		r = a / b;
		if (r < 0.0)                    // ray goes away from triangle
			return 0;                   // => no intersect
		else if( r > 1.0 )              // is past the end of the line segment
			return 0;

		// intersect point of ray and plane
		output.x = R.a.x + r*dir.x;
		output.y = R.a.y + r*dir.y;
		output.z = R.a.z + r*dir.z;

		// is I inside T?
		if( containedPlane(T.v0,output,u,v,w0)) {
			return 1;
		} else {
			return 0;
		}
	}

	/**
	 *
Finds the intersection between a 3D triangle and a line.  Code ported from [1].  Internal
	 * working variables are provided in this interface to reduce memory creation/destruction.

	 *
	 *

	 * [1] http://geomalgorithms.com/a06-_intersect-2.html
	 *

	 *
	 * @param T (Input) Triangle in 3D space
	 * @param R (Input) Line segment in 3D space.
	 * @param output (Output) Storage for the intersection, if there is one
	 * @return

	 *
-1 = triangle is degenerate (a segment or point)

	 *
0 =  disjoint (no intersect)

	 *
1 =  intersect in unique point. Positive direction or zero

	 *
2 =  are in the same plane

	 *
3 =  intersect in unique point. Negative direction

	 *

	 **/
	public static int intersect(Triangle3D_F64 T , LineParametric3D_F64 R , Point3D_F64 output) {
		return intersect(T,R,output,
				new Vector3D_F64(),new Vector3D_F64(),new Vector3D_F64(),new Vector3D_F64());
	}

	/**
	 *
Finds the intersection between a 3D triangle and a line.  Code ported from [1].  Internal
	 * working variables are provided in this interface to reduce memory creation/destruction.

	 *
	 *

	 * [1] http://geomalgorithms.com/a06-_intersect-2.html
	 *

	 *
	 * @param T (Input) Triangle in 3D space
	 * @param R (Input) Line segment in 3D space.
	 * @param output (Output) Storage for the intersection, if there is one
	 * @param u   (internal use) triangle vectors
	 * @param v   (internal use) triangle vectors
	 * @param n   (internal use) triangle vectors
	 * @param w0  (internal use) ray vector
	 * @return

	 *
-1 = triangle is degenerate (a segment or point)

	 *
0 =  disjoint (no intersect)

	 *
1 =  intersect in unique point. Positive direction or zero

	 *
2 =  are in the same plane

	 *
3 =  intersect in unique point. Negative direction

	 *

	 **/
	public static int intersect(Triangle3D_F64 T , LineParametric3D_F64 R , Point3D_F64 output ,
								Vector3D_F64 u , Vector3D_F64 v , Vector3D_F64 n,
								Vector3D_F64 w0 ) {
		double r, a, b;              // params to calc ray-plane intersect

		// get triangle edge vectors and plane normal
		u.minus(T.v1,T.v0);   // NOTE: these could be precomputed
		v.minus(T.v2,T.v0);
		n.cross(u,v);

		if ( n.normSq() == 0 )        // triangle is degenerate
			return -1;                  // do not deal with this case

		Vector3D_F64 dir = R.slope;
		w0.minus(R.p,T.v0);
		a = -n.dot(w0);
		b = n.dot(dir);
		if (Math.abs(b) < GrlConstants.EPS) { // ray is  parallel to triangle plane
			if (a == 0)                       // ray lies in triangle plane
				return 2;
			else return 0;                    // ray disjoint from plane
		}

		// get intersect point of ray with triangle plane
		r = a / b;

		// intersect point of ray and plane
		output.x = R.p.x + r*dir.x;
		output.y = R.p.y + r*dir.y;
		output.z = R.p.z + r*dir.z;

		// is I inside T?
		if( containedPlane(T.v0,output,u,v,w0)) {
			if( r >= 0 )
				return 1;
			else
				return 3;
		} else {
			return 0;
		}
	}

	/**
	 * Detects if a 2D convex polygon that has been moved into a 3D world is intersected by the 3D line.
	 *

	 * Transformation:
	 * 1) Each 2D point is converted into 3D: X=(x,y,0)
	 * 2) X' = R*X + T, where (R,T) are a rigid body transform from poly to world frames
	 *

	 *
	 * @param polygon (Input) Convex polygon. All points must lie on a plane.
	 * @param line (Input) Line.
	 * @param output (Output) Point of intersection (if any)
	 * @param n (internal use)
	 * @param u (internal use)
	 * @param v (internal use)
	 * @param w0 (internal use)
	 * @return

	 *
-1 = triangle is degenerate (a segment or point)

	 *
0 =  disjoint (no intersect)

	 *
1 =  intersect in unique point. Positive direction or zero

	 *
2 =  are in the same plane

	 *
3 =  intersect in unique point. Negative direction

	 *

	 */
	public static int intersectConvex(FastQueue<Point3D_F64> polygon,
									  LineParametric3D_F64 line , Point3D_F64 output,
									  Vector3D_F64 n, Vector3D_F64 u, Vector3D_F64 v, Vector3D_F64 w0) {
		if( polygon.size < 3 )
			throw new IllegalArgumentException("There must be 3 or more points");

		double r, a, b;              // params to calc ray-plane intersect

		Point3D_F64 v0 = polygon.get(0);
		Point3D_F64 v1 = polygon.get(1);
		Point3D_F64 v2 = polygon.get(2);

		// get triangle edge vectors and plane normal
		u.minus(v1,v0);   // NOTE: these could be precomputed
		v.minus(v2,v0);
		n.cross(u,v);

		if ( n.normSq() == 0 )        // triangle is degenerate
			return -1;                  // do not deal with this case

		Vector3D_F64 dir = line.slope;
		w0.minus(line.p,v0);
		a = -n.dot(w0);
		b = n.dot(dir);
		if (Math.abs(b) < GrlConstants.EPS) { // ray is  parallel to triangle plane
			if (a == 0)                       // ray lies in triangle plane
				return 2;
			else return 0;                    // ray disjoint from plane
		}

		// get intersect point of ray with triangle plane
		r = a / b;

		// intersect point of ray and plane
		output.x = line.p.x + r*dir.x;
		output.y = line.p.y + r*dir.y;
		output.z = line.p.z + r*dir.z;

		// See if it's inside any of the triangles
		for (int i = 2; i < polygon.size; i++) {
			// is I inside T?
			if (containedPlane(v0, output, u, v, w0)) {
				if (r >= 0)
					return 1;
				else
					return 3;
			}

			if (i < polygon.size - 1) {
				u.minus(polygon.get(i), v0);
				v.minus(polygon.get(i+1), v0);
			}
		}
		return 0;
	}

	/**
	 * @see #intersectConvex(FastQueue, LineParametric3D_F64, Point3D_F64, Vector3D_F64, Vector3D_F64, Vector3D_F64, Vector3D_F64)
	 */
	public static int intersectConvex(FastQueue<Point3D_F64> polygon,
									  LineParametric3D_F64 line , Point3D_F64 output) {
		return intersectConvex(polygon,line,output,new Vector3D_F64(),new Vector3D_F64(),new Vector3D_F64(),
				new Vector3D_F64());
	}

	/**
	 * Determines if the point on the same plane as T is contained inside of T.
	 */
	private static boolean containedPlane( Point3D_F64 T_v0,
										   Point3D_F64 output,
										   Vector3D_F64 u , Vector3D_F64 v ,
										   Vector3D_F64 w0 ) {
		double uu, uv, vv, wu, wv, D;
		uu = u.dot(u);
		uv = u.dot(v);
		vv = v.dot(v);
		w0.minus(output,T_v0);
		wu = w0.dot(u);
		wv = w0.dot(v);
		D = uv * uv - uu * vv;

		// get and test parametric coords
		double s, t;
		s = (uv * wv - vv * wu) / D;
		if (s < 0.0 || s > 1.0)        // I is outside T
			return false;
		t = (uv * wu - uu * wv) / D;
		return !(t < 0.0) && !((s + t) > 1.0);  // I is outside T
	}

	/**
	 * Returns true if the point is contained inside the box. The point is considered to be inside the box
	 * if the following test passes for each dimension.  box.x ≤ point.x {@code <} box.x + box.lengthX
	 *
	 * @param box Box
	 * @param point Point which is tested to see if it is inside the box
	 * @return true for inside and false for not
	 */
	public static boolean contained( BoxLength3D_F64 box , Point3D_F64 point ) {

		return( box.p.x <= point.x && point.x < box.p.x + box.lengthX &&
				box.p.y <= point.y && point.y < box.p.y + box.lengthY &&
				box.p.z <= point.z && point.z < box.p.z + box.lengthZ );
	}

	/**
	 *

	 * Returns true if the point is contained inside the box, with an exclusive upper extent.
	 * The point is considered to be inside the box if the following test passes:

	 * box.p0.x ≤ point.x {@code <} box.p1.x

	 * box.p0.y ≤ point.y {@code <} box.p1.y

	 * box.p0.z ≤ point.z {@code <} box.p1.z

	 *

	 *
	 * @param box Box
	 * @param point Point which is tested to see if it is inside the box
	 * @return true for inside and false for not
	 */
	public static boolean contained( Box3D_F64 box , Point3D_F64 point ) {

		return( box.p0.x <= point.x && point.x < box.p1.x &&
				box.p0.y <= point.y && point.y < box.p1.y &&
				box.p0.z <= point.z && point.z < box.p1.z );
	}

	/**
	 *

	 * Returns true if the point is contained inside the box, with an inclusive upper extent.
	 * The point is considered to be inside the box if the following test passes:

	 * box.p0.x ≤ point.x ≤ box.p1.x

	 * box.p0.y ≤ point.y ≤ box.p1.y

	 * box.p0.z ≤ point.z ≤ box.p1.z

	 *

	 *
	 * @param box Box
	 * @param point Point which is tested to see if it is inside the box
	 * @return true for inside and false for not
	 */
	public static boolean contained2( Box3D_F64 box , Point3D_F64 point ) {
		return( box.p0.x <= point.x && point.x <= box.p1.x &&
				box.p0.y <= point.y && point.y <= box.p1.y &&
				box.p0.z <= point.z && point.z <= box.p1.z );
	}

	/**
	 * Returns true if boxB is contained inside of or is identical to boxA.
	 *
	 * @param boxA Box
	 * @param boxB Box which is being tested to see if it is inside of boxA
	 * @return true if inside/identical or false if outside
	 */
	public static boolean contained( Box3D_F64 boxA , Box3D_F64 boxB ) {
		return( boxA.p0.x <= boxB.p0.x && boxA.p1.x >= boxB.p1.x &&
				boxA.p0.y <= boxB.p0.y && boxA.p1.y >= boxB.p1.y &&
				boxA.p0.z <= boxB.p0.z && boxA.p1.z >= boxB.p1.z );
	}

	/**
	 * Returns true if the two boxs intersect each other. p0 is inclusive and p1 is exclusive.
	 * So if the p0 edge and p1 edge overlap perfectly there is no intersection.
	 *
	 * @param boxA Box
	 * @param boxB Box
	 * @return true for intersection and false if no intersection
	 */
	public static boolean intersect( Box3D_F64 boxA , Box3D_F64 boxB ) {
		return( intersect(boxA.p0.x , boxB.p0.x , boxA.p1.x , boxB.p1.x ) &&
				intersect(boxA.p0.y , boxB.p0.y , boxA.p1.y , boxB.p1.y ) &&
				intersect(boxA.p0.z , boxB.p0.z , boxA.p1.z , boxB.p1.z ) );
	}

	protected static boolean intersect( double a0 , double b0 , double a1, double b1 ) {
		if( a0 <= b0 ) {
			return b0 < a1;
		} else {
			return a0 < b1;
		}
	}

	/**
	 * Finds the intersection of a line and sphere.  There can be 0, 1, or 2 intersections.  If there is
	 * 1 intersection the same point is returned twice.
	 *
	 * @param line line
	 * @param sphere sphere
	 * @param a (Output) Storage for point of intersection.  t = max(t0,t1), where t is location on line
	 * @param b (Output) Storage for point of intersection.  t = max(t0,t1), where t is location on line
	 * @return true if the line intersects the sphere
	 */
	public static boolean intersect(LineParametric3D_F64 line , Sphere3D_F64 sphere ,
									Point3D_F64 a, Point3D_F64 b ) {

		// this equation was found by solving for l:
		// ||(P + V*l) - X0|| == r

		double r2 = sphere.radius*sphere.radius;

		double PP = GeometryMath_F64.dot(line.p,line.p);
		double PV = GeometryMath_F64.dot(line.p,line.slope);
		double PX = GeometryMath_F64.dot(line.p,sphere.center);
		double VV = GeometryMath_F64.dot(line.slope,line.slope);
		double VX = GeometryMath_F64.dot(line.slope,sphere.center);
		double XX = GeometryMath_F64.dot(sphere.center,sphere.center);

		// Coefficients in the quadratic equation
		double A = VV;
		double B = 2.0*(PV-VX);
		double C = PP+XX-2.0*PX-r2;

		// solve for the quadratic equation
		double inner = B*B - 4.0*A*C;
		if( inner < 0 )
			return false;
		double sqrt = Math.sqrt(inner);

		double t0 = (-B + sqrt)/(2.0*A);
		double t1 = (-B - sqrt)/(2.0*A);

		line.setPointOnLine(t0,a);
		line.setPointOnLine(t1,b);

		return true;
	}

}


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

package georegression.transform.se;

import georegression.metric.UtilAngle;
import georegression.struct.se.Se2_F64;

/**
 * Performs linear interpolation between two rigid body transforms
 *
 * @author Peter Abeles
 */
public class InterpolateLinearSe2_F64 {

	/**
	 * Perform linear interpolation
	 *
	 * @param a The first transform
	 * @param b The second transform
	 * @param where from 0 to 1, inclusive.  0 means it will be more similar to 'a'.
	 * @param output The interpolated transform
	 */
	public static void interpolate(Se2_F64 a , Se2_F64 b , double where, Se2_F64 output) {

		double w0 = 1.0-where;

		output.T.x = a.T.x*w0 + b.T.x*where;
		output.T.y = a.T.y*w0 + b.T.y*where;

		// interpolating rotation is more difficult
		// This only works well if the difference between the two angles is small
		double yaw0 = a.getYaw();
		double yaw1 = b.getYaw();

		double cw = UtilAngle.distanceCW(yaw0, yaw1);
		double ccw = UtilAngle.distanceCCW(yaw0,yaw1);

		double yaw;
		if( cw > ccw ) {
			yaw = yaw0+ccw*where;
		} else {
			yaw = yaw0-cw*where;
		}
		output.setYaw(yaw);
	}
}


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

import georegression.fitting.MotionTransformPoint;
import georegression.geometry.ConvertRotation3D_F64;
import georegression.geometry.GeometryMath_F64;
import georegression.geometry.UtilPoint3D_F64;
import georegression.struct.point.Point3D_F64;
import georegression.struct.point.Vector3D_F64;
import georegression.struct.se.Se3_F64;
import georegression.struct.so.Quaternion_F64;
import org.ejml.simple.SimpleEVD;
import org.ejml.simple.SimpleMatrix;

import java.util.List;

/**
 *

 * Finds the rigid body motion which minimizes the different between the two sets of associated points in 3D.  Computes
 * the quaternions directly in closed form.
 *

 *

 * The mean square error function that is minimized is:

 * f(q) = (1/N) sum( i=1:N , ||x_i - R(q_r)*p_i + q_t||2 )

 * where q is the transformation parameterized with q_r  quaternions and q_t translation, x is the set of 'to' points
 * and p is the set of 'from' points.
 *

 *

 * The basic cross-covariance method sketch in 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, is implemented below.
 *

 *
 * @author Peter Abeles
 */
public class MotionSe3PointCrossCovariance_F64 implements MotionTransformPoint<Se3_F64, Point3D_F64> {

	// first 4 elements are quaternions and last 3 are translation
	private double param[] = new double[7];

	// rigid body motion
	private Se3_F64 motion = new Se3_F64();

	// temporarily stores the quaternion
	private Quaternion_F64 quat = new Quaternion_F64();

	public double[] getParam() {
		return param;
	}

	@Override
	public Se3_F64 getTransformSrcToDst() {
		return motion;
	}

	@Override
	public boolean process( List<Point3D_F64> srcPts, List<Point3D_F64> dstPts) {
		if( srcPts.size() != dstPts.size() )
			throw new IllegalArgumentException( "There must be a 1 to 1 correspondence between the two sets of points" );

		// find the mean of both sets of points
		Point3D_F64 meanFrom = UtilPoint3D_F64.mean(srcPts, null );
		Point3D_F64 meanTo = UtilPoint3D_F64.mean(dstPts, null );

		final int N = srcPts.size();

		// compute the cross-covariance matrix Sigma of the two sets of points
		// Sigma = (1/N)*sum(i=1:N,[p*x^T]) + mu_p*mu_x^T
		// for performance reasons usage of matrices is postponed
		double s11 = 0, s12 = 0, s13 = 0;
		double s21 = 0, s22 = 0, s23 = 0;
		double s31 = 0, s32 = 0, s33 = 0;

		// mu*mu^2
		double m11 = meanFrom.x * meanTo.x, m12 = meanFrom.x * meanTo.y, m13 = meanFrom.x * meanTo.z;
		double m21 = meanFrom.y * meanTo.x, m22 = meanFrom.y * meanTo.y, m23 = meanFrom.y * meanTo.z;
		double m31 = meanFrom.z * meanTo.x, m32 = meanFrom.z * meanTo.y, m33 = meanFrom.z * meanTo.z;

		for( int i = 0; i < N; i++ ) {
			Point3D_F64 f = srcPts.get( i );
			Point3D_F64 t = dstPts.get( i );

			s11 += f.x * t.x;
			s12 += f.x * t.y;
			s13 += f.x * t.z;
			s21 += f.y * t.x;
			s22 += f.y * t.y;
			s23 += f.y * t.z;
			s31 += f.z * t.x;
			s32 += f.z * t.y;
			s33 += f.z * t.z;
		}

		s11 = s11 / N - m11;
		s12 = s12 / N - m12;
		s13 = s13 / N - m13;
		s21 = s21 / N - m21;
		s22 = s22 / N - m22;
		s23 = s23 / N - m23;
		s31 = s31 / N - m31;
		s32 = s32 / N - m32;
		s33 = s33 / N - m33;

		SimpleMatrix Sigma = new SimpleMatrix( 3, 3, true, new double[]{s11, s12, s13, s21, s22, s23, s31, s32, s33} );

//        Sigma.print();

		SimpleMatrix Delta = new SimpleMatrix( 3, 1, true, new double[]{s23 - s32, s31 - s13, s12 - s21} );

		SimpleMatrix Q = new SimpleMatrix( 4, 4 );
		SimpleMatrix BR = Sigma.plus( Sigma.transpose() ).minus( SimpleMatrix.identity( 3 ).scale( Sigma.trace() ) );

		Q.set( 0, 0, Sigma.trace() );
		Q.insertIntoThis( 0, 1, Delta.transpose() );
		Q.insertIntoThis( 1, 0, Delta );
		Q.insertIntoThis( 1, 1, BR );

//        Q.print();

		extractQuaternionFromQ( Q );

		// translation = meanTo - R*meanFrom
		GeometryMath_F64.mult( motion.getR(), meanFrom, meanFrom );
		Vector3D_F64 T = motion.getT();
		param[4] = T.x = meanTo.x - meanFrom.x;
		param[5] = T.y = meanTo.y - meanFrom.y;
		param[6] = T.z = meanTo.z - meanFrom.z;

		return true;
	}

	/**
	 * The unit eigenvector corresponding to the maximum eigenvalue of Q is the rotation
	 * parameterized as a quaternion.
	 */
	private void extractQuaternionFromQ( SimpleMatrix q ) {
		SimpleEVD<SimpleMatrix> evd = q.eig();
		int indexMax = evd.getIndexMax();

		SimpleMatrix v_max = evd.getEigenVector( indexMax );

		quat.w = (double) v_max.get( 0 );
		quat.x = (double) v_max.get( 1 );
		quat.y = (double) v_max.get( 2 );
		quat.z = (double) v_max.get( 3 );
		quat.normalize();

		ConvertRotation3D_F64.quaternionToMatrix( quat, motion.getR() );
	}

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


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

import georegression.struct.point.Point2D_F64;
import georegression.struct.shapes.Polygon2D_F64;
import org.ddogleg.sorting.QuickSortComparator;
import org.ddogleg.struct.FastQueue;

import java.util.Comparator;

/**
 * Computes the convex hull of a set of points using Andrew's monotone chain algorithm.  O(n log n) for sort and
 * O(N) for convex hull computation, where N is number of input points.
 *
 * @author Peter Abeles
 */
public class AndrewMonotoneConvexHull_F64 {
	// Use this sorting routine to avoid declaring memory each time its called
	QuickSortComparator<Point2D_F64> sorter;

	FastQueue<Point2D_F64> work = new FastQueue<Point2D_F64>(Point2D_F64.class,false);

	public AndrewMonotoneConvexHull_F64() {

		// Sort the points based on their x value.  If the same then use y value
		sorter = new QuickSortComparator<>(new Comparator<Point2D_F64>() {
			@Override
			public int compare(Point2D_F64 a, Point2D_F64 b) {
				if( a.x < b.x )
					return -1;
				else if( a.x > b.x )
					return 1;
				else if( a.y < b.y )
					return -1;
				else if( a.y > b.y )
					return 1;
				return 0;
			}
		});
	}

	/**
	 * Computes the convex hull.  The output will be in counter-clockwise order.
	 *
	 * @param input List of input points.  The list will be modified by sorting
	 * @param length Number of valid elements in list
	 * @param hull (Output) Where the complex hull is written to
	 */
	public void process( Point2D_F64[] input , int length , Polygon2D_F64 hull )
	{
		// ahdnle special cases
		if( length == 2 ) {
			hull.vertexes.resize(length);
			for (int i = 0; i < length; i++) {
				hull.get(i).set(input[i]);
			}
			return;
		}

		sorter.sort(input,length);
		work.reset();

		// construct the lower hull
		for (int i = 0; i < length; i++) {
			Point2D_F64 p = input[i];
			//Contains at least 2 points and the last two points and 'p' do not make a counter-clockwise turn
			while( work.size() >= 2 && subtractThenCross(p,work.getTail(0),work.getTail(1)) >= 0) {
				// remove the last points from the hull
				work.removeTail();
			}
			// append p to the end
			work.add(p);
		}

		work.removeTail();
		int minSize = work.size+2;

		// construct upper hull
		for(int i = length-1 ; i >= 0 ; i --)                // Finding top layer from hull
		{
			//Contains at least 2 points and the last two points and 'p' do not make a counter-clockwise turn
			Point2D_F64 p = input[i];
			while( work.size() >= minSize && subtractThenCross(p,work.getTail(0),work.getTail(1)) >= 0 ) {
				work.removeTail();
			}
			// append p to the end
			work.add(p);
		}
		work.removeTail();

		// create a copy for the output
		// the work buffer contains references to the input points, but to be safe the output should have its
		// own instances
		hull.vertexes.resize(work.size);
		for (int i = 0; i < work.size(); i++) {
			hull.vertexes.data[i].set(work.get(i));
		}
	}

	/**
	 * Performs the following operation: output = z-component[ (a-b) cross (a-c) ]
	 */
	private static double subtractThenCross( Point2D_F64 a , Point2D_F64 b , Point2D_F64 c ) {
		double x0 = b.x - a.x;
		double y0 = b.y - a.y;

		double x1 = c.x - a.x;
		double y1 = c.y - a.y;

		return x0 * y1 - y0 * x1;
	}

}


/*
 * 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.point.Vector2D_F64;
import org.ejml.data.Complex_F64;
import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
import org.ejml.interfaces.decomposition.EigenDecomposition_F64;

/**
 * Computes a containment ellipse given a covariance
 *
 * @author Peter Abeles
 */
public class CovarianceToEllipse_F64 {

	EigenDecomposition_F64<DMatrixRMaj> eigen = DecompositionFactory_DDRM.eig(2, true);
	DMatrixRMaj Q = new DMatrixRMaj(2,2);

	// major axis
	Vector2D_F64 x = new Vector2D_F64();
	// minor axis
	Vector2D_F64 y = new Vector2D_F64();

	// major and minor axis length (in that order)
	double lengthX,lengthY;

	// number of standard deviations the ellipse should be
	double numStdev = 1;

	public void setNumStdev(double numStdev) {
		this.numStdev = numStdev;
	}

	/**
	 * Specifies the covariance matrix. Q = [a11 a12; a12 a22]
	 * @param a11 element in covariance matrix.
	 * @param a12 element in covariance matrix.
	 * @param a22  element in covariance matrix.
	 * @return true if it was successful or false if something went wrong
	 */
	public boolean setCovariance( double a11 , double a12, double a22 ) {
		Q.data[0] = a11;
		Q.data[1] = a12;
		Q.data[2] = a12;
		Q.data[3] = a22;

		if( !eigen.decompose(Q) ) {
			System.err.println("Eigenvalue decomposition failed!");
			return false;
		}

		Complex_F64 v0 = eigen.getEigenvalue(0);
		Complex_F64 v1 = eigen.getEigenvalue(1);

		DMatrixRMaj a0,a1;

		if( v0.getMagnitude2() > v1.getMagnitude2() ) {
			a0 = eigen.getEigenVector(0);
			a1 = eigen.getEigenVector(1);
			lengthX = (double) v0.getMagnitude();
			lengthY = (double) v1.getMagnitude();
		} else {
			a0 = eigen.getEigenVector(1);
			a1 = eigen.getEigenVector(0);
			lengthX = (double) v1.getMagnitude();
			lengthY = (double) v0.getMagnitude();
		}

		if( a0 == null || a1 == null ) {
			System.err.println("Complex eigenvalues: "+v0+"  "+v1);
			return false;
		}

		lengthX = Math.sqrt(lengthX);
		lengthY = Math.sqrt(lengthY);

		x.set( (double) a0.get(0) , (double) a0.get(1));
		y.set( (double) a1.get(0) , (double) a1.get(1));

		return true;
	}

	/**
	 * @return Vector which defines the major axis
	 */
	public Vector2D_F64 getMajorVector() {
		return x;
	}

	/**
	 * @return Vector which defines the minor axis
	 */
	public Vector2D_F64 getMinorVector() {
		return y;
	}

	/**
	 * @return Angle between the major axis and the x-axis
	 */
	public double getAngle() {
		return Math.atan2( x.y, x.x );
	}

	/**
	 * @return Length of the major axis
	 */
	public double getMajorAxis() {
		return numStdev*lengthX;
	}

	/**
	 * @return Length of the minor axis
	 */
	public double getMinorAxis() {
		return numStdev*lengthY;
	}
}


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

import georegression.geometry.ConvertRotation3D_F64;
import georegression.geometry.GeometryMath_F64;
import georegression.struct.EulerType;
import georegression.struct.affine.Affine2D_F64;
import georegression.struct.point.Vector3D_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_F64;

/**
 * Various operations related to {@link SpecialEuclidean} transformations.
 *
 * @author Peter Abeles
 */
public class SpecialEuclideanOps_F64 {

	/**
	 * Sets the provided transform so that it does not transform any points.
	 *
	 * @param se The transform which is to be set to no motion.
	 */
	public static void setToNoMotion( Se3_F64 se ) {
		CommonOps_DDRM.setIdentity( se.getR() );
		se.getT().set( 0, 0, 0 );
	}

	/**
	 * Converts {@link Se2_F64} into {@link Affine2D_F64}.
	 * @param se (Input) Se2
	 * @param affine (Output) Equivalent affine.  If null a new object will be declared.
	 * @return Equivalent affine.
	 */
	public static Affine2D_F64 toAffine( Se2_F64 se , Affine2D_F64 affine ) {
		if( affine == null )
			affine = new Affine2D_F64();

		affine.a11 = se.c;
		affine.a12 = -se.s;
		affine.a21 = se.s;
		affine.a22 = se.c;

		affine.tx = se.T.x;
		affine.ty = se.T.y;

		return affine;
	}

	/**
	 * Converts it into a 4 by 4 homogeneous matrix.
	 *
	 * @param se  original 3D transform
	 * @param ret Where the results will be written to.  If null a new matrix is declared. Modified.
	 * @return equivalent homogeneous transform.
	 */
	public static DMatrixRMaj toHomogeneous( Se3_F64 se, DMatrixRMaj ret ) {
		if( ret == null )
			ret = new DMatrixRMaj( 4, 4 );
		else {
			ret.set( 3, 0, 0 );
			ret.set( 3, 1, 0 );
			ret.set( 3, 2, 0 );
		}

		CommonOps_DDRM.insert( se.getR(), ret, 0, 0 );
		Vector3D_F64 T = se.getT();

		ret.set( 0, 3, T.x );
		ret.set( 1, 3, T.y );
		ret.set( 2, 3, T.z );
		ret.set( 3, 3, 1 );

		return ret;
	}

	/**
	 * Converts a homogeneous representation into {@link Se3_F64}.
	 *
	 * @param H   Homogeneous 4 by 4 matrix.
	 * @param ret If not null where the results are written to.
	 * @return Se3_F64 transform.
	 */
	public static Se3_F64 toSe3(DMatrixRMaj H, Se3_F64 ret ) {
		if( H.numCols != 4 || H.numRows != 4 )
			throw new IllegalArgumentException( "The homogeneous matrix must be 4 by 4 by definition." );

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

		ret.setTranslation( (double) H.get( 0, 3 ), (double) H.get( 1, 3 ), (double) H.get( 2, 3 ) );

		CommonOps_DDRM.extract( H, 0, 3, 0, 3, ret.getR(), 0, 0 );

		return ret;
	}

	/**
	 * Converts it into a 3 by 3 homogeneous matrix.
	 *
	 * @param se  original 2D transform
	 * @param ret Where the results will be written to.  If null a new matrix is declared. Modified.
	 * @return equivalent homogeneous transform.
	 */
	public static DMatrixRMaj toHomogeneous( Se2_F64 se, DMatrixRMaj ret ) {
		if( ret == null )
			ret = new DMatrixRMaj( 3, 3 );
		else {
			ret.set( 2, 0, 0 );
			ret.set( 2, 1, 0 );
		}

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

		ret.set( 0, 0, c );
		ret.set( 0, 1, -s );
		ret.set( 1, 0, s );
		ret.set( 1, 1, c );
		ret.set( 0, 2, se.getX() );
		ret.set( 1, 2, se.getY() );
		ret.set( 2, 2, 1 );

		return ret;
	}

	/**
	 * Converts a homogeneous representation into {@link Se2_F64}.
	 *
	 * @param H   Homogeneous 3 by 3 matrix.
	 * @param ret If not null where the results are written to.
	 * @return Se3_F64 transform.
	 */
	public static Se2_F64 toSe2( DMatrixRMaj H, Se2_F64 ret ) {
		if( H.numCols != 3 || H.numRows != 3 )
			throw new IllegalArgumentException( "The homogeneous matrix must be 3 by 3 by definition." );

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

		ret.setTranslation( (double) H.get( 0, 2 ), (double) H.get( 1, 2 ) );

		double c = (double) H.get( 0, 0 );
		double s = (double) H.get( 1, 0 );

		ret.setYaw( Math.atan2( s, c ) );

		return ret;
	}

	/**
	 * Sets the value of an {@link Se3_F64} using Euler XYZ coordinates for the rotation and
	 * a translation vector.
	 *
	 * @param dx   Translation along x-axis.
	 * @param dy   Translation along y-axis.
	 * @param dz   Translation along z-axis.
	 * @param rotX Rotation around X axis.
	 * @param rotY Rotation around Y axis.
	 * @param rotZ Rotation around Z axis.
	 * @param se   If not null then the transform is written here.
	 * @return The transform.
	 */
	public static Se3_F64 eulerXyz(double dx, double dy, double dz, double rotX, double rotY, double rotZ,
								   Se3_F64 se) {
		return eulerXyz(dx, dy, dz, EulerType.XYZ,rotX, rotY, rotZ, se);
	}

	public static Se3_F64 eulerXyz(double dx, double dy, double dz,
								   EulerType type, double rotX, double rotY, double rotZ,
								   Se3_F64 se) {
		if( se == null )
			se = new Se3_F64();

		ConvertRotation3D_F64.eulerToMatrix(type, rotX, rotY, rotZ, se.getR() );
		Vector3D_F64 T = se.getT();
		T.x = dx;
		T.y = dy;
		T.z = dz;

		return se;
	}

	/**
	 * Create SE3 using axis-angle for rotation and XYZ tanslation
	 *
	 * @param dx   Translation along x-axis.
	 * @param dy   Translation along y-axis.
	 * @param dz   Translation along z-axis.
	 * @param rotX x-axis component
	 * @param rotY y-axis component
	 * @param rotZ z-axis component
	 * @param se   If not null then the transform is written here.
	 * @return The transform.
	 */
	public static Se3_F64 axisXyz(double dx, double dy, double dz, double rotX, double rotY, double rotZ,
								  Se3_F64 se) {
		if( se == null )
			se = new Se3_F64();

		double theta = Math.sqrt(rotX*rotX + rotY+rotY + rotZ*rotZ);
		if( theta == 0 ) {
			CommonOps_DDRM.setIdentity(se.R);
		} else {
			ConvertRotation3D_F64.rodriguesToMatrix( rotX/theta, rotY/theta, rotZ/theta,theta, se.getR() );
		}

		Vector3D_F64 T = se.getT();
		T.x = dx;
		T.y = dy;
		T.z = dz;
		return se;
	}

	public static Se3_F64 quatXyz(double dx, double dy, double dz,
								  double qw, double qx, double qy, double qz,
								  Se3_F64 se) {
		if( se == null )
			se = new Se3_F64();

		ConvertRotation3D_F64.quaternionToMatrix(qw,qx,qy,qz, se.getR() );

		Vector3D_F64 T = se.getT();
		T.x = dx;
		T.y = dy;
		T.z = dz;
		return se;
	}

	/**
	 * Can be used to see if two transforms are identical to within tolerance
	 *
	 * @param a transform
	 * @param b tranform
	 * @param tolT Tolerance for translation
	 * @param tolR Tolerance for rotation in radians
	 * @return true if identical or false if not
	 */
	public static boolean isIdentical( Se3_F64 a , Se3_F64 b , double tolT , double tolR ) {
		if( Math.abs(a.T.x-b.T.x) > tolT )
			return false;
		if( Math.abs(a.T.y-b.T.y) > tolT )
			return false;
		if( Math.abs(a.T.z-b.T.z) > tolT )
			return false;

		DMatrixRMaj D = new DMatrixRMaj(3,3);
		CommonOps_DDRM.multTransA(a.R,b.R,D);

		Rodrigues_F64 rod = new Rodrigues_F64();
		ConvertRotation3D_F64.matrixToRodrigues(D,rod);

		return rod.theta <= tolR;
	}

	/**
	 * Finds the best fit projection of 'a' onto SE(3).  This is useful when a was estimated using a linear algorithm.
	 *
	 *
See page 280 of "An Invitation to 3-D Vision, From Images to Geometric Models" 1st Ed. 2004. Springer.

	 * @param a Approximate SE(3). Modified.
	 * @return true if successful
	 */
	public static boolean bestFit( Se3_F64 a ) {
		SingularValueDecomposition_F64<DMatrixRMaj> svd = DecompositionFactory_DDRM.svd(true,true,true);

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

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

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

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

		// compute the determinant of the singular matrix
		double b = 1.0;
		double s[] = svd.getSingularValues();

		for( int i = 0; i < svd.numberOfSingularValues(); i++ ) {
			b *= s[i];
		}

		b = Math.signum(det) / Math.pow(b,1.0/3.0);

		GeometryMath_F64.scale(a.T,b);

		return true;
	}
}


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

package georegression.metric;

import georegression.geometry.UtilPoint2D_F64;
import georegression.geometry.UtilPoint2D_I32;
import georegression.struct.line.LineParametric2D_I32;
import georegression.struct.line.LineSegment2D_I32;
import georegression.struct.point.Point2D_I32;

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

	 * Returns the distance the closest point on a line segment is from the specified point.
	 * The closest point is bounded to be along the line segment.
	 *

	 *
	 * @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( LineSegment2D_I32 line,
								   Point2D_I32 p ) {
		int a = line.b.x - line.a.x;
		int 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_I32.distance(line.a.x, line.a.y, p.x, p.y);
		} else if( t > 1.0 )
			return UtilPoint2D_I32.distance( line.b.x, line.b.y, p.x, p.y );

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

	/**
	 *

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

	 *
	 * @param line A line. 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_I32 line, Point2D_I32 p ) {
		int a = line.slopeX;
		int b = line.slopeY;

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

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

	/**
	 *

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

	 *
	 * @param line A line. Not modified.
	 * @param p The point. Not modified.
	 * @return Distance the closest point on the line is away from the point.
	 */
	public static double distanceSq( LineParametric2D_I32 line, Point2D_I32 p ) {
		int a = line.slopeX;
		int b = line.slopeY;

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

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


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

/**
 * Contains code for converting between different 3D coordinate systems
 *
 * @author Peter Abeles
 */
public class ConvertCoordinates3D_F64 {

	/**
	 * Converts latitude and longitude coordinates into a unit vector
	 *
	 * @param lat Radian value from pi to -pi
	 * @param lon Radian value from -pi/2 to pi/2
	 * @param vector (output) Storage for unit vector.  If null is passed in a new instance will be created.
	 * @return Unit vector
	 */
	public static <T extends GeoTuple3D_F64<T>> T latlonToUnitVector(double lat , double lon , T vector ) {
		if( vector == null )
			vector = (T)new Vector3D_F64();

		vector.x = Math.cos(lat) * Math.cos(lon);
		vector.y = Math.cos(lat) * Math.sin(lon);
		vector.z = -Math.sin(lat);

		return vector;
	}
}


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

/**
 * Functions related to circles.
 *
 * @author Peter Abeles
 */
public class UtilCircle2D_F64 {

	/**
	 * Computes (x-x_c)**2 + (y-y_c)**2 - r. If (x,y) lies on the circle then it should be 0.
	 * @param x x-coordinate of a point
	 * @param y y-coordinate of a point
	 * @param circle circle
	 * @return Result of the equation
	 */
	public static double evaluate( double x , double y, Circle2D_F64 circle ) {
		x -= circle.center.x;
		y -= circle.center.y;

		return x*x + y*y - circle.radius*circle.radius;

	}

	/**
	 * Given three points find the circle that intersects all three. If false is returned that means the points all
	 * lie along a line and there is no circle.
	 * @param x0 Point
	 * @param x1 Point
	 * @param x2 Point
	 * @param circle (Output) found circle
	 * @return true if a circle was found or false if not
	 */
	public static boolean circle(Point2D_F64 x0 , Point2D_F64 x1 , Point2D_F64 x2 , Circle2D_F64 circle ) {

		// points that lie on line a and b
		double xa = (x0.x+x1.x)/2.0;
		double ya = (x0.y+x1.y)/2.0;
		double xb = (x1.x+x2.x)/2.0;
		double yb = (x1.y+x2.y)/2.0;

		// slopes of lines a and b
		double m2 = x0.x-x1.x;
		double m1 = x1.y-x0.y;

		double n2 = x2.x-x1.x;
		double n1 = x1.y-x2.y;

		// find the intersection of the lines
		double bottom = m2*n1-n2*m1;
		if( bottom == 0 )
			return false;

		double alpha = (-m2*(xb-xa) + m1*(yb-ya))/bottom;

		circle.center.x = xb + n1*alpha;
		circle.center.y = yb + n2*alpha;
		circle.radius = circle.center.distance(x0);

		return true;
	}

	/**
	 * Radius squares of the circle that passes through these three points.
	 * @param x0 Point
	 * @param x1 Point
	 * @param x2 Point
	 * @return Radius squares of circle or NaN if colinear
	 */
	public static double circleRadiusSq(Point2D_F64 x0 , Point2D_F64 x1 , Point2D_F64 x2) {
		// points that lie on line a and b
		double xa = (x0.x+x1.x)/2.0;
		double ya = (x0.y+x1.y)/2.0;
		double xb = (x1.x+x2.x)/2.0;
		double yb = (x1.y+x2.y)/2.0;

		// slopes of lines a and b
		double m2 = x0.x-x1.x;
		double m1 = x1.y-x0.y;

		double n2 = x2.x-x1.x;
		double n1 = x1.y-x2.y;

		// find the intersection of the lines
		double bottom = m2*n1-n2*m1;
		if( bottom == 0 )
			return Double.NaN;

		double alpha = (-m2*(xb-xa) + m1*(yb-ya))/bottom;

		double dx = xb + n1*alpha - x0.x;
		double dy = yb + n2*alpha - x0.y;
		return dx*dx + dy*dy;
	}
}