signature:
public class GeometryMath_F64 { /** * Computes the inner matrix product : < br >
 * ret = x < sup > T < / sup > A < sup > T < / sup > y
 * @ param a 3D point .
 * @ param M 3 by 3 matrix .
 * @ param b 3D point .
 * @ return scalar number */
public static double innerProdTranM ( GeoTuple3D_F64 a , DMatrixRMaj M , GeoTuple3D_F64 b ) { } }

implementation: if ( M . numRows != 3 || M . numCols != 3 ) throw new IllegalArgumentException ( "M must be 3 by 3." ) ; DMatrixRMaj m1 = new DMatrixRMaj ( 3 , 1 , true , a . x , a . y , a . z ) ; DMatrixRMaj m2 = new DMatrixRMaj ( 3 , 1 , true , b . x , b . y , b . z ) ; return ( double ) ( VectorVectorMult_DDRM . innerProdTranA ( m1 , M , m2 ) ) ;


signature:
public class GeometryMath_F64 { /** * Computes the outer product of two vectors : < br >
 * O = a * b < sup > T < / sup >
 * @ param a 3D vector
 * @ param b 3D vector
 * @ param ret 3 x 3 matrix or null .
 * @ return outer product of two 3d vectors */
public static DMatrixRMaj outerProd ( GeoTuple3D_F64 a , GeoTuple3D_F64 b , DMatrixRMaj ret ) { } }

implementation: if ( ret == null ) ret = new DMatrixRMaj ( 3 , 3 ) ; ret . data [ 0 ] = a . x * b . x ; ret . data [ 1 ] = a . x * b . y ; ret . data [ 2 ] = a . x * b . z ; ret . data [ 3 ] = a . y * b . x ; ret . data [ 4 ] = a . y * b . y ; ret . data [ 5 ] = a . y * b . z ; ret . data [ 6 ] = a . z * b . x ; ret . data [ 7 ] = a . z * b . y ; ret . data [ 8 ] = a . z * b . z ; return ret ;


signature:
public class GeometryMath_F64 { /** * Adds the outer product of two vectors onto a matrix : < br >
 * ret = A + scalar * a * b < sup > T < / sup >
 * @ param A 3x3 matrix
 * @ param b 3D vector
 * @ param c 3D vector
 * @ param ret 3 x 3 matrix or null .
 * @ return outer product of two 3d vectors */
public static DMatrixRMaj addOuterProd ( DMatrixRMaj A , double scalar , GeoTuple3D_F64 b , GeoTuple3D_F64 c , DMatrixRMaj ret ) { } }

implementation: if ( ret == null ) ret = new DMatrixRMaj ( 3 , 3 ) ; ret . data [ 0 ] = A . data [ 0 ] + scalar * b . x * c . x ; ret . data [ 1 ] = A . data [ 1 ] + scalar * b . x * c . y ; ret . data [ 2 ] = A . data [ 2 ] + scalar * b . x * c . z ; ret . data [ 3 ] = A . data [ 3 ] + scalar * b . y * c . x ; ret . data [ 4 ] = A . data [ 4 ] + scalar * b . y * c . y ; ret . data [ 5 ] = A . data [ 5 ] + scalar * b . y * c . z ; ret . data [ 6 ] = A . data [ 6 ] + scalar * b . z * c . x ; ret . data [ 7 ] = A . data [ 7 ] + scalar * b . z * c . y ; ret . data [ 8 ] = A . data [ 8 ] + scalar * b . z * c . z ; return ret ;


signature:
public class GeometryMath_F64 { /** * Computes the inner matrix product : ret = a ' * M * b < br >
 * where ret is a scalar number . ' a ' and ' b ' are automatically converted into homogeneous
 * coordinates .
 * @ param a 2D point .
 * @ param M 3 by 3 matrix .
 * @ param b 2D point .
 * @ return scalar number , */
public static double innerProd ( GeoTuple2D_F64 a , DMatrixRMaj M , GeoTuple2D_F64 b ) { } }

implementation: if ( M . numRows != 3 || M . numCols != 3 ) throw new IllegalArgumentException ( "M must be 3 by 3." ) ; DMatrixRMaj m1 = new DMatrixRMaj ( 3 , 1 , true , a . x , a . y , 1 ) ; DMatrixRMaj m2 = new DMatrixRMaj ( 3 , 1 , true , b . x , b . y , 1 ) ; return ( double ) ( VectorVectorMult_DDRM . innerProdA ( m1 , M , m2 ) ) ;


signature:
public class GeometryMath_F64 { /** * Multiplies each element in the tuple by ' v ' . < br >
 * p < sub > i < / sub > = p < sub > i < / sub > * v
 * @ param p tuple .
 * @ param v scaling factor . */
public static void scale ( GeoTuple3D_F64 p , double v ) { } }


signature:
public class GeometryMath_F64 { /** * Divides each element by ' v '
 * @ param p tuple
 * @ param v divisor */
public static void divide ( GeoTuple3D_F64 p , double v ) { } }


signature:
public class GeometryMath_F64 { /** * Changes the sign of the vector : < br >
 * < br >
 * T = - T
 * @ param t Vector whose sign is being changed . Modified . */
public static void changeSign ( GeoTuple3D_F64 t ) { } }


signature:
public class GeometryMath_F64 { /** * Converts a GeoTuple3D _ F64 into DMatrixRMaj
 * @ param in Input vector
 * @ param out Output matrix . If null a new matrix will be declared
 * @ return Converted matrix */
public static DMatrixRMaj toMatrix ( GeoTuple3D_F64 in , DMatrixRMaj out ) { } }

implementation: 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 ;


signature:
public class GeometryMath_F64 { /** * Converts a DMatrixRMaj into GeoTuple3D _ F64
 * @ param in Input matrix
 * @ param out Output vector . */
public static void toTuple3D ( DMatrixRMaj in , GeoTuple3D_F64 out ) { } }

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


signature:
public class Rodrigues_F64 { /** * Assign the Rodrigues coordinates using a 3 element vector . Theta is the vector ' s
 * magnitude and the axis of rotation is the unit vector .
 * @ param x x - component of 3 vector
 * @ param y y - component of 3 vector
 * @ param z z - component of 3 vector */
public void setParamVector ( double x , double y , double z ) { } }

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


signature:
public class UtilAngle { /** * Converts an angle which is ( - pi to pi ) into a half circle angle ( - pi / 2 to pi / 2 ) .
 * @ param angle Angle between - pi and pi .
 * @ return - pi / 2 to pi / 2 */
public static double toHalfCircle ( double angle ) { } }

implementation: if ( angle < 0 ) angle += Math . PI ; if ( angle > Math . PI / 2.0 ) angle -= Math . PI ; return angle ;


signature:
public class UtilAngle { /** * Returns an angle which is equivalent to the one provided , but between ( inclusive ) - & pi ; and & pi ; . */
public static double bound ( double ang ) { } }

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


signature:
public class UtilAngle { /** * Returns an angle which is equivalent to the one provided , but between ( inclusive ) - & pi ; and & pi ; . */
public static float bound ( float ang ) { } }

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


signature:
public class UtilAngle { /** * Bounds the angle between - & pi ; / 2 and & pi ; / 2
 * @ param angle angle in radians
 * @ return bounded angle */
public static double boundHalf ( double angle ) { } }

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


signature:
public class UtilAngle { /** * Bounds the angle between - & pi ; / 2 and & pi ; / 2
 * @ param angle angle in radians
 * @ return bounded angle */
public static float boundHalf ( float angle ) { } }

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


signature:
public class UtilAngle { /** * Angular distance in radians to go from angA to angB in counter clock - wise direction .
 * The resulting angle will be from 0 to 2 & pi ; .
 * @ param angA First angle . - pi to pi
 * @ param angB Second angle - pi to pi
 * @ return An angle from 0 to 2 & pi ; */
public static double distanceCCW ( double angA , double angB ) { } }

