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

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

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

		return dst;
	}

public static DMatrixRMaj convert ( ConicGeneral_F64 src , DMatrixRMaj dst ) { if ( dst == null ) dst = new DMatrixRMaj ( 3 , 3 ) ; else dst . reshape ( 3 , 3 ) ; double B = src . B / 2.0 ; double D = src . D / 2.0 ; double E = src . E / 2.0 ; dst . data [ 0 ] = src . A ; dst . data [ 1 ] = B ; dst . data [ 2 ] = D ; dst . data [ 3 ] = B ; dst . data [ 4 ] = src . C ; dst . data [ 5 ] = E ; dst . data [ 6 ] = D ; dst . data [ 7 ] = E ; dst . data [ 8 ] = src . F ; return dst ; }

Converts symmetric 3x3 matrix back into a conic.

<pre>
[A    B/2  D/2]
dst = [B/2  C    E/2]
[D/2  E/2  F  ]
</pre>

Converts symmetric 3x3 matrix back into a conic .


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

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

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

		return dst;
	}

public static DMatrix3x3 convert ( ConicGeneral_F64 src , DMatrix3x3 dst ) { if ( dst == null ) dst = new DMatrix3x3 ( ) ; double B = src . B / 2.0 ; double D = src . D / 2.0 ; double E = src . E / 2.0 ; dst . a11 = src . A ; dst . a12 = B ; dst . a13 = D ; dst . a21 = B ; dst . a22 = src . C ; dst . a23 = E ; dst . a31 = D ; dst . a32 = E ; dst . a33 = src . F ; return dst ; }

Converts the conic into a symmetric 3x3 matrix

<pre>
[A    B/2  D/2]
dst = [B/2  C    E/2]
[D/2  E/2  F  ]
</pre>


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

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

		return dst;
	}

Converts the conic into a parabola. If the conic isn't a parabola then it is converted into one
by adjusting the value of B.

@param src (Input) Conic
@param dst (Output) Optional storage for converted parabola
@return Parabola

Converts the conic into a parabola . If the conic isn t a parabola then it is converted into one by adjusting the value of B .


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

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

		return dst;
	}

public static ConicGeneral_F64 convert ( ParabolaGeneral_F64 src , ConicGeneral_F64 dst ) { if ( dst == null ) dst = new ConicGeneral_F64 ( ) ; dst . A = src . A * src . A ; dst . B = src . A * src . C * 2.0 ; dst . C = src . C * src . C ; dst . D = src . D ; dst . E = src . E ; dst . F = src . F ; return dst ; }

Converts the parabola into a conic.

@param src (Input) Parabola
@param dst (Output) Optional storage for converted conic
@return Conic


public static void divide( GeoTuple3D_F64 p , double v ) {
		p.x /= v;
		p.y /= v;
		p.z /= v;
	}

public static void divide ( 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 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;
	}

public
static
DMatrixRMaj
toMatrix
(
GeoTuple3D_F64
in
,
DMatrixRMaj
out
)
{
if
(
out
==
)
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 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 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);
	}

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

Converts a DMatrixRMaj into  GeoTuple3D_F64

@param in Input matrix
@param out Output 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;
		}
	}

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

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

Assign the Rodrigues coordinates using a 3 element vector . Theta is the vector s magnitude and the axis of rotation is the unit vector .


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

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

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.

Angular distance between two half circle angles .


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

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 .


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
	}

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
}

Determines if the point on the same plane as T is contained inside of T.

Determines if the point on the same plane as T is contained inside of T .


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

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

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 .


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

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


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

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

The unit eigenvector corresponding to the maximum eigenvalue of Q is the rotation
parameterized as a quaternion.

The unit eigenvector corresponding to the maximum eigenvalue of Q is the rotation parameterized as a quaternion .


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

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

Computes the convex hull . The output will be in counter - clockwise order .


public static void setToNoMotion( Se3_F64 se ) {
		CommonOps_DDRM.setIdentity( se.getR() );
		se.getT().set( 0, 0, 0 );
	}

public static void setToNoMotion ( Se3_F64 se ) { CommonOps_DDRM . setIdentity ( se . getR ( ) ) ; se . getT ( ) . set ( 0 , 0 , 0 ) ; }

Sets the provided transform so that it does not transform any points.

@param se The transform which is to be set to no motion.

Sets the provided transform so that it does not transform any points .


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

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

Converts it into a 4 by 4 homogeneous matrix .


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

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

Converts it into a 3 by 3 homogeneous matrix .


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

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.

Create SE3 using axis - angle for rotation and XYZ tanslation


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

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

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

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

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

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

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

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 .


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

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

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

Radius squares of the circle that passes through these three points .


public static Point3D_F64 closestPoint(LineParametric3D_F64 l0,
										   LineParametric3D_F64 l1,
										   Point3D_F64 ret) {
		if( ret == null ) {
			ret = new Point3D_F64();
		}

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

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

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

		t0 /= bottom;

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

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

		return ret;
	}

Returns the point which minimizes the distance between the two lines in 3D.  If the
two lines are parallel the result is undefined.

@param l0  first line. Not modified.
@param l1  second line. Not modified.
@param ret (Optional) Storage for the closest point. If null a new point is declared. Modified.
@return Closest point between two lines.

Returns the point which minimizes the distance between the two lines in 3D . If the two lines are parallel the result is undefined .


public static Point3D_F64 closestPoint(LineParametric3D_F64 line, Point3D_F64 pt, Point3D_F64 ret)
	{
		if( ret == null ) {
			ret = new Point3D_F64();
		}

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

		double n2 = line.slope.normSq();

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

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

		return ret;
	}

public static Point3D_F64 closestPoint ( LineParametric3D_F64 line , Point3D_F64 pt , Point3D_F64 ret ) { if ( ret == null ) { ret = new Point3D_F64 ( ) ; } double dx = pt . x - line . p . x ; double dy = pt . y - line . p . y ; double dz = pt . z - line . p . z ; double n2 = line . slope . normSq ( ) ; double d = ( line . slope . x * dx + line . slope . y * dy + line . slope . z * dz ) ; ret . x = line . p . x + d * line . slope . x / n2 ; ret . y = line . p . y + d * line . slope . y / n2 ; ret . z = line . p . z + d * line . slope . z / n2 ; return ret ; }

Finds the closest point on a line to the specified point.

@param line Line on which the closest point is being found.  Not modified.
@param pt   The point whose closest point is being looked for.  Not modified.
@param ret  Storage for the solution.  Can be same as instance as 'pt'. If null is passed in a new point is created. Modified.

Finds the closest point on a line to the specified point .


public static Point3D_F64 closestPointOrigin( PlaneGeneral3D_F64 plane , Point3D_F64 found ) {
		if( found == null )
			found = new Point3D_F64();

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

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

		return found;
	}