implementation: if ( angB >= angA ) return angB - angA ; else return GrlConstants . PI2 - ( angA - angB ) ;


signature:
public class UtilAngle { /** * Angular distance in radians to go from angA to angB in counter clock - wise direction .
 * The resulting angle will be from 0 to 2 & pi ; .
 * @ param angA First angle . - pi to pi
 * @ param angB Second angle - pi to pi
 * @ return An angle from 0 to 2 & pi ; */
public static float distanceCCW ( float angA , float angB ) { } }

implementation: if ( angB >= angA ) return angB - angA ; else return GrlConstants . F_PI2 - ( angA - angB ) ;


signature:
public class UtilAngle { /** * Angular distance in radians to go from angA to angB in clock - wise direction .
 * The resulting angle will be from 0 to 2 & pi ; .
 * @ param angA First angle . - pi to pi
 * @ param angB Second angle - pi to pi
 * @ return An angle from 0 to 2 & pi ; */
public static double distanceCW ( double angA , double angB ) { } }

implementation: if ( angA >= angB ) return angA - angB ; else return GrlConstants . PI2 - ( angB - angA ) ;


signature:
public class UtilAngle { /** * Angular distance in radians to go from angA to angB in clock - wise direction .
 * The resulting angle will be from 0 to 2 & pi ; .
 * @ param angA First angle . - pi to pi
 * @ param angB Second angle - pi to pi
 * @ return An angle from 0 to 2 & pi ; */
public static float distanceCW ( float angA , float angB ) { } }

implementation: if ( angA >= angB ) return angA - angB ; else return GrlConstants . F_PI2 - ( angB - angA ) ;


signature:
public class UtilAngle { /** * Returns the difference between two angles and bounds the result between - pi and pi : < br >
 * result = angA - angB < br >
 * and takes in account boundary conditions .
 * @ param angA first angle . Must be between - pi and pi .
 * @ param angB second angle . Must be between - pi and pi .
 * @ return an angle between - pi and pi */
public static double minus ( double angA , double angB ) { } }

implementation: double diff = angA - angB ; if ( diff > Math . PI ) { return GrlConstants . PI2 - diff ; } else if ( diff < - Math . PI ) return - GrlConstants . PI2 - diff ; return diff ;


signature:
public class UtilAngle { /** * Returns the difference between two angles and bounds the result between - pi and pi : < br >
 * result = angA - angB < br >
 * and takes in account boundary conditions .
 * @ param angA first angle . Must be between - pi and pi .
 * @ param angB second angle . Must be between - pi and pi .
 * @ return an angle between - pi and pi */
public static float minus ( float angA , float angB ) { } }

implementation: float diff = angA - angB ; if ( diff > GrlConstants . F_PI ) { return GrlConstants . F_PI2 - diff ; } else if ( diff < - GrlConstants . F_PI ) return - GrlConstants . F_PI2 - diff ; return diff ;


signature:
public class UtilAngle { /** * Angular distance between two half circle angles .
 * @ param angA Angle between - pi / 2 and pi / 2.
 * @ param angB Angle between - pi / 2 and pi / 2.
 * @ return Acute angle between the two input angles . */
public static double distHalf ( double angA , double angB ) { } }

implementation: double a = Math . abs ( angA - angB ) ; if ( a <= Math . PI / 2 ) return a ; else return Math . PI - a ;


signature:
public class UtilAngle { /** * Ensures a reflective bound so that the numbers from 0 to 1 , where 0 is inclusive and 1 is inclusive .
 * < pre > Examples :
 * 1.5 = 0.5
 * - 0.25 = 0.25
 * - 0.75 = 0.75
 * 0 = 0
 * 1 = 1
 * 0.999 = 0.999
 * 2 = 0
 * - 1 = 1
 * < / pre >
 * @ return A value from 0 to 1 . [ 0,1] */
public static double reflectZeroToOne ( double value ) { } }

implementation: if ( value < 0 ) value = - value ; value = value % 2.0 ; if ( value > 1.0 ) return 2.0 - value ; return value ;


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

implementation: double dx = plane . p . x - line . p . x ; double dy = plane . p . y - line . p . y ; double dz = plane . p . z - line . p . z ; double top = dx * plane . n . x + dy * plane . n . y + dz * plane . n . z ; double bottom = line . slope . dot ( plane . n ) ; if ( bottom == 0 ) return false ; double d = top / bottom ; intersection . x = line . p . x + d * line . slope . x ; intersection . y = line . p . y + d * line . slope . y ; intersection . z = line . p . z + d * line . slope . z ; return true ;


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

implementation: double top = plane . D - plane . A * line . p . x - plane . B * line . p . y - plane . C * line . p . z ; double bottom = plane . A * line . slope . x + plane . B * line . slope . y + plane . C * line . slope . z ; if ( bottom == 0 ) return false ; double d = top / bottom ; intersection . x = line . p . x + d * line . slope . x ; intersection . y = line . p . y + d * line . slope . y ; intersection . z = line . p . z + d * line . slope . z ; return true ;


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

implementation:
// 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 ;


signature:
public class Intersection3D_F64 { /** * < p > Finds the intersection between a 3D triangle and a line - segment . Code ported from [ 1 ] . < / p >
 * [ 1 ] http : / / geomalgorithms . com / a06 - _ intersect - 2 . html
 * @ param T ( Input ) Triangle in 3D space
 * @ param R ( Input ) Line segment in 3D space .
 * @ param output ( Output ) Storage for the intersection , if there is one
 * @ return - 1 = triangle is degenerate ( a segment or point ) < br >
 * 0 = disjoint ( no intersect ) < br >
 * 1 = intersect in unique point I1 < br >
 * 2 = are in the same plane */
public static int intersect ( Triangle3D_F64 T , LineSegment3D_F64 R , Point3D_F64 output ) { } }

implementation: return intersect ( T , R , output , new Vector3D_F64 ( ) , new Vector3D_F64 ( ) , new Vector3D_F64 ( ) , new Vector3D_F64 ( ) , new Vector3D_F64 ( ) ) ;


signature:
public class Intersection3D_F64 { /** * < p > Finds the intersection between a 3D triangle and a line - segment . Code ported from [ 1 ] . Internal
 * working variables are provided in this interface to reduce memory creation / destruction . < / p >
 * [ 1 ] http : / / geomalgorithms . com / a06 - _ intersect - 2 . html
 * @ param T ( Input ) Triangle in 3D space
 * @ param R ( Input ) Line segment in 3D space .
 * @ param output ( Output ) Storage for the intersection , if there is one
 * @ param u ( internal use ) triangle vectors
 * @ param v ( internal use ) triangle vectors
 * @ param n ( internal use ) triangle vectors
 * @ param dir ( internal use ) ray vector
 * @ param w0 ( internal use ) ray vector
 * @ return - 1 = triangle is degenerate ( a segment or point )
 * 0 = disjoint ( no intersect )
 * 1 = intersect in unique point I1
 * 2 = are in the same plane */
public static int intersect ( Triangle3D_F64 T , LineSegment3D_F64 R , Point3D_F64 output , Vector3D_F64 u , Vector3D_F64 v , Vector3D_F64 n , Vector3D_F64 dir , Vector3D_F64 w0 ) { } }

implementation:
double r , a , b ; // params to calc ray - plane intersect
// get triangle edge vectors and plane normal
u . minus ( T . v1 , T . v0 ) ; // NOTE : these could be precomputed
v . minus ( T . v2 , T . v0 ) ; n . cross ( u , v ) ; if ( n . normSq ( ) == 0 ) // triangle is degenerate
return - 1 ; // do not deal with this case
dir . minus ( R . b , R . a ) ; // ray direction vector
w0 . minus ( R . a , T . v0 ) ; a = - n . dot ( w0 ) ; b = n . dot ( dir ) ; if ( Math . abs ( b ) < GrlConstants . EPS ) { // ray is parallel to triangle plane
if ( a == 0 ) // ray lies in triangle plane
return 2 ; else return 0 ; // ray disjoint from plane
} // get intersect point of ray with triangle plane
r = a / b ; if ( r < 0.0 ) // ray goes away from triangle
return 0 ; // = > no intersect
else if ( r > 1.0 ) // is past the end of the line segment
return 0 ; // intersect point of ray and plane
output . x = R . a . x + r * dir . x ; output . y = R . a . y + r * dir . y ; output . z = R . a . z + r * dir . z ; // is I inside T ?
if ( containedPlane ( T . v0 , output , u , v , w0 ) ) { return 1 ; } else { return 0 ; }


signature:
public class Intersection3D_F64 { /** * < p > Finds the intersection between a 3D triangle and a line . Code ported from [ 1 ] . Internal
 * working variables are provided in this interface to reduce memory creation / destruction . < / p >
 * [ 1 ] http : / / geomalgorithms . com / a06 - _ intersect - 2 . html
 * @ param T ( Input ) Triangle in 3D space
 * @ param R ( Input ) Line segment in 3D space .
 * @ param output ( Output ) Storage for the intersection , if there is one
 * @ return < ul style = " list - style - type : none " >
 * < li > - 1 = triangle is degenerate ( a segment or point ) < / li >
 * < li > 0 = disjoint ( no intersect ) < / li >
 * < li > 1 = intersect in unique point . Positive direction or zero < / li >
 * < li > 2 = are in the same plane < / li >
 * < li > 3 = intersect in unique point . Negative direction < / li >
 * < / ul > */
public static int intersect ( Triangle3D_F64 T , LineParametric3D_F64 R , Point3D_F64 output ) { } }

implementation: return intersect ( T , R , output , new Vector3D_F64 ( ) , new Vector3D_F64 ( ) , new Vector3D_F64 ( ) , new Vector3D_F64 ( ) ) ;


signature:
public class Intersection3D_F64 { /** * Detects if a 2D convex polygon that has been moved into a 3D world is intersected by the 3D line .
 * < pre >
 * Transformation :
 * 1 ) Each 2D point is converted into 3D : X = ( x , y , 0)
 * 2 ) X ' = R * X + T , where ( R , T ) are a rigid body transform from poly to world frames
 * < / pre >
 * @ param polygon ( Input ) Convex polygon . All points must lie on a plane .
 * @ param line ( Input ) Line .
 * @ param output ( Output ) Point of intersection ( if any )
 * @ param n ( internal use )
 * @ param u ( internal use )
 * @ param v ( internal use )
 * @ param w0 ( internal use )
 * @ return < ul style = " list - style - type : none " >
 * < li > - 1 = triangle is degenerate ( a segment or point ) < / li >
 * < li > 0 = disjoint ( no intersect ) < / li >
 * < li > 1 = intersect in unique point . Positive direction or zero < / li >
 * < li > 2 = are in the same plane < / li >
 * < li > 3 = intersect in unique point . Negative direction < / li >
 * < / ul > */
public static int intersectConvex ( FastQueue < Point3D_F64 > polygon , LineParametric3D_F64 line , Point3D_F64 output , Vector3D_F64 n , Vector3D_F64 u , Vector3D_F64 v , Vector3D_F64 w0 ) { } }

implementation:
if ( polygon . size < 3 ) throw new IllegalArgumentException ( "There must be 3 or more points" ) ; double r , a , b ; // params to calc ray - plane intersect
Point3D_F64 v0 = polygon . get ( 0 ) ; Point3D_F64 v1 = polygon . get ( 1 ) ; Point3D_F64 v2 = polygon . get ( 2 ) ; // get triangle edge vectors and plane normal
u . minus ( v1 , v0 ) ; // NOTE : these could be precomputed
v . minus ( v2 , v0 ) ; n . cross ( u , v ) ; if ( n . normSq ( ) == 0 ) // triangle is degenerate
return - 1 ; // do not deal with this case
Vector3D_F64 dir = line . slope ; w0 . minus ( line . p , v0 ) ; a = - n . dot ( w0 ) ; b = n . dot ( dir ) ; if ( Math . abs ( b ) < GrlConstants . EPS ) { // ray is parallel to triangle plane
if ( a == 0 ) // ray lies in triangle plane
return 2 ; else return 0 ; // ray disjoint from plane
} // get intersect point of ray with triangle plane
r = a / b ; // intersect point of ray and plane
output . x = line . p . x + r * dir . x ; output . y = line . p . y + r * dir . y ; output . z = line . p . z + r * dir . z ; // See if it ' s inside any of the triangles
for ( int i = 2 ; i < polygon . size ; i ++ ) { // is I inside T ?
if ( containedPlane ( v0 , output , u , v , w0 ) ) { if ( r >= 0 ) return 1 ; else return 3 ; } if ( i < polygon . size - 1 ) { u . minus ( polygon . get ( i ) , v0 ) ; v . minus ( polygon . get ( i + 1 ) , v0 ) ; } } return 0 ;


signature:
public class Intersection3D_F64 { /** * Determines if the point on the same plane as T is contained inside of T . */
private static boolean containedPlane ( Point3D_F64 T_v0 , Point3D_F64 output , Vector3D_F64 u , Vector3D_F64 v , Vector3D_F64 w0 ) { } }

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


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

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


signature:
public class Intersection3D_F64 { /** * Returns true if the point is contained inside the box , with an exclusive upper extent .
 * The point is considered to be inside the box if the following test passes : < br >
 * box . p0 . x & le ; point . x { @ code < } box . p1 . x < br >
 * box . p0 . y & le ; point . y { @ code < } box . p1 . y < br >
 * box . p0 . z & le ; point . z { @ code < } box . p1 . z < br >
 * @ param box Box
 * @ param point Point which is tested to see if it is inside the box
 * @ return true for inside and false for not */
public static boolean contained ( Box3D_F64 box , Point3D_F64 point ) { } }

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


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

implementation:
// 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 ;


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

implementation:
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 ) ;


signature:
public class MotionSe3PointCrossCovariance_F64 { /** * The unit eigenvector corresponding to the maximum eigenvalue of Q is the rotation
 * parameterized as a quaternion . */
private void extractQuaternionFromQ ( SimpleMatrix q ) { } }

implementation: 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 ( ) ) ;


signature:
public class AndrewMonotoneConvexHull_F64 { /** * Computes the convex hull . The output will be in counter - clockwise order .
 * @ param input List of input points . The list will be modified by sorting
 * @ param length Number of valid elements in list
 * @ param hull ( Output ) Where the complex hull is written to */
public void process ( Point2D_F64 [ ] input , int length , Polygon2D_F64 hull ) { } }

implementation:
// 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 ) ) ; }


signature:
public class AndrewMonotoneConvexHull_F64 { /** * Performs the following operation : output = z - component [ ( a - b ) cross ( a - c ) ] */
private static double subtractThenCross ( Point2D_F64 a , Point2D_F64 b , Point2D_F64 c ) { } }

implementation: double x0 = b . x - a . x ; double y0 = b . y - a . y ; double x1 = c . x - a . x ; double y1 = c . y - a . y ; return x0 * y1 - y0 * x1 ;


signature:
public class CovarianceToEllipse_F64 { /** * Specifies the covariance matrix . Q = [ a11 a12 ; a12 a22]
 * @ param a11 element in covariance matrix .
 * @ param a12 element in covariance matrix .
 * @ param a22 element in covariance matrix .
 * @ return true if it was successful or false if something went wrong */
public boolean setCovariance ( double a11 , double a12 , double a22 ) { } }