public static Point3D_F64 closestPointOrigin ( PlaneGeneral3D_F64 plane , Point3D_F64 found ) { if ( found == null ) found = new Point3D_F64 ( ) ; double n2 = plane . A * plane . A + plane . B * plane . B + plane . C * plane . C ; found . x = plane . A * plane . D / n2 ; found . y = plane . B * plane . D / n2 ; found . z = plane . C * plane . D / n2 ; return found ; }

Finds the closest point on the plane to the origin.

@param plane The plane
@param found (Optional) Storage for the closest point.  Can be same as instance as 'pt'. If null a new point is declared internally.
@return The closest point

Finds the closest point on the plane to the origin .


public static Point3D_F64 closestPoint(LineSegment3D_F64 line, Point3D_F64 pt, Point3D_F64 ret) {
		if( ret == null ) {
			ret = new Point3D_F64();
		}

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

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

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

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

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

		return ret;
	}

Finds the closest point on a line segment to the specified point.

@param line Line on which the closest point is being found.  Not modified.
@param pt   The point whose closest point is being looked for.  Not modified.
@param ret  (Optional) Storage for the solution.  Can be same as instance as 'pt'. If null is passed in a new point is created. Modified.
@return The closest point

Finds the closest point on a line segment to the specified point .


public static Point3D_F64 closestPoint( Point3D_F64 vertexA, Point3D_F64 vertexB, Point3D_F64 vertexC,
											Point3D_F64 point , Point3D_F64 ret) {

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

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

		alg.closestPoint(point,ret);

		return ret;
	}

public static Point3D_F64 closestPoint ( Point3D_F64 vertexA , Point3D_F64 vertexB , Point3D_F64 vertexC , Point3D_F64 point , Point3D_F64 ret ) { if ( ret == null ) { ret = new Point3D_F64 ( ) ; } DistancePointTriangle3D_F64 alg = new DistancePointTriangle3D_F64 ( ) ; alg . setTriangle ( vertexA , vertexB , vertexC ) ; alg . closestPoint ( point , ret ) ; return ret ; }

Closest point from a 3D triangle to a point.

@see DistancePointTriangle3D_F64

@param vertexA Vertex in a 3D triangle.
@param vertexB Vertex in a 3D triangle.
@param vertexC Vertex in a 3D triangle.
@param point Point for which the closest point on the triangle is found
@param ret (Optional) Storage for the solution.  If null is passed in a new point is created. Modified.
@return The closest point

Closest point from a 3D triangle to a point .


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

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

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

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

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

		return result;
	}

Applies a 2D special euclidean transform to the point and stores the results in another
variable.

@param se	 The transform.
@param orig   Original point being transformed. Not modified.
@param result Where the results are stored.  Can be the same as orig. If null a new
instance is created. Modified.
@return Transformed point.

Applies a 2D special euclidean transform to the point and stores the results in another variable .