implementation: Q . data [ 0 ] = a11 ; Q . data [ 1 ] = a12 ; Q . data [ 2 ] = a12 ; Q . data [ 3 ] = a22 ; if ( ! eigen . decompose ( Q ) ) { System . err . println ( "Eigenvalue decomposition failed!" ) ; return false ; } Complex_F64 v0 = eigen . getEigenvalue ( 0 ) ; Complex_F64 v1 = eigen . getEigenvalue ( 1 ) ; DMatrixRMaj a0 , a1 ; if ( v0 . getMagnitude2 ( ) > v1 . getMagnitude2 ( ) ) { a0 = eigen . getEigenVector ( 0 ) ; a1 = eigen . getEigenVector ( 1 ) ; lengthX = ( double ) v0 . getMagnitude ( ) ; lengthY = ( double ) v1 . getMagnitude ( ) ; } else { a0 = eigen . getEigenVector ( 1 ) ; a1 = eigen . getEigenVector ( 0 ) ; lengthX = ( double ) v1 . getMagnitude ( ) ; lengthY = ( double ) v0 . getMagnitude ( ) ; } if ( a0 == null || a1 == null ) { System . err . println ( "Complex eigenvalues: " + v0 + "  " + v1 ) ; return false ; } lengthX = Math . sqrt ( lengthX ) ; lengthY = Math . sqrt ( lengthY ) ; x . set ( ( double ) a0 . get ( 0 ) , ( double ) a0 . get ( 1 ) ) ; y . set ( ( double ) a1 . get ( 0 ) , ( double ) a1 . get ( 1 ) ) ; return true ;


signature:
public class SpecialEuclideanOps_F64 { /** * Sets the provided transform so that it does not transform any points .
 * @ param se The transform which is to be set to no motion . */
public static void setToNoMotion ( Se3_F64 se ) { } }

implementation: CommonOps_DDRM . setIdentity ( se . getR ( ) ) ; se . getT ( ) . set ( 0 , 0 , 0 ) ;


signature:
public class SpecialEuclideanOps_F64 { /** * Converts { @ link Se2 _ F64 } into { @ link Affine2D _ F64 } .
 * @ param se ( Input ) Se2
 * @ param affine ( Output ) Equivalent affine . If null a new object will be declared .
 * @ return Equivalent affine . */
public static Affine2D_F64 toAffine ( Se2_F64 se , Affine2D_F64 affine ) { } }

implementation: if ( affine == null ) affine = new Affine2D_F64 ( ) ; affine . a11 = se . c ; affine . a12 = - se . s ; affine . a21 = se . s ; affine . a22 = se . c ; affine . tx = se . T . x ; affine . ty = se . T . y ; return affine ;


signature:
public class SpecialEuclideanOps_F64 { /** * Converts it into a 4 by 4 homogeneous matrix .
 * @ param se original 3D transform
 * @ param ret Where the results will be written to . If null a new matrix is declared . Modified .
 * @ return equivalent homogeneous transform . */
public static DMatrixRMaj toHomogeneous ( Se3_F64 se , DMatrixRMaj ret ) { } }

implementation: 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 ;


signature:
public class SpecialEuclideanOps_F64 { /** * Converts a homogeneous representation into { @ link Se3 _ F64 } .
 * @ param H Homogeneous 4 by 4 matrix .
 * @ param ret If not null where the results are written to .
 * @ return Se3 _ F64 transform . */
public static Se3_F64 toSe3 ( DMatrixRMaj H , Se3_F64 ret ) { } }

implementation: if ( H . numCols != 4 || H . numRows != 4 ) throw new IllegalArgumentException ( "The homogeneous matrix must be 4 by 4 by definition." ) ; if ( ret == null ) ret = new Se3_F64 ( ) ; ret . setTranslation ( ( double ) H . get ( 0 , 3 ) , ( double ) H . get ( 1 , 3 ) , ( double ) H . get ( 2 , 3 ) ) ; CommonOps_DDRM . extract ( H , 0 , 3 , 0 , 3 , ret . getR ( ) , 0 , 0 ) ; return ret ;


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

implementation: 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 ;


signature:
public class SpecialEuclideanOps_F64 { /** * Converts a homogeneous representation into { @ link Se2 _ F64 } .
 * @ param H Homogeneous 3 by 3 matrix .
 * @ param ret If not null where the results are written to .
 * @ return Se3 _ F64 transform . */
public static Se2_F64 toSe2 ( DMatrixRMaj H , Se2_F64 ret ) { } }

implementation: if ( H . numCols != 3 || H . numRows != 3 ) throw new IllegalArgumentException ( "The homogeneous matrix must be 3 by 3 by definition." ) ; if ( ret == null ) ret = new Se2_F64 ( ) ; ret . setTranslation ( ( double ) H . get ( 0 , 2 ) , ( double ) H . get ( 1 , 2 ) ) ; double c = ( double ) H . get ( 0 , 0 ) ; double s = ( double ) H . get ( 1 , 0 ) ; ret . setYaw ( Math . atan2 ( s , c ) ) ; return ret ;


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

implementation: return eulerXyz ( dx , dy , dz , EulerType . XYZ , rotX , rotY , rotZ , se ) ;


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

implementation: 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 ;


signature:
public class SpecialEuclideanOps_F64 { /** * Can be used to see if two transforms are identical to within tolerance
 * @ param a transform
 * @ param b tranform
 * @ param tolT Tolerance for translation
 * @ param tolR Tolerance for rotation in radians
 * @ return true if identical or false if not */
public static boolean isIdentical ( Se3_F64 a , Se3_F64 b , double tolT , double tolR ) { } }

implementation: 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 ;


signature:
public class SpecialEuclideanOps_F64 { /** * Finds the best fit projection of ' a ' onto SE ( 3 ) . This is useful when a was estimated using a linear algorithm .
 * < p > See page 280 of " An Invitation to 3 - D Vision , From Images to Geometric Models " 1st Ed . 2004 . Springer . < / p >
 * @ param a Approximate SE ( 3 ) . Modified .
 * @ return true if successful */
public static boolean bestFit ( Se3_F64 a ) { } }

implementation:
SingularValueDecomposition_F64 < DMatrixRMaj > svd = DecompositionFactory_DDRM . svd ( true , true , true ) ; if ( ! svd . decompose ( a . R ) ) throw new RuntimeException ( "SVD Failed" ) ; CommonOps_DDRM . multTransB ( svd . getU ( null , false ) , svd . getV ( null , false ) , a . R ) ; // determinant should be + 1
double det = CommonOps_DDRM . det ( a . R ) ; if ( det < 0 ) { CommonOps_DDRM . scale ( - 1 , a . R ) ; } // compute the determinant of the singular matrix
double b = 1.0 ; double s [ ] = svd . getSingularValues ( ) ; for ( int i = 0 ; i < svd . numberOfSingularValues ( ) ; i ++ ) { b *= s [ i ] ; } b = Math . signum ( det ) / Math . pow ( b , 1.0 / 3.0 ) ; GeometryMath_F64 . scale ( a . T , b ) ; return true ;


signature:
public class Distance2D_I32 { /** * Returns the distance the closest point on a line segment is from the specified point .
 * The closest point is bounded to be along the line segment .
 * @ param line A line segment . Not modified .
 * @ param p The point . Not modified .
 * @ return Distance the closest point on the line is away from the point . */
public static double distance ( LineSegment2D_I32 line , Point2D_I32 p ) { } }

implementation:
int a = line . b . x - line . a . x ; int b = line . b . y - line . a . y ; double t = a * ( p . x - line . a . x ) + b * ( p . y - line . a . y ) ; t /= ( a * a + b * b ) ; // if the point of intersection is past the end points return the distance
// from the closest end point
if ( t < 0 ) { return UtilPoint2D_I32 . distance ( line . a . x , line . a . y , p . x , p . y ) ; } else if ( t > 1.0 ) return UtilPoint2D_I32 . distance ( line . b . x , line . b . y , p . x , p . y ) ; // return the distance of the closest point on the line
return UtilPoint2D_F64 . distance ( line . a . x + t * a , line . a . y + t * b , p . x , p . y ) ;


signature:
public class Distance2D_I32 { /** * Returns the distance of a point to the closest point on a line .
 * @ param line A line . Not modified .
 * @ param p The point . Not modified .
 * @ return Distance the closest point on the line is away from the point . */
public static double distance ( LineParametric2D_I32 line , Point2D_I32 p ) { } }

implementation:
int a = line . slopeX ; int b = line . slopeY ; double t = a * ( p . x - line . p . x ) + b * ( p . y - line . p . y ) ; t /= ( a * a + b * b ) ; // return the distance of the closest point on the line
return UtilPoint2D_F64 . distance ( line . p . x + t * a , line . p . y + t * b , p . x , p . y ) ;


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

implementation: 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 ;


signature:
public class UtilCircle2D_F64 { /** * Computes ( x - x _ c ) * * 2 + ( y - y _ c ) * * 2 - r . If ( x , y ) lies on the circle then it should be 0.
 * @ param x x - coordinate of a point
 * @ param y y - coordinate of a point
 * @ param circle circle
 * @ return Result of the equation */
public static double evaluate ( double x , double y , Circle2D_F64 circle ) { } }

implementation: x -= circle . center . x ; y -= circle . center . y ; return x * x + y * y - circle . radius * circle . radius ;


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

implementation:
// 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 ;


signature:
public class UtilCircle2D_F64 { /** * Radius squares of the circle that passes through these three points .
 * @ param x0 Point
 * @ param x1 Point
 * @ param x2 Point
 * @ return Radius squares of circle or NaN if colinear */
public static double circleRadiusSq ( Point2D_F64 x0 , Point2D_F64 x1 , Point2D_F64 x2 ) { } }

implementation:
// 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 ;


signature:
public class ClosestPoint3D_F64 { /** * Returns the point which minimizes the distance between the two lines in 3D . If the
 * two lines are parallel the result is undefined .
 * @ param l0 first line . Not modified .
 * @ param l1 second line . Not modified .
 * @ param ret ( Optional ) Storage for the closest point . If null a new point is declared . Modified .
 * @ return Closest point between two lines . */
public static Point3D_F64 closestPoint ( LineParametric3D_F64 l0 , LineParametric3D_F64 l1 , Point3D_F64 ret ) { } }

implementation:
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 ;


signature:
public class ClosestPoint3D_F64 { /** * Finds the closest point on line lo to l1 and on l1 to l0 . The solution is returned in
 * ' param ' as a value of ' t ' for each line .
 * point on l0 = l0 . a + param [ 0 ] * l0 . slope < br >
 * point on l1 = l1 . a + param [ 1 ] * l1 . slope
 * @ param l0 first line . Not modified .
 * @ param l1 second line . Not modified .
 * @ param param param [ 0 ] for line0 location and param [ 1 ] for line1 location .
 * @ return False if the lines are parallel or true if a solution was found . */
public static boolean closestPoints ( LineParametric3D_F64 l0 , LineParametric3D_F64 l1 , double param [ ] ) { } }

implementation:
double dX = l0 . p . x - l1 . p . x ; double dY = l0 . p . y - l1 . p . y ; double dZ = l0 . p . z - l1 . p . z ; // this solution is from : http : / / local . wasp . uwa . edu . au / ~ pbourke / geometry / lineline3d /
double dv01v1 = MiscOps . dot ( dX , dY , dZ , l1 . slope ) ; double dv1v0 = MiscOps . dot ( l1 . slope , l0 . slope ) ; double dv1v1 = MiscOps . dot ( l1 . slope , l1 . slope ) ; double t0 = dv01v1 * dv1v0 - MiscOps . dot ( dX , dY , dZ , l0 . slope ) * dv1v1 ; double bottom = MiscOps . dot ( l0 . slope , l0 . slope ) * dv1v1 - dv1v0 * dv1v0 ; if ( bottom == 0 ) return false ; t0 /= bottom ; // ( d1343 + mua d4321 ) / d4343
double t1 = ( dv01v1 + t0 * dv1v0 ) / dv1v1 ; param [ 0 ] = t0 ; param [ 1 ] = t1 ; return true ;


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

implementation: 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 ;


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

implementation: double dx = pt . x - line . p . x ; double dy = pt . y - line . p . y ; double dz = pt . z - line . p . z ; return ( line . slope . x * dx + line . slope . y * dy + line . slope . z * dz ) / line . slope . normSq ( ) ;


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

implementation: if ( found == null ) found = new Point3D_F64 ( ) ; double A = plane . n . x ; double B = plane . n . y ; double C = plane . n . z ; double D = plane . n . x * plane . p . x + plane . n . y * plane . p . y + plane . n . z * plane . p . z ; double top = A * point . x + B * point . y + C * point . z - D ; double n2 = A * A + B * B + C * C ; found . x = point . x - A * top / n2 ; found . y = point . y - B * top / n2 ; found . z = point . z - C * top / n2 ; return found ;


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

implementation: if ( found == null ) found = new Point3D_F64 ( ) ; double top = plane . A * point . x + plane . B * point . y + plane . C * point . z - plane . D ; double n2 = plane . A * plane . A + plane . B * plane . B + plane . C * plane . C ; found . x = point . x - plane . A * top / n2 ; found . y = point . y - plane . B * top / n2 ; found . z = point . z - plane . C * top / n2 ; return found ;


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

implementation: 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 ;


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

implementation:
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 ;


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

implementation:
if ( ret == null ) { ret = new Point3D_F64 ( ) ; } ret . x = l0 . a . x - l1 . a . x ; ret . y = l0 . a . y - l1 . a . y ; ret . z = l0 . a . z - l1 . a . z ; double slope0_x = l0 . b . x - l0 . a . x ; double slope0_y = l0 . b . y - l0 . a . y ; double slope0_z = l0 . b . z - l0 . a . z ; double slope1_x = l1 . b . x - l1 . a . x ; double slope1_y = l1 . b . y - l1 . a . y ; double slope1_z = l1 . b . z - l1 . a . z ; // normalize the slopes for easier math
double n0 = ( double ) Math . sqrt ( slope0_x * slope0_x + slope0_y * slope0_y + slope0_z * slope0_z ) ; double n1 = ( double ) Math . sqrt ( slope1_x * slope1_x + slope1_y * slope1_y + slope1_z * slope1_z ) ; slope0_x /= n0 ; slope0_y /= n0 ; slope0_z /= n0 ; slope1_x /= n1 ; slope1_y /= n1 ; slope1_z /= n1 ; // this solution is from : http : / / local . wasp . uwa . edu . au / ~ pbourke / geometry / lineline3d /
double dv01v1 = ret . x * slope1_x + ret . y * slope1_y + ret . z * slope1_z ; double dv01v0 = ret . x * slope0_x + ret . y * slope0_y + ret . z * slope0_z ; double dv1v0 = slope1_x * slope0_x + slope1_y * slope0_y + slope1_z * slope0_z ; double t0 = dv01v1 * dv1v0 - dv01v0 ; double bottom = 1 - dv1v0 * dv1v0 ; if ( bottom == 0 ) return null ; t0 /= bottom ; // restrict it to be on the line
if ( t0 < 0 ) return closestPoint ( l1 , l0 . a , ret ) ; if ( t0 > 1 ) return closestPoint ( l1 , l0 . b , ret ) ; // ( d1343 + mua d4321 ) / d4343
double t1 = ( dv01v1 + t0 * dv1v0 ) ; if ( t1 < 0 ) return closestPoint ( l0 , l1 . a , ret ) ; if ( t1 > 1 ) return closestPoint ( l0 , l1 . b , ret ) ; ret . x = ( double ) 0.5 * ( ( l0 . a . x + t0 * slope0_x ) + ( l1 . a . x + t1 * slope1_x ) ) ; ret . y = ( double ) 0.5 * ( ( l0 . a . y + t0 * slope0_y ) + ( l1 . a . y + t1 * slope1_y ) ) ; ret . z = ( double ) 0.5 * ( ( l0 . a . z + t0 * slope0_z ) + ( l1 . a . z + t1 * slope1_z ) ) ; return ret ;


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

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