public static void transform( Se2_F64 se, Point2D_F64 points[], int length ) {

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

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

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

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

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

public static void transform ( Se2_F64 se , Point2D_F64 points [ ] , int length ) { double tranX = se . getX ( ) ; double tranY = se . getY ( ) ; final double c = se . getCosineYaw ( ) ; final double s = se . getSineYaw ( ) ; for ( int i = 0 ; i < length ; i ++ ) { Point2D_F64 pt = points [ i ] ; double x = pt . x ; double y = pt . y ; pt . x = tranX + x * c - y * s ; pt . y = tranY + x * s + y * c ; } }

Applies a 2D special euclidean transform to an array of points.

@param se	 The transform.
@param points Array of points which are to be transformed.  Modified.
@param length The number of elements in the array that are to be processed.

Applies a 2D special euclidean transform to an array of points .


public boolean isEquals( Quadrilateral_F64 quad , double tol ) {
		tol *= tol;

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

public boolean isEquals ( Quadrilateral_F64 quad , double tol ) { tol *= tol ; if ( a . distance2 ( quad . a ) > tol ) return false ; if ( b . distance2 ( quad . b ) > tol ) return false ; if ( c . distance2 ( quad . c ) > tol ) return false ; return d . distance2 ( quad . d ) <= tol ; }

Returns true if the two quadrilaterals are equal to each other to within tolerance.  Equality is defined
by seeing if the distance between two equivalent vertexes is within tolerance.

@param quad The second quadrilateral
@param tol Maximum allowed distance between vertexes.
@return true if equals or false if not

Returns true if the two quadrilaterals are equal to each other to within tolerance . Equality is defined by seeing if the distance between two equivalent vertexes is within tolerance .


public static double acuteAngle( LineGeneral2D_F64 a , LineGeneral2D_F64 b ) {
		double la = Math.sqrt(a.A*a.A + a.B*a.B);
		double lb = Math.sqrt(b.A*b.A + b.B*b.B);

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

Returns the acute angle between the slope of two lines.  Lines do not need to ever
intersect.  Found using the dot product.

@param a (input) line
@param b (input) line
@return acute angle in radians

Returns the acute angle between the slope of two lines . Lines do not need to ever intersect . Found using the dot product .


public static LineParametric2D_F64 convert( LinePolar2D_F64 src , LineParametric2D_F64 ret )
	{
		if( ret == null )
			ret = new LineParametric2D_F64();

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

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

		return ret;
	}

public static LineParametric2D_F64 convert ( LinePolar2D_F64 src , LineParametric2D_F64 ret ) { if ( ret == null ) ret = new LineParametric2D_F64 ( ) ; double c = ( double ) Math . cos ( src . angle ) ; double s = ( double ) Math . sin ( src . angle ) ; ret . p . set ( c * src . distance , s * src . distance ) ; ret . slope . set ( - s , c ) ; return ret ; }

Converts a line from polar form to parametric.

@param src (input) line is polar notation
@param ret (output) line in parametric notation.  If null a new instance will be created.
@return Converted line in parametric notation

Converts a line from polar form to parametric .


public static LinePolar2D_F64 convert( LineGeneral2D_F64 src , LinePolar2D_F64 ret )
	{
		if( ret == null )
			ret = new LinePolar2D_F64();

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

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

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

		return ret;
	}

public static LinePolar2D_F64 convert ( LineGeneral2D_F64 src , LinePolar2D_F64 ret ) { if ( ret == null ) ret = new LinePolar2D_F64 ( ) ; double r = Math . sqrt ( src . A * src . A + src . B * src . B ) ; double sign = src . C < 0 ? - 1 : 1 ; ret . angle = Math . atan2 ( - sign * src . B / r , - sign * src . A / r ) ; ret . distance = sign * src . C / r ; return ret ; }

Converts a line from general to polar.

@param src (input) line is general notation
@param ret (output) line in polar notation.  If null a new instance will be created.
@return Converted line in polar notation


public static LineParametric2D_F64 convert( LineSegment2D_F64 src , LineParametric2D_F64 ret )
	{
		if( ret == null )
			ret = new LineParametric2D_F64();

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

		return ret;
	}

public static LineParametric2D_F64 convert ( LineSegment2D_F64 src , LineParametric2D_F64 ret ) { if ( ret == null ) ret = new LineParametric2D_F64 ( ) ; ret . p . set ( src . a ) ; ret . slope . set ( src . slopeX ( ) , src . slopeY ( ) ) ; return ret ; }

Converts a line segment into a parametric line.  The start point will be 'src.a' and the
direction will be in the direction of 'src.b-src.a'

@param src (input) line segment
@param ret (output) line in parametric notation.  If null a new instance will be created.
@return Converted line in parametric notation

Converts a line segment into a parametric line . The start point will be src . a and the direction will be in the direction of src . b - src . a


public static LineGeneral2D_F64 convert( LineSegment2D_F64 src , LineGeneral2D_F64 ret )
	{
		return convert(src.a,src.b,ret);
	}

public static LineGeneral2D_F64 convert ( LineSegment2D_F64 src , LineGeneral2D_F64 ret ) { return convert ( src . a , src . b , ret ) ; }

Converts a line segment into a general line.

@param src (Input) line segment
@param ret (output) line in general notation. If null a new instance will be created.
@return Line in general notation

Converts a line segment into a general line .


public static LineGeneral2D_F64 convert( Point2D_F64 a , Point2D_F64 b , LineGeneral2D_F64 ret )
	{
		if( ret == null )
			ret = new LineGeneral2D_F64();

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

		return ret;
	}

public static LineGeneral2D_F64 convert ( Point2D_F64 a , Point2D_F64 b , LineGeneral2D_F64 ret ) { if ( ret == null ) ret = new LineGeneral2D_F64 ( ) ; ret . A = a . y - b . y ; ret . B = b . x - a . x ; ret . C = - ( ret . A * a . x + ret . B * a . y ) ; return ret ; }

Converts a line segment into a general line. Line segment is defined by two points.

@param a (Input) End point of line segment
@param b (Input) End point of line segment
@param ret (output) line in general notation. If null a new instance will be created.
@return Line in general notation

Converts a line segment into a general line . Line segment is defined by two points .


public static LineParametric2D_F64 convert( Point2D_F64 a , Point2D_F64 b , LineParametric2D_F64 ret )
	{
		if( ret == null )
			ret = new LineParametric2D_F64();

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

		return ret;
	}

public static LineParametric2D_F64 convert ( Point2D_F64 a , Point2D_F64 b , LineParametric2D_F64 ret ) { if ( ret == null ) ret = new LineParametric2D_F64 ( ) ; ret . p . set ( a ) ; ret . slope . x = b . x - a . x ; ret . slope . y = b . y - a . y ; return ret ; }

Converts a line segment into a line in parametric format. It will point from a to b. Point a and b must
be unique.

@param a (Input) End point of line segment
@param b (Input) End point of line segment
@param ret (output) Storage for new line.. If null a new instance will be created.
@return The resulting line

Converts a line segment into a line in parametric format . It will point from a to b . Point a and b must be unique .


public static LinePolar2D_F64 convert( LineParametric2D_F64 src , LinePolar2D_F64 ret )
	{
		if( ret == null )
			ret = new LinePolar2D_F64();

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

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

		return ret;
	}

public static LinePolar2D_F64 convert ( LineParametric2D_F64 src , LinePolar2D_F64 ret ) { if ( ret == null ) ret = new LinePolar2D_F64 ( ) ; double top = src . slope . y * src . p . x - src . slope . x * src . p . y ; ret . distance = top / src . slope . norm ( ) ; ret . angle = Math . atan2 ( - src . slope . x , src . slope . y ) ; if ( ret . distance < 0 ) { ret . distance = - ret . distance ; ret . angle = UtilAngle . bound ( ret . angle + Math . PI ) ; } return ret ; }

Converts a line from parametric to polar.

@param src (Input) line in parametric notation
@param ret (output) line in polar notation. If null a new instance will be created.
@return Line in polar notation

Converts a line from parametric to polar .


public static LineGeneral2D_F64 convert( LineParametric2D_F64 src , LineGeneral2D_F64 ret ) {
		if( ret == null ) {
			ret = new LineGeneral2D_F64();
		}

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

		return ret;
	}

public static LineGeneral2D_F64 convert ( LineParametric2D_F64 src , LineGeneral2D_F64 ret ) { if ( ret == null ) { ret = new LineGeneral2D_F64 ( ) ; } ret . A = - src . slope . y ; ret . B = src . slope . x ; ret . C = - ret . A * src . p . x - ret . B * src . p . y ; return ret ; }

Converts a line from parametric to general

@param src (input) line in parametric notation.
@param ret (output) line in general notation.  If null a new instance will be created.
@return Converted line in general notation


public static LineParametric2D_F64 convert( LineGeneral2D_F64 src ,  LineParametric2D_F64 ret ) {
		if( ret == null ) {
			ret = new LineParametric2D_F64();
		}

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

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

		return ret;
	}

public
static
LineParametric2D_F64
convert
(
LineGeneral2D_F64
src
,
LineParametric2D_F64
ret
)
{
if
(
ret
==
)
{
ret
=
new
LineParametric2D_F64
(
)
;
}
ret
.
slope
.
x
=
src
.
B
;
ret
.
slope
.
y
=
src
.
A
;
// find a point on the line
if
(
Math
.
abs
(
src
.
B
)
>
Math
.
abs
(
src
.
A
)
)
{
ret
.
p
.
y
=
src
.
C
/
src
.
B
;
ret
.
p
.
x
=
0
;
}
else
{
ret
.
p
.
x
=
src
.
C
/
src
.
A
;
ret
.
p
.
y
=
0
;
}
return
ret
;
}

Converts a line from general to parametric

@param src (input) line in general notation.
@param ret (output) line in parametric notation.  If null a new instance will be created.
@return Converted line in parametric notation


public static PlaneGeneral3D_F64 convert( PlaneNormal3D_F64 input , PlaneGeneral3D_F64 output ) {
		if( output == null )
			output = new PlaneGeneral3D_F64();

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

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

		return output;
	}

public static PlaneGeneral3D_F64 convert ( PlaneNormal3D_F64 input , PlaneGeneral3D_F64 output ) { if ( output == null ) output = new PlaneGeneral3D_F64 ( ) ; Vector3D_F64 n = input . n ; Point3D_F64 p = input . p ; output . A = n . x ; output . B = n . y ; output . C = n . z ; output . D = n . x * p . x + n . y * p . y + n . z * p . z ; return output ; }

Converts a plane in normal form into a general equation

@param input Plane in normal form.
@param output (Optional) Storage for output plane in general form. If null a new instance will be declared.
@return The convert plane in general form.


public static PlaneNormal3D_F64 convert( PlaneTangent3D_F64 input , PlaneNormal3D_F64 output ) {
		if( output == null )
			output = new PlaneNormal3D_F64();

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

		output.p.set(input);

		return output;
	}

Converts a plane in tangent form into a plane in normal form

@param input Plane in tangent form.
@param output (Optional) Storage for output plane in normal form. If null a new instance will be declared.
@return The convert plane in general form.


public static PlaneNormal3D_F64 convert( Se3_F64 planeToWorld , PlaneNormal3D_F64 output ) {
		if( output == null )
			output = new PlaneNormal3D_F64();

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

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

		return output;
	}

Defines a plane using a 3D rigid body transform. +z is the 3rd column the rotation matrix. The plane's
point is the translation. The plane reference frame is the x-y plane.
@param planeToWorld (Input) SE3 transform from the plane into the "world" frame.
@param output (Output) Plane
@return Plane

Defines a plane using a 3D rigid body transform . + z is the 3rd column the rotation matrix . The plane s point is the translation . The plane reference frame is the x - y plane .


public static void point2Dto3D(Point3D_F64 origin ,
								   Vector3D_F64 axisX , Vector3D_F64 axisY,
								   Point2D_F64 A , Point3D_F64 output ) {

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

public static void point2Dto3D ( Point3D_F64 origin , Vector3D_F64 axisX , Vector3D_F64 axisY , Point2D_F64 A , Point3D_F64 output ) { output . x = origin . x + axisX . x * A . x + axisY . y * A . y ; output . y = origin . y + axisX . y * A . x + axisY . y * A . y ; output . z = origin . z + axisX . z * A . x + axisY . y * A . y ; }

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

Given a point on the plane s 2D coordinate system convert it back into a 3D point .


public static Se3_F64 planeToWorld( PlaneGeneral3D_F64 plane , Se3_F64 planeToWorld ) {
		if( planeToWorld == null )
			planeToWorld = new Se3_F64();

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

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

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

public static Se3_F64 planeToWorld ( PlaneGeneral3D_F64 plane , Se3_F64 planeToWorld ) { if ( planeToWorld == null ) planeToWorld = new Se3_F64 ( ) ; Vector3D_F64 axisZ = new Vector3D_F64 ( plane . A , plane . B , plane . C ) ; axisZ . normalize ( ) ; Vector3D_F64 axisX = new Vector3D_F64 ( ) ; Vector3D_F64 axisY = new Vector3D_F64 ( ) ; UtilPlane3D_F64 . selectAxis2D ( axisZ , axisX , axisY ) ; return planeToWorld ( plane , axisX , axisY , axisZ , planeToWorld ) ; }

Creates a transform from the plane's reference frame into world's reference frame.  The z-axis is set to the
plane's normal and the x-axis and y-axis are arbitrarily choosen.  Points which lie along the plane will
lie along its x-y plane.

@param plane Plane
@param planeToWorld (Optional) storage for the planeToWorld transform.  Can be null.
@return Transform from planeToWorld

Creates a transform from the plane s reference frame into world s reference frame . The z - axis is set to the plane s normal and the x - axis and y - axis are arbitrarily choosen . Points which lie along the plane will lie along its x - y plane .


public boolean isIdentical( T t, double tol ) {
		if( t.getDimension() != getDimension() )
			return false;

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

			if( diff > tol )
				return false;
		}

		return true;
	}

public boolean isIdentical ( T t , double tol ) { if ( t . getDimension ( ) != getDimension ( ) ) return false ; int N = getDimension ( ) ; for ( int i = 0 ; i < N ; i ++ ) { double diff = Math . abs ( getIdx ( i ) - t . getIdx ( i ) ) ; if ( diff > tol ) return false ; } return true ; }

Checks to see if the two GeoTuple have values which are nearly the same.  False is always
returned if the dimension is different.

@param t   The GeoTuple it is being compared against.
@param tol How similar each element must be for them to be considered identical.
@return if they are identical or not.

Checks to see if the two GeoTuple have values which are nearly the same . False is always returned if the dimension is different .


@Override
	public T copy() {
		T ret = createNewInstance();

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

		return ret;
	}

@ Override public T copy ( ) { T ret = createNewInstance ( ) ; int N = getDimension ( ) ; for ( int i = 0 ; i < N ; i ++ ) { ret . setIdx ( i , getIdx ( i ) ) ; } return ret ; }

Generic copy routine.  It is recommended that this be overridden with a faster implementation.

@return An exact copy of this GeoTuple.

Generic copy routine . It is recommended that this be overridden with a faster implementation .


public double normSq() {
		double total = 0;
		int N = getDimension();
		for( int i = 0; i < N; i++ ) {
			double a = getIdx( i );
			total += a * a;
		}

		return total;
	}

public double normSq ( ) { double total = 0 ; int N = getDimension ( ) ; for ( int i = 0 ; i < N ; i ++ ) { double a = getIdx ( i ) ; total += a * a ; } return total ; }

Computes the square of the Euclidean norm.

@return norm squared.

Computes the square of the Euclidean norm .


public static double computeT( LineParametric3D_F64 line , Point3D_F64 pointOnLine ) {

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

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

		double t;

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

public static double computeT ( LineParametric3D_F64 line , Point3D_F64 pointOnLine ) { double dx = pointOnLine . x - line . p . x ; double dy = pointOnLine . y - line . p . y ; double dz = pointOnLine . z - line . p . z ; double adx = Math . abs ( dx ) ; double ady = Math . abs ( dy ) ; double adz = Math . abs ( dz ) ; double t ; if ( adx > ady ) { if ( adx > adz ) { t = dx / line . slope . x ; } else { t = dz / line . slope . z ; } } else if ( ady > adz ) { t = dy / line . slope . y ; } else { t = dz / line . slope . z ; } return t ; }

Computes the value of T for a point on the parametric line

@param line The line
@param pointOnLine Point on a line
@return Value of T for the point


public boolean hasUncountable() {
		return UtilEjml.isUncountable(A) || UtilEjml.isUncountable(C)
				|| UtilEjml.isUncountable(D) || UtilEjml.isUncountable(E) ||
				UtilEjml.isUncountable(F);
	}

public boolean hasUncountable ( ) { return UtilEjml . isUncountable ( A ) || UtilEjml . isUncountable ( C ) || UtilEjml . isUncountable ( D ) || UtilEjml . isUncountable ( E ) || UtilEjml . isUncountable ( F ) ; }

Returns true if any of its parameters have an uncountable number


public boolean isEquivalent( ParabolaGeneral_F64 parabola , double tol ) {
		double scale = relativeScale(parabola);

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

		return true;
	}

public boolean isEquivalent ( ParabolaGeneral_F64 parabola , double tol ) { double scale = relativeScale ( parabola ) ; if ( Math . abs ( A * scale - parabola . A ) > tol ) return false ; if ( Math . abs ( C * scale - parabola . C ) > tol ) return false ; if ( Math . abs ( D * scale - parabola . D ) > tol ) return false ; if ( Math . abs ( E * scale - parabola . E ) > tol ) return false ; if ( Math . abs ( F * scale - parabola . F ) > tol ) return false ; return true ; }

Determines if they are equivalent up to a scale factor


public Point3D_F64 center( Point3D_F64 storage ) {
		if( storage == null )
			storage = new Point3D_F64();
		storage.x = (p0.x + p1.x)/2.0;
		storage.y = (p0.y + p1.y)/2.0;
		storage.z = (p0.z + p1.z)/2.0;
		return storage;
	}

public Point3D_F64 center ( Point3D_F64 storage ) { if ( storage == null ) storage = new Point3D_F64 ( ) ; storage . x = ( p0 . x + p1 . x ) / 2.0 ; storage . y = ( p0 . y + p1 . y ) / 2.0 ; storage . z = ( p0 . z + p1 . z ) / 2.0 ; return storage ; }

Computes and return the center of the cube.
@param storage Optional storage for the center.  If null a new instance will be created and returned.
@return The cube's center point

Computes and return the center of the cube .


public static double acuteAngle( LineSegment2D_I32 line0 , LineSegment2D_I32 line1 ) {
		int dx0 = line0.b.x - line0.a.x;
		int dy0 = line0.b.y - line0.a.y;
		int dx1 = line1.b.x - line1.a.x;
		int dy1 = line1.b.y - line1.a.y;

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

public static double acuteAngle ( LineSegment2D_I32 line0 , LineSegment2D_I32 line1 ) { int dx0 = line0 . b . x - line0 . a . x ; int dy0 = line0 . b . y - line0 . a . y ; int dx1 = line1 . b . x - line1 . a . x ; int dy1 = line1 . b . y - line1 . a . y ; double bottom = Math . sqrt ( dx0 * dx0 + dy0 * dy0 ) * Math . sqrt ( dx1 * dx1 + dy1 * dy1 ) ; return Math . acos ( ( dx0 * dx1 + dy0 * dy1 ) / bottom ) ; }

Computes the acute angle between the two lines.  Does not check for intersection

@param line0 First line
@param line1 Second line
@return Acute angle in radians

Computes the acute angle between the two lines . Does not check for intersection


public static boolean isInfiniteH(Point4D_F64 p , double tol ) {
		double n = Math.sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
		return Math.abs(p.w) <= n*tol;
	}

public static boolean isInfiniteH ( Point4D_F64 p , double tol ) { double n = Math . sqrt ( p . x * p . x + p . y * p . y + p . z * p . z ) ; return Math . abs ( p . w ) <= n * tol ; }

Checks to see if the homogenous 3D point lies on the plane at infinity

@param p (Input) Homogenous point
@param tol (Input) tolerance. Try EPS
@return true if on plane at infinity


public static List<Point4D_F64> randomN( Point3D_F64 center , double w, double stdev, int num, Random rand ) {
		List<Point4D_F64> ret = new ArrayList<>();

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

			ret.add( p );
		}

		return ret;
	}

public static List < Point4D_F64 > randomN ( Point3D_F64 center , double w , double stdev , int num , Random rand ) { List < Point4D_F64 > ret = new ArrayList <> ( ) ; for ( int i = 0 ; i < num ; i ++ ) { Point4D_F64 p = new Point4D_F64 ( ) ; p . x = center . x + rand . nextGaussian ( ) * stdev ; p . y = center . y + rand . nextGaussian ( ) * stdev ; p . z = center . z + rand . nextGaussian ( ) * stdev ; p . w = w ; ret . add ( p ) ; } return ret ; }

Normally distributed homogenous 3D point. w is fixed

Normally distributed homogenous 3D point . w is fixed


public static Point3D_F64 h_to_e( Point4D_F64 p ) {
		Point3D_F64 out = new Point3D_F64();
		h_to_e(p,out);
		return out;
	}

public static Point3D_F64 h_to_e ( Point4D_F64 p ) { Point3D_F64 out = new Point3D_F64 ( ) ; h_to_e ( p , out ) ; return out ; }

Converts a point from homogenous coordinates into Euclidean
@param p 3D point in homogenous coordinates
@return 3D point


public static Vector2D_F64 minus( Point2D_F64 a , Point2D_F64 b , Vector2D_F64 output ) {
		if( output == null )
			output = new Vector2D_F64();

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

		return output;
	}

public static Vector2D_F64 minus ( Point2D_F64 a , Point2D_F64 b , Vector2D_F64 output ) { if ( output == null ) output = new Vector2D_F64 ( ) ; output . x = a . x - b . x ; output . y = a . y - b . y ; return output ; }

Sets the vector equal to 'a' - 'b'.

@param a point
@param b point
@param output (output) optional storage for vector.
@return Solution


public static boolean identicalSign( double xa , double ya , double xb , double yb , double tol ) {
		double dx0 = xb-xa;
		double dy0 = yb-ya;
		double dx1 = xb+xa;
		double dy1 = yb+ya;

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

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

	}

public static boolean identicalSign ( double xa , double ya , double xb , double yb , double tol ) { double dx0 = xb - xa ; double dy0 = yb - ya ; double dx1 = xb + xa ; double dy1 = yb + ya ; double error0 = dx0 * dx0 + dy0 * dy0 ; double error1 = dx1 * dx1 + dy1 * dy1 ; if ( error0 < error1 ) { return error0 <= tol * tol ; } else { return error1 <= tol * tol ; } }

Tests to see if the two vectors are identical up to a sign difference
@param xa x-component of vector 'a'
@param ya y-component of vector 'a'
@param xb x-component of vector 'b'
@param yb y-component of vector 'b'
@param tol Tolerance in Euclidan distance
@return true if identical to within tolerance and a sign ambiguity


public void set(RectangleLength2D_I32 r) {
		this.x0 = r.x0;
		this.y0 = r.y0;
		this.width = r.width;
		this.height = r.height;
	}

public void set ( RectangleLength2D_I32 r ) { this . x0 = r . x0 ; this . y0 = r . y0 ; this . width = r . width ; this . height = r . height ; }

Sets this rectangle to be equal to the passed in rectangle.
@param r Rectangle which this is to be set equal to

Sets this rectangle to be equal to the passed in rectangle .


public static EllipseQuadratic_F64 convert( EllipseRotated_F64 input , EllipseQuadratic_F64 output ) {
		if( output == null )
			output = new EllipseQuadratic_F64();

		double x0 = input.center.x;
		double y0 = input.center.y;
		double a = input.a;
		double b = input.b;
		double phi = input.phi;

		double cphi = Math.cos(phi);
		double sphi = Math.sin(phi);
		double cphi2 = cphi*cphi;
		double sphi2 = sphi*sphi;

		double a2 = a*a;
		double b2 = b*b;
		double x02 = x0*x0;
		double y02 = y0*y0;

		// TODO simplfy using more trig identities
		output.A = cphi2/a2 + sphi2/b2;
		output.B = sphi*cphi/a2 - sphi*cphi/b2;
		output.C = sphi2/a2 + cphi2/b2;
		output.D = -x0*cphi2/a2 - y0*sphi*cphi/a2 - x0*sphi2/b2 + y0*sphi*cphi/b2;
		output.E = -x0*sphi*cphi/a2 - y0*sphi2/a2 + x0*sphi*cphi/b2 - y0*cphi2/b2;
		output.F = x02*cphi2/a2 + 2*x0*y0*sphi*cphi/a2 + y02*sphi2/a2 +
				x02*sphi2/b2 - 2*x0*y0*sphi*cphi/b2 + y02*cphi2/b2 - 1;

		return output;
	}

Convert from rotated to quadratic.

@param input Input rotated format.
@param output (Optional) Storage for quadratic format.  Can be null.
@return Ellipse in quadratic format.


public static Point2D_F64 computePoint( double t , EllipseRotated_F64 ellipse , Point2D_F64 output ) {
		if( output == null )
			output = new Point2D_F64();

		double ct = Math.cos(t);
		double st = Math.sin(t);
		double cphi = Math.cos(ellipse.phi);
		double sphi = Math.sin(ellipse.phi);

		// coordinate in ellipse frame
		double x = ellipse.a*ct;
		double y = ellipse.b*st;

		// put into global frame
		output.x = ellipse.center.x + x*cphi - y*sphi;
		output.y = ellipse.center.y + x*sphi + y*cphi;

		return output;
	}

public
static
Point2D_F64
computePoint
(
double
t
,
EllipseRotated_F64
ellipse
,
Point2D_F64
output
)
{
if
(
output
==
)
output
=
new
Point2D_F64
(
)
;
double
ct
=
Math
.
cos
(
t
)
;
double
st
=
Math
.
sin
(
t
)
;
double
cphi
=
Math
.
cos
(
ellipse
.
phi
)
;
double
sphi
=
Math
.
sin
(
ellipse
.
phi
)
;
// coordinate in ellipse frame
double
x
=
ellipse
.
a
*
ct
;
double
y
=
ellipse
.
b
*
st
;
// put into global frame
output
.
x
=
ellipse
.
center
.
x
+
x
*
cphi
y
*
sphi
;
output
.
y
=
ellipse
.
center
.
y
+
x
*
sphi
+
y
*
cphi
;
return
output
;
}

Computes the point on the ellipse at location 't', where t is an angle in radians

@param t An angle in radians from 0 to 2*PI
@param ellipse Ellipse
@param output (Optional) point on the ellipse .  Can be null.
@return Point on the ellipse

Computes the point on the ellipse at location t where t is an angle in radians


public static double computeAngle( Point2D_F64 p , EllipseRotated_F64 ellipse ) {
		// put point into ellipse's reference frame
		double ce = Math.cos(ellipse.phi);
		double se = Math.sin(ellipse.phi);

		// world into ellipse frame
		double xc = p.x - ellipse.center.x;
		double yc = p.y - ellipse.center.y;

		double x =  ce*xc + se*yc;
		double y = -se*xc + ce*yc;

		return Math.atan2( y/ellipse.b , x/ellipse.a );
	}

Computes the value of 't' used to specify a point's location

@param p Point on the ellipse
@param ellipse Ellipse
@return Angle from -pi to pi

Computes the value of t used to specify a point s location


public static Vector2D_F64 computeTangent( double t ,
											   EllipseRotated_F64 ellipse ,
											   Vector2D_F64 output  ) {
		if( output == null )
			output = new Vector2D_F64();

		double ct = Math.cos(t);
		double st = Math.sin(t);
		double cphi = Math.cos(ellipse.phi);
		double sphi = Math.sin(ellipse.phi);

		// point in ellipse frame multiplied by b^2 and a^2
		double x = ellipse.a*ct*ellipse.b*ellipse.b;
		double y = ellipse.b*st*ellipse.a*ellipse.a;

		// rotate vector normal into world frame
		double rx = x*cphi - y*sphi;
		double ry = x*sphi + y*cphi;

		// normalize and change into tangent
		double r = Math.sqrt(rx*rx + ry*ry);

		output.x = -ry/r;
		output.y = rx/r;

		return output;
	}

Computes the tangent to the ellipse at the specified location

@param t Location on the ellipse.  Radians
@param ellipse Ellipse equation
@param output Optional storage for tangent
@return The tangent


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

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
)
!=
;
tempLine
.
b
=
temp1
;
boolean
crossed1
=
Intersection2D_F64
.
intersection
(
centerLine
,
tempLine
,
junk
)
!=
;
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
;
}

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

Selects a tangent point on the ellipse which is closest to the original source point of A .


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

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.

Used to retrieve the corners of the box .


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

Distance of the closest point between two lines . Parallel lines are correctly handled .


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.

@param l Line. Not modified.
@param p Point. Not modified.
@return distance.

Distance from the point to the closest point on the line .


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 from the point to the closest point on the line segment.

@param l Line. Not modified.
@param p Point. Not modified.
@return distance.

Distance from the point to the closest point on the line segment .


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

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

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

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 .


public static double distance( Cylinder3D_F64 cylinder, Point3D_F64 point ) {

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

		return r - cylinder.radius;
	}

public static double distance ( Cylinder3D_F64 cylinder , Point3D_F64 point ) { double r = Distance3D_F64 . distance ( cylinder . line , point ) ; return r - cylinder . 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

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 .


public static double distance( LineSegment2D_F64 segmentA , LineSegment2D_F64 segmentB ) {
		return Math.sqrt(distanceSq(segmentA, segmentB));
	}

public static double distance ( LineSegment2D_F64 segmentA , LineSegment2D_F64 segmentB ) { return Math . sqrt ( distanceSq ( segmentA , segmentB ) ) ; }

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

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 distance( Quadrilateral_F64 quad , Point2D_F64 p ) {
		return Math.sqrt(distanceSq(quad,p));
	}

public static double distance ( Quadrilateral_F64 quad , Point2D_F64 p ) { return Math . sqrt ( distanceSq ( quad , p ) ) ; }

Returns the Euclidean distance of the closest point on the quadrilateral to the provided point.

@param quad Quadrilateral
@param p Point
@return Distance apart

Returns the Euclidean distance of the closest point on the quadrilateral to the provided point .


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

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 squared of the closest point on the quadrilateral to the provided point.

@param quad Quadrilateral
@param p Point
@return Distance squared apart

Returns the Euclidean distance squared of the closest point on the quadrilateral to the provided point .


public static double distance( Polygon2D_F64 poly , Point2D_F64 p ) {
		return Math.sqrt(distanceSq(poly, p, null));
	}

public static double distance ( Polygon2D_F64 poly , Point2D_F64 p ) { return Math . sqrt ( distanceSq ( poly , p , null ) ) ; }

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

Returns the Euclidean distance of the closest point on the Polygon to the provided point .


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

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

Returns the Euclidean distance squared of the closest point on the Polygon to the provided point .


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

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

Returns the distance of the closest point on the line from the origin
@param line Line
@return Euclidean distance


public static double distance(EllipseRotated_F64 ellipse , Point2D_F64 p ) {
		return Math.sqrt(distance2(ellipse, p));
	}

public static double distance ( EllipseRotated_F64 ellipse , Point2D_F64 p ) { return Math . sqrt ( distance2 ( ellipse , p ) ) ; }

Euclidean distance of closest point on ellipse to point 'p'.

@param ellipse Ellipse
@param p Point
@return Euclidean distance

Euclidean distance of closest point on ellipse to point p .


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

Euclidean distance squared of closest point on ellipse to point 'p'.

@param ellipse Ellipse
@param p Point
@return Euclidean distance squared

Euclidean distance squared of closest point on ellipse to point p .


public void addTransform( boolean forward, T tran ) {
		path.add( new Node<T>( tran, forward ) );
	}

public void addTransform ( boolean forward , T tran ) { path . add ( new Node < T > ( tran , forward ) ) ; }

Adds the next transform in the sequence.

@param forward Does the transform work in the forward or reverse direction.
@param tran	The transform.

Adds the next transform in the sequence .


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

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

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

Finds the closest point on the line segment to the provided point p .


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

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

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

Computes the closest point on an ellipse to the provided point . If there are multiple solutions then one is arbitrarily chosen .


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

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

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

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

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

Creates a random vector where each axis is selected from a uniform distribution .


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

Selects a vector which will be perpendicular.

@param A (Input) A vector
@param output (Optional) storage for output
@return Perpendicular vector

Selects a vector which will be perpendicular .


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;

	}

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

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.

Checks to see if the two vectors are identical to within tolerance . Each axis is checked individually .


public static void normalize( Vector3D_F64 v ) {
		double a = v.norm();

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

public static void normalize ( Vector3D_F64 v ) { double a = v . norm ( ) ; v . x /= a ; v . y /= a ; v . z /= a ; }

Rescales the vector such that its normal is equal to one.

@param v Vector being normalized.

Rescales the vector such that its normal is equal to one .


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

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

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.

Creates a matrix from the set of column vectors . Each vector is a column in the new matrix .


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

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

Converts matrices into vectors.  All matrices must be vectors with 3 elements.

@param m A 3x1 or 1x3 matrix
@return Equivalent 3D vector

Converts matrices into vectors . All matrices must be vectors with 3 elements .


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

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

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 distance from the point
@param x x-coordinate
@param y y-coordinate
@return distance


public void setEllipse( EllipseRotated_F64 ellipse ) {
		this.ellipse = ellipse;
		ce = Math.cos(ellipse.phi);
		se = Math.sin(ellipse.phi);
	}

public void setEllipse ( EllipseRotated_F64 ellipse ) { this . ellipse = ellipse ; ce = Math . cos ( ellipse . phi ) ; se = Math . sin ( ellipse . phi ) ; }

Specifies the ellipse which point distance is going to be found from
@param ellipse Ellipse description


public void normalize() {
		double n = Math.sqrt( w * w + x * x + y * y + z * z);

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

public void normalize ( ) { double n = Math . sqrt ( w * w + x * x + y * y + z * z ) ; w /= n ; x /= n ; y /= n ; z /= n ; }

Converts the quaternion into a unit quaternion.

Converts the quaternion into a unit quaternion .


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

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

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

Computes the area of an arbitrary triangle from 3 - vertices .


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

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 quadrilateral computed from two triangles.

@param quad quadrilateral
@return area

Area of a quadrilateral computed from two triangles .


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

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

Area of a simple polygon.  Meaning it can be concave or convex, but can't have self intersections
@param poly Simple polygon
@return area

Area of a simple polygon . Meaning it can be concave or convex but can t have self intersections


public static Point2D_F64 mean( Point2D_F64[] list , int offset , int length , Point2D_F64 mean ) {
		if( mean == null )
			mean = new Point2D_F64();

		double x = 0;
		double y = 0;

		for (int i = 0; i < length; i++) {
			Point2D_F64 p = list[offset+i];
			x += p.getX();
			y += p.getY();
		}

		x /= length;
		y /= length;

		mean.set(x, y);
		return mean;
	}

public static Point2D_F64 mean ( Point2D_F64 [ ] list , int offset , int length , Point2D_F64 mean ) { if ( mean == null ) mean = new Point2D_F64 ( ) ; double x = 0 ; double y = 0 ; for ( int i = 0 ; i < length ; i ++ ) { Point2D_F64 p = list [ offset + i ] ; x += p . getX ( ) ; y += p . getY ( ) ; } x /= length ; y /= length ; mean . set ( x , y ) ; return mean ; }

Finds the point which has the mean location of all the points in the array. This is also known
as the centroid.

@param list List of points
@param offset First index in list
@param length Length of elements in list
@param mean Storage for mean point.  If null then a new instance will be declared
@return The found mean

Finds the point which has the mean location of all the points in the array . This is also known as the centroid .


public static List<Point2D_F64> orderCCW( List<Point2D_F64> points ) {
		Point2D_F64 center = mean(points,null);

		double angles[] = new double[ points.size() ];
		for (int i = 0; i < angles.length; i++) {
			Point2D_F64 p = points.get(i);

			double dx = p.x - center.x;
			double dy = p.y - center.y;

			angles[i] = Math.atan2(dy,dx);
		}

		int order[] = new int[ points.size() ];

		QuickSort_F64 sorter = new QuickSort_F64();
		sorter.sort(angles,0,points.size(),order);

		List<Point2D_F64> out = new ArrayList<Point2D_F64>(points.size());
		for (int i = 0; i < points.size(); i++) {
			out.add(points.get(order[i]));
		}

		return out;
	}

public static List < Point2D_F64 > orderCCW ( List < Point2D_F64 > points ) { Point2D_F64 center = mean ( points , null ) ; double angles [ ] = new double [ points . size ( ) ] ; for ( int i = 0 ; i < angles . length ; i ++ ) { Point2D_F64 p = points . get ( i ) ; double dx = p . x - center . x ; double dy = p . y - center . y ; angles [ i ] = Math . atan2 ( dy , dx ) ; } int order [ ] = new int [ points . size ( ) ] ; QuickSort_F64 sorter = new QuickSort_F64 ( ) ; sorter . sort ( angles , 0 , points . size ( ) , order ) ; List < Point2D_F64 > out = new ArrayList < Point2D_F64 > ( points . size ( ) ) ; for ( int i = 0 ; i < points . size ( ) ; i ++ ) { out . add ( points . get ( order [ i ] ) ) ; } return out ; }

Puts the points into counter-clockwise order around their center.

@param points List of points. Not modified.
@return ordered list

Puts the points into counter - clockwise order around their center .


public static void computeNormal(List<Point2D_F64> points , Point2D_F64 mean , DMatrix covariance ) {
		if( covariance.getNumCols() != 2 || covariance.getNumRows() != 2 ) {
			if (covariance instanceof ReshapeMatrix) {
				((ReshapeMatrix)covariance).reshape(2,2);
			} else {
				throw new IllegalArgumentException("Must be a 2x2 matrix");
			}
		}

		mean(points,mean);

		double xx=0,xy=0,yy=0;

		for (int i = 0; i < points.size(); i++) {
			Point2D_F64 p = points.get(i);
			double dx = p.x-mean.x;
			double dy = p.y-mean.y;

			xx += dx*dx;
			xy += dx*dy;
			yy += dy*dy;
		}
		xx /= points.size();
		xy /= points.size();
		yy /= points.size();

		covariance.unsafe_set(0,0,xx);
		covariance.unsafe_set(0,1,xy);
		covariance.unsafe_set(1,0,xy);
		covariance.unsafe_set(1,1,yy);
	}

public
static
void
computeNormal
(
List
<
Point2D_F64
>
points
,
Point2D_F64
mean
,
DMatrix
covariance
)
{
if
(
covariance
.
getNumCols
(
)
!=
2
||
covariance
.
getNumRows
(
)
!=
2
)
{
if
(
covariance
instanceof
ReshapeMatrix
)
{
(
(
ReshapeMatrix
)
covariance
)
.
reshape
(
2
,
2
)
;
}
else
{
throw
new
IllegalArgumentException
(
"Must be a 2x2 matrix"
)
;
}
}
mean
(
points
,
mean
)
;
double
xx
=
0
,
xy
=
0
,
yy
=
0
;
for
(
int
i
=
0
;
i
<
points
.
size
(
)
;
i
++
)
{
Point2D_F64
p
=
points
.
get
(
i
)
;
double
dx
=
p
.
x
mean
.
x
;
double
dy
=
p
.
y
mean
.
y
;
xx
+=
dx
*
dx
;
xy
+=
dx
*
dy
;
yy
+=
dy
*
dy
;
}
xx
/=
points
.
size
(
)
;
xy
/=
points
.
size
(
)
;
yy
/=
points
.
size
(
)
;
covariance
.
unsafe_set
(
0
,
0
,
xx
)
;
covariance
.
unsafe_set
(
0
,
1
,
xy
)
;
covariance
.
unsafe_set
(
1
,
0
,
xy
)
;
covariance
.
unsafe_set
(
1
,
1
,
yy
)
;
}

Computes the mean and covariance matrix from the set of points. This describes a normal distribution
@param points (Input) points
@param mean (Output) mean of the points
@param covariance (Output) 2x2 covariance matrix

Computes the mean and covariance matrix from the set of points . This describes a normal distribution


public static boolean isConvex( Polygon2D_I32 poly ) {
		// if the cross product of all consecutive triples is positive or negative then it is convex

		final int N = poly.size();
		int numPositive = 0;
		for (int i = 0; i < N; i++) {
			int j = (i+1)%N;
			int k = (i+2)%N;

			Point2D_I32 a = poly.vertexes.data[i];
			Point2D_I32 b = poly.vertexes.data[j];
			Point2D_I32 c = poly.vertexes.data[k];

			int dx0 = a.x-b.x;
			int dy0 = a.y-b.y;

			int dx1 = c.x-b.x;
			int dy1 = c.y-b.y;

			int z = dx0 * dy1 - dy0 * dx1;
			if( z > 0 )
				numPositive++;
			// z can be zero if there are duplicate points.
			// not sure if it should throw an exception if its "bad" or not
		}

		return( numPositive == 0 || numPositive == N );
	}

Determines if the polugon is convex or concave.

@param poly Polygon
@return true if convex and false if concave

Determines if the polugon is convex or concave .


public void timesIP( double scalar ) {
		x *= scalar;
		y *= scalar;
		z *= scalar;
		w *= scalar;
	}

public void timesIP ( double scalar ) { x *= scalar ; y *= scalar ; z *= scalar ; w *= scalar ; }

In-place scalar multiplication
@param scalar value that it is multiplied by


public double maxAbs() {
		double absX = Math.abs(x);
		double absY = Math.abs(y);
		double absZ = Math.abs(z);
		double absW = Math.abs(w);

		double found = Math.max(absX,absY);
		if( found < absZ )
			found = absZ;
		if( found < absW )
			found = absW;
		return found;
	}

public double maxAbs ( ) { double absX = Math . abs ( x ) ; double absY = Math . abs ( y ) ; double absZ = Math . abs ( z ) ; double absW = Math . abs ( w ) ; double found = Math . max ( absX , absY ) ; if ( found < absZ ) found = absZ ; if ( found < absW ) found = absW ; return found ; }

Returns the absolute value of the component with the largest absolute value
@return max absolute value


public static double distance( double x0 , double y0 , double z0 ,
								   double x1 , double y1 , double z1) {
		return norm(x1-x0,y1-y0,z1-z0);
	}

public static double distance ( double x0 , double y0 , double z0 , double x1 , double y1 , double z1 ) { return norm ( x1 - x0 , y1 - y0 , z1 - z0 ) ; }

Euclidean distance between the two specified points
@param x0 x-axis on Point 0
@param y0 y-axis on Point 0
@param z0 z-axis on Point 0
@param x1 x-axis on Point 1
@param y1 y-axis on Point 1
@param z1 z-axis on Point 1
@return Euclidean distance


public static double distanceSq( double x0 , double y0 , double z0 ,
								   double x1 , double y1 , double z1) {
		double dx = x1-x0;
		double dy = y1-y0;
		double dz = z1-z0;

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

public static double distanceSq ( double x0 , double y0 , double z0 , double x1 , double y1 , double z1 ) { double dx = x1 - x0 ; double dy = y1 - y0 ; double dz = z1 - z0 ; return dx * dx + dy * dy + dz * dz ; }

Euclidean distance squared between the two specified points
@param x0 x-axis on Point 0
@param y0 y-axis on Point 0
@param z0 z-axis on Point 0
@param x1 x-axis on Point 1
@param y1 y-axis on Point 1
@param z1 z-axis on Point 1
@return Euclidean distance squared