signature:
public class ClosestPoint3D_F64 { /** * Computes the closest point along the line to the plane as a function of ' t ' : < br >
 * [ x , y , z ] = [ x _ 0 , y _ 0 , z _ 0 ] + t · [ slopeX , slopeY , slopZ ]
 * If there is no intersection then Double . NaN is returned .
 * @ param line The line along which the closest point is being found . Not modified .
 * @ param plane Plane being checked for intersection
 * @ return Distance as a function of ' t ' . NaN if there is no intersection . */
public static double closestPointT ( LineParametric3D_F64 line , PlaneNormal3D_F64 plane ) { } }

implementation: double dx = plane . p . x - line . p . x ; double dy = plane . p . y - line . p . y ; double dz = plane . p . z - line . p . z ; double top = dx * plane . n . x + dy * plane . n . y + dz * plane . n . z ; double bottom = line . slope . dot ( plane . n ) ; if ( bottom == 0 ) return Double . NaN ; return top / bottom ;


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

implementation:
if ( result == null ) { result = new Point2D_F64 ( ) ; } // copy the values so that no errors happen if orig and result are the same instance
double x = orig . x ; double y = orig . y ; double z = H . a31 * x + H . a32 * y + H . a33 ; result . x = ( H . a11 * x + H . a12 * y + H . a13 ) / z ; result . y = ( H . a21 * x + H . a22 * y + H . a23 ) / z ; return result ;


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

implementation:
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 ;


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

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


signature:
public class SePointOps_F64 { /** * Applies a 3D special euclidean transform to a list of points .
 * @ param se The transform .
 * @ param points List of points which are to be transformed . Modified . */
public static void transform ( Se3_F64 se , Point3D_F64 [ ] points , int start , int length ) { } }

implementation: for ( int i = 0 ; i < length ; i ++ ) { Point3D_F64 p = points [ i + start ] ; transform ( se , p , p ) ; }


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

implementation: for ( Point3D_F64 p : points ) { transform ( se , p , p ) ; }


signature:
public class SePointOps_F64 { /** * Applies the transform specified by SpecialEuclidean to a point . < br >
 * < br >
 * p ' = R * p + T
 * Both origPt and tranPt can be the same instance .
 * @ param se SpecialEuclidean transform . Not modified .
 * @ param origPt Original coordinate of the point . Not modified .
 * @ param tranPt Storage for transformed coordinate of the point . Point declared if null . Modified .
 * @ return Transformed point . */
public static Point3D_F64 transform ( Se3_F64 se , Point3D_F64 origPt , Point3D_F64 tranPt ) { } }

implementation: if ( tranPt == null ) tranPt = new Point3D_F64 ( ) ; DMatrixRMaj R = se . getR ( ) ; Vector3D_F64 T = se . getT ( ) ; GeometryMath_F64 . mult ( R , origPt , tranPt ) ; GeometryMath_F64 . add ( tranPt , T , tranPt ) ; return tranPt ;


signature:
public class SePointOps_F64 { /** * Applies the transform specified by SpecialEuclidean to a homogenous point . < br >
 * < br >
 * p ' = [ R t ] * p
 * Both origPt and tranPt can be the same instance .
 * @ param se SpecialEuclidean transform . Not modified .
 * @ param origPt Original coordinate of the point . Not modified .
 * @ param tranPt Storage for transformed coordinate of the point . Point declared if null . Modified .
 * @ return Transformed point . */
public static Point3D_F64 transform ( Se3_F64 se , Point4D_F64 origPt , Point3D_F64 tranPt ) { } }

implementation: if ( tranPt == null ) tranPt = new Point3D_F64 ( ) ; DMatrixRMaj R = se . getR ( ) ; Vector3D_F64 T = se . getT ( ) ; double P11 = R . data [ 0 ] , P12 = R . data [ 1 ] , P13 = R . data [ 2 ] , P14 = T . x ; double P21 = R . data [ 3 ] , P22 = R . data [ 4 ] , P23 = R . data [ 5 ] , P24 = T . y ; double P31 = R . data [ 6 ] , P32 = R . data [ 7 ] , P33 = R . data [ 8 ] , P34 = T . z ; tranPt . x = P11 * origPt . x + P12 * origPt . y + P13 * origPt . z + P14 * origPt . w ; tranPt . y = P21 * origPt . x + P22 * origPt . y + P23 * origPt . z + P24 * origPt . w ; tranPt . z = P31 * origPt . x + P32 * origPt . y + P33 * origPt . z + P34 * origPt . w ; return tranPt ;


signature:
public class SePointOps_F64 { /** * Applies the transform in the reverse direction < br >
 * < br >
 * p = R < sup > T < / sup > * ( p ' - T )
 * Both origPt and tranPt can be the same instance .
 * @ param se SpecialEuclidean transform .
 * @ param origPt Original coordinate of the point .
 * @ param tranPt Transformed coordinate of the point . */
public static Point3D_F64 transformReverse ( Se3_F64 se , Point3D_F64 origPt , Point3D_F64 tranPt ) { } }

implementation: if ( tranPt == null ) tranPt = new Point3D_F64 ( ) ; DMatrixRMaj R = se . getR ( ) ; Vector3D_F64 T = se . getT ( ) ; GeometryMath_F64 . sub ( origPt , T , tranPt ) ; GeometryMath_F64 . multTran ( R , tranPt , tranPt ) ; return tranPt ;


signature:
public class Quadrilateral_F64 { /** * Converts the polygon into a list .
 * @ param storage ( Optional ) storage to put the vertexes into
 * @ param copy If points will be copied otherwise a reference of points will be returned
 * @ return List of vertexes */
public List < Point2D_F64 > convert ( @ Nullable List < Point2D_F64 > storage , boolean copy ) { } }

implementation: if ( storage == null ) storage = new ArrayList < > ( ) ; else storage . clear ( ) ; if ( copy ) { for ( int i = 0 ; i < 4 ; i ++ ) { storage . add ( get ( i ) . copy ( ) ) ; } } else { for ( int i = 0 ; i < 4 ; i ++ ) { storage . add ( get ( i ) ) ; } } return storage ;


signature:
public class Quadrilateral_F64 { /** * Sets the polygon to be the same as the list . A true copy is created and no references
 * to points in the list are saved .
 * @ param list List which the polygon will be set to */
public void set ( List < Point2D_F64 > list ) { } }

implementation: if ( list . size ( ) != 4 ) throw new IllegalArgumentException ( "List must have size of 4" ) ; a . set ( list . get ( 0 ) ) ; b . set ( list . get ( 1 ) ) ; c . set ( list . get ( 2 ) ) ; d . set ( list . get ( 3 ) ) ;


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

implementation: 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 ;


signature:
public class UtilLine2D_F64 { /** * Returns the acute angle between the slope of two lines . Lines do not need to ever
 * intersect . Found using the dot product .
 * @ param a ( input ) line
 * @ param b ( input ) line
 * @ return acute angle in radians */
public static double acuteAngle ( LineGeneral2D_F64 a , LineGeneral2D_F64 b ) { } }

implementation:
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 ) ;


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

implementation: double value = a . A * b . A + a . B * b . B ; if ( value < - 1.0 ) value = - 1.0 ; else if ( value > 1.0 ) value = 1.0 ; return Math . acos ( value ) ;


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

implementation: 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 ;


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

implementation: if ( ret == null ) ret = new LineGeneral2D_F64 ( ) ; double c = ( double ) Math . cos ( src . angle ) ; double s = ( double ) Math . sin ( src . angle ) ; ret . A = c ; ret . B = s ; ret . C = - src . distance ; return ret ;


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

implementation: 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 ;


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

implementation: if ( ret == null ) ret = new LineParametric2D_F64 ( ) ; ret . p . set ( src . a ) ; ret . slope . set ( src . slopeX ( ) , src . slopeY ( ) ) ; return ret ;


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

implementation: return convert ( src . a , src . b , ret ) ;


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

implementation: 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 ;


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

implementation: 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 ;


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

implementation: 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 ;


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

implementation: 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 ;


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

implementation:
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 ;


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

implementation: 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 ;


signature:
public class UtilPlane3D_F64 { /** * Converts a plane in general form into normal form . The point on the plane in normal form will be the
 * closest point to the origin .
 * < p > NOTE : The normal is not normalized . < / p >
 * @ param input Plane in general form .
 * @ param output ( Optional ) Storage for output plane in normal form . If null a new instance will be declared .
 * @ return The plane in normal form . */
public static PlaneNormal3D_F64 convert ( PlaneGeneral3D_F64 input , PlaneNormal3D_F64 output ) { } }

implementation: if ( output == null ) output = new PlaneNormal3D_F64 ( ) ; double top = - input . D ; double n2 = input . A * input . A + input . B * input . B + input . C * input . C ; output . p . x = - input . A * top / n2 ; output . p . y = - input . B * top / n2 ; output . p . z = - input . C * top / n2 ; output . n . set ( input . A , input . B , input . C ) ; return output ;


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

implementation:
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 ;


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

implementation:
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 ;


signature:
public class UtilPlane3D_F64 { /** * Converts the plane into Hessian normal form . This is done by dividing each coefficient by the
 * Euclidean norm of ( A , B , C ) .
 * @ param plane The input plane . Modified . */
public static void hessianNormalForm ( PlaneGeneral3D_F64 plane ) { } }

implementation: double n = Math . sqrt ( plane . A * plane . A + plane . B * plane . B + plane . C * plane . C ) ; plane . A /= n ; plane . B /= n ; plane . C /= n ; plane . D /= n ;


signature:
public class UtilPlane3D_F64 { /** * Applies the plane ' s definition to test to see if a point is one the plane
 * @ param plane Equation of the plane
 * @ param point Equation of the point .
 * @ return zero if the point is one the plane . */
public static double evaluate ( PlaneGeneral3D_F64 plane , Point3D_F64 point ) { } }

implementation: return plane . A * point . x + plane . B * point . y + plane . C * point . z - plane . D ;


signature:
public class UtilPlane3D_F64 { /** * Applies the plane ' s definition to test to see if a point is one the plane
 * @ param plane Equation of the plane
 * @ param point Equation of the point .
 * @ return zero if the point is one the plane . */
public static double evaluate ( PlaneNormal3D_F64 plane , Point3D_F64 point ) { } }

implementation: double dx = point . x - plane . p . x ; double dy = point . y - plane . p . y ; double dz = point . z - plane . p . z ; return plane . n . x * dx + plane . n . y * dy + plane . n . z * dz ;


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

implementation: UtilVector3D_F64 . perpendicularCanonical ( normal , axisX ) ; axisX . normalize ( ) ; axisY . cross ( normal , axisX ) ; axisY . normalize ( ) ;


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

implementation: double x = A . x - pointOnPlane . x ; double y = A . y - pointOnPlane . y ; double z = A . z - pointOnPlane . z ; output . x = x * axisX . x + y * axisX . y + z * axisX . z ; output . y = x * axisY . x + y * axisY . y + z * axisY . z ;


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

implementation: 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 ;


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

implementation: 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 ) ;


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

implementation: 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 ;


signature:
public class GeoTuple_F64 { /** * Generic copy routine . It is recommended that this be overridden with a faster implementation .
 * @ return An exact copy of this GeoTuple . */
@ Override public T copy ( ) { } }

implementation: T ret = createNewInstance ( ) ; int N = getDimension ( ) ; for ( int i = 0 ; i < N ; i ++ ) { ret . setIdx ( i , getIdx ( i ) ) ; } return ret ;


signature:
public class GeoTuple_F64 { /** * Computes the square of the Euclidean norm .
 * @ return norm squared . */
public double normSq ( ) { } }

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


signature:
public class UtilLine3D_F64 { /** * Converts a { @ link LineSegment3D _ F64 } into { @ link LineParametric3D _ F64 } .
 * @ param line Line segment
 * @ param output Storage for converted line . If null new line will be declared .
 * @ return Line in parametric format */
public static LineParametric3D_F64 convert ( LineSegment3D_F64 line , LineParametric3D_F64 output ) { } }

implementation: if ( output == null ) output = new LineParametric3D_F64 ( ) ; output . p . set ( line . a ) ; output . slope . x = line . b . x - line . a . x ; output . slope . y = line . b . y - line . a . y ; output . slope . z = line . b . z - line . a . z ; return output ;


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

implementation: 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 ;


signature:
public class ParabolaGeneral_F64 { /** * Returns true if any of its parameters have an uncountable number */
public boolean hasUncountable ( ) { } }

implementation: return UtilEjml . isUncountable ( A ) || UtilEjml . isUncountable ( C ) || UtilEjml . isUncountable ( D ) || UtilEjml . isUncountable ( E ) || UtilEjml . isUncountable ( F ) ;


signature:
public class ParabolaGeneral_F64 { /** * Determines if they are equivalent up to a scale factor */
public boolean isEquivalent ( ParabolaGeneral_F64 parabola , double tol ) { } }

implementation: 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 ;


signature:
public class FitLine_I32 { /** * Computes the unweighted best fit line to the set of points using the polar line equation . The solution
 * is optimal in the Euclidean sense , see [ 1 ] for more details .
 * [ 1 ] K . Arras , R . Siegwart , " Feature Extraction and Scene Interpretation for Map - Based Navigation
 * and Map Building " Proc . SPIE , Mobile Robotics XIII , Vol . 3210 , 1997
 * @ param points Set of points on the line .
 * @ param start First index in the list
 * @ param length number of variables used in the list
 * @ param ret Storage for the line . If null a new line will be declared .
 * @ return Best fit line */
public static LinePolar2D_F32 polar ( List < Point2D_I32 > points , int start , int length , LinePolar2D_F32 ret ) { } }

implementation: if ( ret == null ) ret = new LinePolar2D_F32 ( ) ; int sumX = 0 ; int sumY = 0 ; final int N = length ; for ( int i = 0 ; i < N ; i ++ ) { Point2D_I32 p = points . get ( start + i ) ; sumX += p . x ; sumY += p . y ; } float meanX = sumX / ( float ) N ; float meanY = sumY / ( float ) N ; float top = 0 ; float bottom = 0 ; for ( int i = 0 ; i < N ; i ++ ) { Point2D_I32 p = points . get ( start + i ) ; float dx = meanX - p . x ; float dy = meanY - p . y ; top += dx * dy ; bottom += dy * dy - dx * dx ; } ret . angle = ( float ) Math . atan2 ( - 2.0f * top , bottom ) / 2.0f ; ret . distance = ( float ) ( meanX * Math . cos ( ret . angle ) + meanY * Math . sin ( ret . angle ) ) ; return ret ;


signature:
public class Box3D_F64 { /** * The box ' s area . area = lengthX * lengthY * lengthZ
 * @ return area */
public double area ( ) { } }


signature:
public class Box3D_F64 { /** * Computes and return the center of the cube .
 * @ param storage Optional storage for the center . If null a new instance will be created and returned .
 * @ return The cube ' s center point */
public Point3D_F64 center ( Point3D_F64 storage ) { } }

implementation: 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 ;


signature:
public class UtilLine2D_I32 { /** * Computes the acute angle between the two lines . Does not check for intersection
 * @ param line0 First line
 * @ param line1 Second line
 * @ return Acute angle in radians */
public static double acuteAngle ( LineSegment2D_I32 line0 , LineSegment2D_I32 line1 ) { } }

implementation: 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 ) ;


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

implementation: MotionTransformPoint < Se2_F64 , Point2D_F64 > alg = fitPoints2D ( ) ; alg . process ( from , to ) ; return alg . getTransformSrcToDst ( ) ;


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

implementation: MotionTransformPoint < Se3_F64 , Point3D_F64 > alg = fitPoints3D ( ) ; alg . process ( from , to ) ; return alg . getTransformSrcToDst ( ) ;


signature:
public class UtilPoint4D_F64 { /** * Checks to see if the homogenous 3D point lies on the plane at infinity
 * @ param p ( Input ) Homogenous point
 * @ param tol ( Input ) tolerance . Try EPS
 * @ return true if on plane at infinity */
public static boolean isInfiniteH ( Point4D_F64 p , double tol ) { } }

implementation: double n = Math . sqrt ( p . x * p . x + p . y * p . y + p . z * p . z ) ; return Math . abs ( p . w ) <= n * tol ;


signature:
public class UtilPoint4D_F64 { /** * Normally distributed homogenous 3D point . w is fixed */
public static List < Point4D_F64 > randomN ( Point3D_F64 center , double w , double stdev , int num , Random rand ) { } }

implementation: 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 ;


signature:
public class UtilPoint4D_F64 { /** * Converts a point from homogenous coordinates into Euclidean
 * @ param p 3D point in homogenous coordinates
 * @ return 3D point */
public static Point3D_F64 h_to_e ( Point4D_F64 p ) { } }

implementation: Point3D_F64 out = new Point3D_F64 ( ) ; h_to_e ( p , out ) ; return out ;


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

implementation:
if ( result == null ) { result = new Vector2D_F64 ( ) ; } // copy the values so that no errors happen if orig and result are the same instance
double x = orig . x ; double y = orig . y ; result . x = se . a11 * x + se . a12 * y ; result . y = se . a21 * x + se . a22 * y ; return result ;


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

implementation:
if ( result == null ) { result = new Point2D_F64 ( ) ; } // copy the values so that no errors happen if orig and result are the same instance
double x = orig . x ; double y = orig . y ; result . x = se . tx + se . a11 * x + se . a12 * y ; result . y = se . ty + se . a21 * x + se . a22 * y ; return result ;


signature:
public class UtilVector2D_F64 { /** * Returns the acute angle between the two vectors . Computed using the dot product .
 * @ param a vector
 * @ param b vector
 * @ return acute angle */
public static double acute ( Vector2D_F64 a , Vector2D_F64 b ) { } }

implementation: double dot = a . dot ( b ) ; double value = dot / ( a . norm ( ) * b . norm ( ) ) ; if ( value > 1.0 ) value = 1.0 ; else if ( value < - 1.0 ) value = - 1.0 ; return Math . acos ( value ) ;


signature:
public class UtilVector2D_F64 { /** * Sets the vector equal to ' a ' - ' b ' .
 * @ param a point
 * @ param b point
 * @ param output ( output ) optional storage for vector .
 * @ return Solution */
public static Vector2D_F64 minus ( Point2D_F64 a , Point2D_F64 b , Vector2D_F64 output ) { } }

implementation: if ( output == null ) output = new Vector2D_F64 ( ) ; output . x = a . x - b . x ; output . y = a . y - b . y ; return output ;


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

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


signature:
public class RectangleLength2D_F64 { /** * Sets this rectangle to be equal to the passed in rectangle .
 * @ param r Rectangle which this is to be set equal to */
public void set ( RectangleLength2D_I32 r ) { } }

implementation: this . x0 = r . x0 ; this . y0 = r . y0 ; this . width = r . width ; this . height = r . height ;


signature:
public class Vector3D_F64 { /** * In - place minus operation . this = a - b .
 * @ param a Point
 * @ param b Point */
public void minus ( Point3D_F64 a , Point3D_F64 b ) { } }

implementation: x = a . x - b . x ; y = a . y - b . y ; z = a . z - b . z ;


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

implementation: return x * a . x + y * a . y + z * a . z ;


signature:
public class UtilEllipse_F64 { /** * Convert from quadratic to rotated formats . Equations taken from [ 1 ] .
 * [ 1 ] David Eberly " Information About Ellipses " , Geometric Tools , LLC 2011
 * @ param input Input in quadratic format .
 * @ param output ( Optional ) Storage for converted format . Can be null .
 * @ return Ellipse in rotated format . */
public static EllipseRotated_F64 convert ( EllipseQuadratic_F64 input , EllipseRotated_F64 output ) { } }

implementation:
if ( output == null ) output = new EllipseRotated_F64 ( ) ; double a11 = input . A ; double a12 = input . B ; double a22 = input . C ; double b1 = 2 * input . D ; double b2 = 2 * input . E ; double c = input . F ; output . center . x = ( a22 * b1 - a12 * b2 ) / ( 2 * ( a12 * a12 - a11 * a22 ) ) ; output . center . y = ( a11 * b2 - a12 * b1 ) / ( 2 * ( a12 * a12 - a11 * a22 ) ) ; double k1 = output . center . x ; double k2 = output . center . y ; double mu = 1.0 / ( a11 * k1 * k1 + 2 * a12 * k1 * k2 + a22 * k2 * k2 - c ) ; double m11 = mu * a11 ; double m12 = mu * a12 ; double m22 = mu * a22 ; double inner = Math . sqrt ( ( m11 - m22 ) * ( m11 - m22 ) + 4 * m12 * m12 ) ; double l1 = ( ( m11 + m22 ) + inner ) / 2.0 ; double l2 = ( ( m11 + m22 ) - inner ) / 2.0 ; output . b = 1 / ( double ) Math . sqrt ( l1 ) ; output . a = 1 / ( double ) Math . sqrt ( l2 ) ; // direction of minor axis
double dx , dy ; if ( m11 >= m22 ) { dx = l1 - m22 ; dy = m12 ; } else { dx = m12 ; dy = l1 - m11 ; } // direction of major axis
output . phi = Math . atan2 ( - dx , dy ) ; if ( output . phi < - GrlConstants . PId2 ) { output . phi += ( double ) Math . PI ; } else if ( output . phi > GrlConstants . PId2 ) { output . phi -= ( double ) Math . PI ; } return output ;


signature:
public class UtilEllipse_F64 { /** * Convert from rotated to quadratic .
 * @ param input Input rotated format .
 * @ param output ( Optional ) Storage for quadratic format . Can be null .
 * @ return Ellipse in quadratic format . */
public static EllipseQuadratic_F64 convert ( EllipseRotated_F64 input , EllipseQuadratic_F64 output ) { } }

implementation:
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 ;


signature:
public class UtilEllipse_F64 { /** * Computes the value of the quadratic ellipse function at point ( x , y ) . Should equal 0 if the point
 * is along the ellipse .
 * @ param x x - coordinate
 * @ param y y - coordinate
 * @ param ellipse Ellipse equation being evaluated .
 * @ return value of ellipse equation at point ( x , y ) */
public static double evaluate ( double x , double y , EllipseQuadratic_F64 ellipse ) { } }

implementation: return ellipse . A * x * x + 2 * ellipse . B * x * y + ellipse . C * y * y + 2 * ellipse . D * x + 2 * ellipse . E * y + ellipse . F ;