Input:
 Computes the exponential map for a twist :
 exp ( hat ( xi ) * theta ) = [ SO ( I - SO ) * ( w cross v ) + w * w T v&Theta ; 0 1 ]
 SO = exp ( hat ( w ) * theta )
 [CODESPLIT] public static Se3_F64 exponential ( TwistCoordinate_F64 twist , double theta , Se3_F64 motion ) { if ( motion == null ) { motion = new Se3_F64 ( ) ; } double w_norm = twist . w . norm ( ) ; if ( w_norm == 0.0 ) { CommonOps_DDRM . setIdentity ( motion . R ) ; motion . T . x = twist . v . x * theta ; motion . T . y = twist . v . y * theta ; motion . T . z = twist . v . z * theta ; return motion ; } DMatrixRMaj R = motion . getR ( ) ; // First handle the SO region.  This Rodrigues equation double wx = twist . w . x / w_norm , wy = twist . w . y / w_norm , wz = twist . w . z / w_norm ; ConvertRotation3D_F64 . rodriguesToMatrix ( wx , wy , wz , theta * w_norm , R ) ; theta *= w_norm ; // Now compute the translational component // (I - SO)*(w cross v) + w*w'*v*theta double vx = twist . v . x , vy = twist . v . y , vz = twist . v . z ; double wv_x = wy * vz - wz * vy ; double wv_y = wz * vx - wx * vz ; double wv_z = wx * vy - wy * vx ; double left_x = ( 1 - R . data [ 0 ] ) * wv_x - R . data [ 1 ] * wv_y - R . data [ 2 ] * wv_z ; double left_y = - R . data [ 3 ] * wv_x + ( 1 - R . data [ 4 ] ) * wv_y - R . data [ 5 ] * wv_z ; double left_z = - R . data [ 6 ] * wv_x - R . data [ 7 ] * wv_y + ( 1 - R . data [ 8 ] ) * wv_z ; double right_x = ( wx * wx * vx + wx * wy * vy + wx * wz * vz ) * theta ; double right_y = ( wy * wx * vx + wy * wy * vy + wy * wz * vz ) * theta ; double right_z = ( wz * wx * vx + wz * wy * vy + wz * wz * vz ) * theta ; motion . T . x = ( double ) left_x + right_x ; motion . T . y = ( double ) left_y + right_y ; motion . T . z = ( double ) left_z + right_z ; motion . T . divide ( w_norm ) ; return motion ; }


Input: Converts a rigid body motion into a twist coordinate . The value of theta used to generate the motion is assumed to be one . [CODESPLIT] public static TwistCoordinate_F64 twist ( Se3_F64 motion , TwistCoordinate_F64 twist ) { if ( twist == null ) twist = new TwistCoordinate_F64 ( ) ; if ( MatrixFeatures_DDRM . isIdentity ( motion . R , GrlConstants . TEST_F64 ) ) { twist . w . set ( 0 , 0 , 0 ) ; twist . v . set ( motion . T ) ; } else { Rodrigues_F64 rod = new Rodrigues_F64 ( ) ; ConvertRotation3D_F64 . matrixToRodrigues ( motion . R , rod ) ; twist . w . set ( rod . unitAxisRotation ) ; double theta = rod . theta ; // A = (I-SO)*hat(w) + w*w'*theta DMatrixRMaj A = CommonOps_DDRM . identity ( 3 ) ; CommonOps_DDRM . subtract ( A , motion . R , A ) ; DMatrixRMaj w_hat = GeometryMath_F64 . crossMatrix ( twist . w , null ) ; DMatrixRMaj tmp = A . copy ( ) ; CommonOps_DDRM . mult ( tmp , w_hat , A ) ; Vector3D_F64 w = twist . w ; A . data [ 0 ] += w . x * w . x * theta ; A . data [ 1 ] += w . x * w . y * theta ; A . data [ 2 ] += w . x * w . z * theta ; A . data [ 3 ] += w . y * w . x * theta ; A . data [ 4 ] += w . y * w . y * theta ; A . data [ 5 ] += w . y * w . z * theta ; A . data [ 6 ] += w . z * w . x * theta ; A . data [ 7 ] += w . z * w . y * theta ; A . data [ 8 ] += w . z * w . z * theta ; DMatrixRMaj y = new DMatrixRMaj ( 3 , 1 ) ; y . data [ 0 ] = motion . T . x ; y . data [ 1 ] = motion . T . y ; y . data [ 2 ] = motion . T . z ; DMatrixRMaj x = new DMatrixRMaj ( 3 , 1 ) ; CommonOps_DDRM . solve ( A , y , x ) ; twist . w . scale ( rod . theta ) ; twist . v . x = ( double ) x . data [ 0 ] ; twist . v . y = ( double ) x . data [ 1 ] ; twist . v . z = ( double ) x . data [ 2 ] ; twist . v . scale ( rod . theta ) ; } return twist ; }


Input: Specify the two transforms which values are to be interpolated between [CODESPLIT] public void setTransforms ( Se3_F64 initial , Se3_F64 end ) { this . initial . set ( initial ) ; translation . x = end . T . x - initial . T . x ; translation . y = end . T . y - initial . T . y ; translation . z = end . T . z - initial . T . z ; CommonOps_DDRM . multTransA ( initial . getR ( ) , end . getR ( ) , R ) ; ConvertRotation3D_F64 . matrixToRodrigues ( R , rotation ) ; rotMagnitude = rotation . theta ; }


Input: Interpolates a value between the first and second transform . A value close to 0 will be more similar to the initial and 1 more similar to the end . [CODESPLIT] public void interpolate ( double where , Se3_F64 output ) { rotation . setTheta ( where * rotMagnitude ) ; ConvertRotation3D_F64 . rodriguesToMatrix ( rotation , R ) ; output . T . x = initial . T . x + where * translation . x ; output . T . y = initial . T . y + where * translation . y ; output . T . z = initial . T . z + where * translation . z ; CommonOps_DDRM . mult ( initial . R , R , output . R ) ; }


Input: Makes sure x0 y0 is the lower extent and x1 y1 is the upper extent [CODESPLIT] public void enforceExtents ( ) { if ( p1 . x < p0 . x ) { double tmp = p1 . x ; p1 . x = p0 . x ; p0 . x = tmp ; } if ( p1 . y < p0 . y ) { double tmp = p1 . y ; p1 . y = p0 . y ; p0 . y = tmp ; } }


Input: Converts a 2D polygon into a 3D polygon . The 2D points will lie on the x - y plane ( e . g . ( x y 0 )) and are converted to 3D using polyToWorld . [CODESPLIT] public static void polygon2Dto3D ( Polygon2D_F64 polygon2D , Se3_F64 polyToWorld , FastQueue < Point3D_F64 > output ) { output . resize ( polygon2D . size ( ) ) ; for ( int i = 0 ; i < polygon2D . size ( ) ; i ++ ) { Point2D_F64 p2 = polygon2D . get ( i ) ; Point3D_F64 p3 = output . get ( i ) ; p3 . set ( p2 . x , p2 . y , 0 ) ; SePointOps_F64 . transform ( polyToWorld , p3 , p3 ) ; } }


Input: <p > Computes the weighted best fit line to the set of points using the polar line equation . The solution is optimal in the Euclidean sense see [ 1 ] for more details . < / p > [CODESPLIT] public static LinePolar2D_F64 polar ( List < Point2D_F64 > points , double weights [ ] , LinePolar2D_F64 ret ) { final int N = points . size ( ) ; double totalWeight = 0 ; for ( int i = 0 ; i < N ; i ++ ) { totalWeight += weights [ i ] ; } if ( totalWeight == 0 ) return null ; if ( ret == null ) ret = new LinePolar2D_F64 ( ) ; double meanX = 0 ; double meanY = 0 ; for ( int i = 0 ; i < N ; i ++ ) { Point2D_F64 p = points . get ( i ) ; double w = weights [ i ] ; meanX += w * p . x ; meanY += w * p . y ; } meanX /= totalWeight ; meanY /= totalWeight ; double top = 0 ; double bottom = 0 ; for ( int i = 0 ; i < N ; i ++ ) { Point2D_F64 p = points . get ( i ) ; double w = weights [ i ] ; double dx = meanX - p . x ; double dy = meanY - p . y ; top += w * dx * dy ; bottom += w * ( dy * dy - dx * dx ) ; } top /= totalWeight ; bottom /= totalWeight ; ret . angle = Math . atan2 ( - 2.0 * top , bottom ) / 2.0 ; ret . distance = ( double ) ( meanX * Math . cos ( ret . angle ) + meanY * Math . sin ( ret . angle ) ) ; return ret ; }


Input: SVD based method for fitting a plane to a set of points . The plane s equation is returned as a point on the plane and the normal vector . [CODESPLIT] public boolean svd ( List < Point3D_F64 > points , Point3D_F64 outputCenter , Vector3D_F64 outputNormal ) { final int N = points . size ( ) ; // find the centroid outputCenter . set ( 0 , 0 , 0 ) ; for ( int i = 0 ; i < N ; i ++ ) { Point3D_F64 p = points . get ( i ) ; outputCenter . x += p . x ; outputCenter . y += p . y ; outputCenter . z += p . z ; } outputCenter . x /= N ; outputCenter . y /= N ; outputCenter . z /= N ; return solvePoint ( points , outputCenter , outputNormal ) ; }


Input: SVD based method for fitting a plane to a set of points and a known point on the plane . The plane s equation is returned as a point on the plane and the normal vector . [CODESPLIT] public boolean solvePoint ( List < Point3D_F64 > points , Point3D_F64 pointOnPlane , Vector3D_F64 outputNormal ) { final int N = points . size ( ) ; // construct the matrix A . reshape ( N , 3 ) ; int index = 0 ; for ( int i = 0 ; i < N ; i ++ ) { Point3D_F64 p = points . get ( i ) ; A . data [ index ++ ] = p . x - pointOnPlane . x ; A . data [ index ++ ] = p . y - pointOnPlane . y ; A . data [ index ++ ] = p . z - pointOnPlane . z ; } // decompose and find the singular value if ( ! solverNull . process ( A , 1 , nullspace ) ) return false ; // the normal is the singular vector outputNormal . x = ( double ) nullspace . unsafe_get ( 0 , 0 ) ; outputNormal . y = ( double ) nullspace . unsafe_get ( 1 , 0 ) ; outputNormal . z = ( double ) nullspace . unsafe_get ( 2 , 0 ) ; return true ; }


Input: Resturns the length of the specified side that is composed of point index and index + 1 [CODESPLIT] public double getSideLength ( int index ) { Point2D_F64 a = vertexes . get ( index ) ; Point2D_F64 b = vertexes . get ( ( index + 1 ) % vertexes . size ) ; return ( double ) a . distance ( b ) ; }


Input: Returns true if the point is inside the polygon . Points along the border are ambiguously considered inside or outside . [CODESPLIT] public boolean isInside ( Point2D_F64 p ) { if ( isConvex ( ) ) { return Intersection2D_F64 . containConvex ( this , p ) ; } else { return Intersection2D_F64 . containConcave ( this , p ) ; } }


Input: Returns the line / edge defined by vertex index and index + 1 . [CODESPLIT] public LineSegment2D_F64 getLine ( int index , LineSegment2D_F64 storage ) { if ( storage == null ) storage = new LineSegment2D_F64 ( ) ; int j = ( index + 1 ) % vertexes . size ; storage . a . set ( get ( index ) ) ; storage . b . set ( get ( j ) ) ; return storage ; }


Input: Converts the polygon into a list . [CODESPLIT] public List < Point2D_F64 > convert ( @ Nullable List < Point2D_F64 > storage , boolean copy ) { if ( storage == null ) storage = new ArrayList <> ( ) ; else storage . clear ( ) ; if ( copy ) { for ( int i = 0 ; i < vertexes . size ; i ++ ) { storage . add ( vertexes . get ( i ) . copy ( ) ) ; } } else { storage . addAll ( vertexes . toList ( ) ) ; } return storage ; }


Input: 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 . [CODESPLIT] public void set ( List < Point2D_F64 > list ) { vertexes . resize ( list . size ( ) ) ; for ( int i = 0 ; i < list . size ( ) ; i ++ ) { vertexes . data [ i ] . set ( list . get ( i ) ) ; } }


Input: Converts symmetric 3x3 matrix back into a conic . [CODESPLIT] public static DMatrixRMaj convert ( ConicGeneral_F64 src , DMatrixRMaj dst ) { if ( dst == null ) dst = new DMatrixRMaj ( 3 , 3 ) ; else dst . reshape ( 3 , 3 ) ; double B = src . B / 2.0 ; double D = src . D / 2.0 ; double E = src . E / 2.0 ; dst . data [ 0 ] = src . A ; dst . data [ 1 ] = B ; dst . data [ 2 ] = D ; dst . data [ 3 ] = B ; dst . data [ 4 ] = src . C ; dst . data [ 5 ] = E ; dst . data [ 6 ] = D ; dst . data [ 7 ] = E ; dst . data [ 8 ] = src . F ; return dst ; }


Input: Converts symmetric 3x3 matrix back into a conic . Only the upper right portion of src is read . [CODESPLIT] public static ConicGeneral_F64 convert ( DMatrix3x3 src , ConicGeneral_F64 dst ) { if ( dst == null ) dst = new ConicGeneral_F64 ( ) ; dst . A = src . a11 ; dst . B = 2 * src . a12 ; dst . D = 2 * src . a13 ; dst . C = src . a22 ; dst . E = 2 * src . a23 ; dst . F = src . a33 ; return dst ; }


Input: Converts the conic into a symmetric 3x3 matrix [CODESPLIT] public static DMatrix3x3 convert ( ConicGeneral_F64 src , DMatrix3x3 dst ) { if ( dst == null ) dst = new DMatrix3x3 ( ) ; double B = src . B / 2.0 ; double D = src . D / 2.0 ; double E = src . E / 2.0 ; dst . a11 = src . A ; dst . a12 = B ; dst . a13 = D ; dst . a21 = B ; dst . a22 = src . C ; dst . a23 = E ; dst . a31 = D ; dst . a32 = E ; dst . a33 = src . F ; return dst ; }


Input: Converts symmetric 3x3 matrix back into a conic . Only the upper right portion of src is read . [CODESPLIT] public static ConicGeneral_F64 convert ( DMatrixRMaj src , ConicGeneral_F64 dst ) { if ( dst == null ) dst = new ConicGeneral_F64 ( ) ; dst . A = src . data [ 0 ] ; dst . B = 2 * src . data [ 1 ] ; dst . D = 2 * src . data [ 2 ] ; dst . C = src . data [ 4 ] ; dst . E = 2 * src . data [ 5 ] ; dst . F = src . data [ 8 ] ; return dst ; }


Input: Converts the conic into a parabola . If the conic isn t a parabola then it is converted into one by adjusting the value of B . [CODESPLIT] public static ParabolaGeneral_F64 convert ( ConicGeneral_F64 src , ParabolaGeneral_F64 dst ) { if ( dst == null ) dst = new ParabolaGeneral_F64 ( ) ; // NOTE haven't put much through if this is the correct way to handle negative values of A or C dst . A = Math . signum ( src . A ) * Math . sqrt ( Math . abs ( src . A ) ) ; dst . C = Math . signum ( src . C ) * Math . sqrt ( Math . abs ( src . C ) ) ; dst . D = src . D ; dst . E = src . E ; dst . F = src . F ; return dst ; }


Input: Converts the parabola into a conic . [CODESPLIT] public static ConicGeneral_F64 convert ( ParabolaGeneral_F64 src , ConicGeneral_F64 dst ) { if ( dst == null ) dst = new ConicGeneral_F64 ( ) ; dst . A = src . A * src . A ; dst . B = src . A * src . C * 2.0 ; dst . C = src . C * src . C ; dst . D = src . D ; dst . E = src . E ; dst . F = src . F ; return dst ; }


Input: } [CODESPLIT] public static ParabolaParametric_F64 convert ( ParabolaGeneral_F64 src , ParabolaParametric_F64 dst ) { if ( dst == null ) dst = new ParabolaParametric_F64 ( ) ; double A = src . A ; double C = src . C ; double D = src . D ; double E = src . E ; double F = src . F ; double bottom = C * D - A * E ; if ( bottom == 0 ) { throw new RuntimeException ( "Not a parabola" ) ; } else { dst . A = - C / bottom ; dst . B = E / bottom ; dst . C = - C * F / bottom ; dst . D = A / bottom ; dst . E = - D / bottom ; dst . F = A * F / bottom ; } return dst ; }


Input: Creates a skew symmetric cross product matrix from the provided tuple . [CODESPLIT] public static DMatrixRMaj crossMatrix ( double x0 , double x1 , double x2 , DMatrixRMaj ret ) { if ( ret == null ) { ret = new DMatrixRMaj ( 3 , 3 ) ; } else { ret . zero ( ) ; } ret . set ( 0 , 1 , - x2 ) ; ret . set ( 0 , 2 , x1 ) ; ret . set ( 1 , 0 , x2 ) ; ret . set ( 1 , 2 , - x0 ) ; ret . set ( 2 , 0 , - x1 ) ; ret . set ( 2 , 1 , x0 ) ; return ret ; }


Input: Creates a skew symmetric cross product matrix from the provided tuple . [CODESPLIT] public static DMatrixRMaj crossMatrix ( GeoTuple3D_F64 v , DMatrixRMaj ret ) { if ( ret == null ) { ret = new DMatrixRMaj ( 3 , 3 ) ; } else { ret . zero ( ) ; } double x = v . getX ( ) ; double y = v . getY ( ) ; double z = v . getZ ( ) ; ret . set ( 0 , 1 , - z ) ; ret . set ( 0 , 2 , y ) ; ret . set ( 1 , 0 , z ) ; ret . set ( 1 , 2 , - x ) ; ret . set ( 2 , 0 , - y ) ; ret . set ( 2 , 1 , x ) ; return ret ; }


Input:
 Computes the cross product :

 c = a x b
 [CODESPLIT] public static void cross ( double a_x , double a_y , double a_z , double b_x , double b_y , double b_z , GeoTuple3D_F64 c ) { c . x = a_y * b_z - a_z * b_y ; c . y = a_z * b_x - a_x * b_z ; c . z = a_x * b_y - a_y * b_x ; }


Input:
 Computes the cross product :

 c = a x b
 where a is in homogeneous coordinates .
 [CODESPLIT] public static void cross ( GeoTuple2D_F64 a , GeoTuple3D_F64 b , GeoTuple3D_F64 c ) { c . x = a . y * b . z - b . y ; c . y = b . x - a . x * b . z ; c . z = a . x * b . y - a . y * b . x ; }


Input:
 Adds two points together .

 c = a + b

 Point c can be the same instance as a or b .
 [CODESPLIT] public static void add ( GeoTuple3D_F64 a , GeoTuple3D_F64 b , GeoTuple3D_F64 c ) { c . x = a . x + b . x ; c . y = a . y + b . y ; c . z = a . z + b . z ; }


Input:
 Adds two points together while scaling them .

 pt 2 = a 0 pt 0 + a 1 pt 1

 Point c can be the same instance as a or b .
 [CODESPLIT] public static void add ( double a0 , GeoTuple3D_F64 pt0 , double a1 , GeoTuple3D_F64 pt1 , GeoTuple3D_F64 pt2 ) { pt2 . x = a0 * pt0 . x + a1 * pt1 . x ; pt2 . y = a0 * pt0 . y + a1 * pt1 . y ; pt2 . z = a0 * pt0 . z + a1 * pt1 . z ; }


Input: <p > ret = p0 + M * p1< / p > [CODESPLIT] public static < T extends GeoTuple3D_F64 > T addMult ( T p0 , DMatrixRMaj M , T p1 , T result ) { if ( M . numRows != 3 || M . numCols != 3 ) throw new IllegalArgumentException ( "Input matrix must be 3 by 3, not " + M . numRows + " " + M . numCols ) ; if ( result == null ) { result = ( T ) p0 . createNewInstance ( ) ; } double x = p1 . x ; double y = p1 . y ; double z = p1 . z ; result . x = p0 . x + ( double ) ( M . data [ 0 ] * x + M . data [ 1 ] * y + M . data [ 2 ] * z ) ; result . y = p0 . y + ( double ) ( M . data [ 3 ] * x + M . data [ 4 ] * y + M . data [ 5 ] * z ) ; result . z = p0 . z + ( double ) ( M . data [ 6 ] * x + M . data [ 7 ] * y + M . data [ 8 ] * z ) ; return result ; }


Input:
 Substracts two points from each other .

 c = a - b

 Point c can be the same instance as a or b .
 [CODESPLIT] public static void sub ( GeoTuple3D_F64 a , GeoTuple3D_F64 b , GeoTuple3D_F64 c ) { c . x = a . x - b . x ; c . y = a . y - b . y ; c . z = a . z - b . z ; }


Input: Rotates a 2D point by the specified angle . [CODESPLIT] public static void rotate ( double theta , GeoTuple2D_F64 pt , GeoTuple2D_F64 solution ) { double c = Math . cos ( theta ) ; double s = Math . sin ( theta ) ; double x = pt . x ; double y = pt . y ; solution . x = c * x - s * y ; solution . y = s * x + c * y ; }


Input: Rotates a 2D point by the specified angle . [CODESPLIT] public static void rotate ( double c , double s , GeoTuple2D_F64 pt , GeoTuple2D_F64 solution ) { double x = pt . x ; double y = pt . y ; solution . x = c * x - s * y ; solution . y = s * x + c * y ; }


Input: mod = M * pt <p > pt and mod can be the same reference . < / p > [CODESPLIT] public static < T extends GeoTuple3D_F64 > T mult ( DMatrixRMaj M , T pt , T result ) { if ( M . numRows != 3 || M . numCols != 3 ) throw new IllegalArgumentException ( "Input matrix must be 3 by 3, not " + M . numRows + " " + M . numCols ) ; if ( result == null ) { result = ( T ) pt . createNewInstance ( ) ; } double x = pt . x ; double y = pt . y ; double z = pt . z ; result . x = ( double ) ( M . data [ 0 ] * x + M . data [ 1 ] * y + M . data [ 2 ] * z ) ; result . y = ( double ) ( M . data [ 3 ] * x + M . data [ 4 ] * y + M . data [ 5 ] * z ) ; result . z = ( double ) ( M . data [ 6 ] * x + M . data [ 7 ] * y + M . data [ 8 ] * z ) ; return ( T ) result ; }


Input: <p > Computes mod = M * pt where both pt and mod are in homogeneous coordinates with z assumed to be equal to 1 and M is a 3x3 matrix . < / p > <p > pt and mod can be the same point . < / p > [CODESPLIT] public static < T extends GeoTuple2D_F64 > T mult ( DMatrixRMaj M , T pt , T mod ) { if ( M . numRows != 3 || M . numCols != 3 ) throw new IllegalArgumentException ( "Input matrix must be 3 by 3, not " + M . numRows + " " + M . numCols ) ; if ( mod == null ) { throw new IllegalArgumentException ( "Must provide an instance in mod" ) ; } double x = pt . x ; double y = pt . y ; double modz = ( double ) ( M . unsafe_get ( 2 , 0 ) * x + M . unsafe_get ( 2 , 1 ) * y + M . unsafe_get ( 2 , 2 ) ) ; mod . x = ( double ) ( ( M . unsafe_get ( 0 , 0 ) * x + M . unsafe_get ( 0 , 1 ) * y + M . unsafe_get ( 0 , 2 ) ) / modz ) ; mod . y = ( double ) ( ( M . unsafe_get ( 1 , 0 ) * x + M . unsafe_get ( 1 , 1 ) * y + M . unsafe_get ( 1 , 2 ) ) / modz ) ; return mod ; }


Input: x = P * X [CODESPLIT] public static void mult ( DMatrixRMaj P , GeoTuple4D_F64 X , GeoTuple3D_F64 mod ) { if ( P . numRows != 3 || P . numCols != 4 ) throw new IllegalArgumentException ( "Input matrix must be 3 by 4 not " + P . numRows + " " + P . numCols ) ; mod . x = P . data [ 0 ] * X . x + P . data [ 1 ] * X . y + P . data [ 2 ] * X . z + P . data [ 3 ] * X . w ; mod . y = P . data [ 4 ] * X . x + P . data [ 5 ] * X . y + P . data [ 6 ] * X . z + P . data [ 7 ] * X . w ; mod . z = P . data [ 8 ] * X . x + P . data [ 9 ] * X . y + P . data [ 10 ] * X . z + P . data [ 11 ] * X . w ; }


Input:
 Computes the following :
 result = cross ( A )  T * M
 where M and result are 3x3 matrices cross ( A ) is the cross product matrix of A .
 [CODESPLIT] public static DMatrixRMaj multCrossATransA ( GeoTuple2D_F64 A , DMatrixRMaj M , DMatrixRMaj result ) { if ( M . numRows != 3 || M . numCols != 3 ) throw new IllegalArgumentException ( "Input matrix must be 3 by 3, not " + M . numRows + " " + M . numCols ) ; if ( result == null ) { result = new DMatrixRMaj ( 3 , 3 ) ; } double x = A . x ; double y = A . y ; double a11 = M . data [ 0 ] ; double a12 = M . data [ 1 ] ; double a13 = M . data [ 2 ] ; double a21 = M . data [ 3 ] ; double a22 = M . data [ 4 ] ; double a23 = M . data [ 5 ] ; double a31 = M . data [ 6 ] ; double a32 = M . data [ 7 ] ; double a33 = M . data [ 8 ] ; result . data [ 0 ] = a21 - a31 * y ; result . data [ 1 ] = a22 - a32 * y ; result . data [ 2 ] = a23 - a33 * y ; result . data [ 3 ] = - a11 + a31 * x ; result . data [ 4 ] = - a12 + a32 * x ; result . data [ 5 ] = - a13 + a33 * x ; result . data [ 6 ] = a11 * y - a21 * x ; result . data [ 7 ] = a12 * y - a22 * x ; result . data [ 8 ] = a13 * y - a23 * x ; return result ; }


Input: mod = M<sup > T< / sup > * pt . Both pt and mod can be the same instance . [CODESPLIT] public static < T extends GeoTuple3D_F64 > T multTran ( DMatrixRMaj M , T pt , T mod ) { if ( M . numRows != 3 || M . numCols != 3 ) throw new IllegalArgumentException ( "Rotation matrices are 3 by 3." ) ; if ( mod == null ) { mod = ( T ) pt . createNewInstance ( ) ; } double x = pt . x ; double y = pt . y ; double z = pt . z ; mod . x = ( double ) ( M . unsafe_get ( 0 , 0 ) * x + M . unsafe_get ( 1 , 0 ) * y + M . unsafe_get ( 2 , 0 ) * z ) ; mod . y = ( double ) ( M . unsafe_get ( 0 , 1 ) * x + M . unsafe_get ( 1 , 1 ) * y + M . unsafe_get ( 2 , 1 ) * z ) ; mod . z = ( double ) ( M . unsafe_get ( 0 , 2 ) * x + M . unsafe_get ( 1 , 2 ) * y + M . unsafe_get ( 2 , 2 ) * z ) ; return ( T ) mod ; }


Input: <p > Computes the inner matrix product : <br > ret = x<sup > T< / sup > A<sup > T< / sup > y < / p > [CODESPLIT] public static double innerProdTranM ( GeoTuple3D_F64 a , DMatrixRMaj M , GeoTuple3D_F64 b ) { if ( M . numRows != 3 || M . numCols != 3 ) throw new IllegalArgumentException ( "M must be 3 by 3." ) ; DMatrixRMaj m1 = new DMatrixRMaj ( 3 , 1 , true , a . x , a . y , a . z ) ; DMatrixRMaj m2 = new DMatrixRMaj ( 3 , 1 , true , b . x , b . y , b . z ) ; return ( double ) ( VectorVectorMult_DDRM . innerProdTranA ( m1 , M , m2 ) ) ; }


Input: Computes the outer product of two vectors : <br > O = a * b<sup > T< / sup > [CODESPLIT] public static DMatrixRMaj outerProd ( GeoTuple3D_F64 a , GeoTuple3D_F64 b , DMatrixRMaj ret ) { if ( ret == null ) ret = new DMatrixRMaj ( 3 , 3 ) ; ret . data [ 0 ] = a . x * b . x ; ret . data [ 1 ] = a . x * b . y ; ret . data [ 2 ] = a . x * b . z ; ret . data [ 3 ] = a . y * b . x ; ret . data [ 4 ] = a . y * b . y ; ret . data [ 5 ] = a . y * b . z ; ret . data [ 6 ] = a . z * b . x ; ret . data [ 7 ] = a . z * b . y ; ret . data [ 8 ] = a . z * b . z ; return ret ; }


Input: Adds the outer product of two vectors onto a matrix : <br > ret = A + scalar * a * b<sup > T< / sup > [CODESPLIT] public static DMatrixRMaj addOuterProd ( DMatrixRMaj A , double scalar , GeoTuple3D_F64 b , GeoTuple3D_F64 c , DMatrixRMaj ret ) { if ( ret == null ) ret = new DMatrixRMaj ( 3 , 3 ) ; ret . data [ 0 ] = A . data [ 0 ] + scalar * b . x * c . x ; ret . data [ 1 ] = A . data [ 1 ] + scalar * b . x * c . y ; ret . data [ 2 ] = A . data [ 2 ] + scalar * b . x * c . z ; ret . data [ 3 ] = A . data [ 3 ] + scalar * b . y * c . x ; ret . data [ 4 ] = A . data [ 4 ] + scalar * b . y * c . y ; ret . data [ 5 ] = A . data [ 5 ] + scalar * b . y * c . z ; ret . data [ 6 ] = A . data [ 6 ] + scalar * b . z * c . x ; ret . data [ 7 ] = A . data [ 7 ] + scalar * b . z * c . y ; ret . data [ 8 ] = A . data [ 8 ] + scalar * b . z * c . z ; return ret ; }


Input: <p > 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 . < / p > [CODESPLIT] public static double innerProd ( GeoTuple2D_F64 a , DMatrixRMaj M , GeoTuple2D_F64 b ) { if ( M . numRows != 3 || M . numCols != 3 ) throw new IllegalArgumentException ( "M must be 3 by 3." ) ; DMatrixRMaj m1 = new DMatrixRMaj ( 3 , 1 , true , a . x , a . y , 1 ) ; DMatrixRMaj m2 = new DMatrixRMaj ( 3 , 1 , true , b . x , b . y , 1 ) ; return ( double ) ( VectorVectorMult_DDRM . innerProdA ( m1 , M , m2 ) ) ; }


Input: <p > Multiplies each element in the tuple by v . <br > p<sub > i< / sub > = p<sub > i< / sub > * v < / p > [CODESPLIT] public static void scale ( GeoTuple3D_F64 p , double v ) { p . x *= v ; p . y *= v ; p . z *= v ; }


Input: Divides each element by v [CODESPLIT] public static void divide ( GeoTuple3D_F64 p , double v ) { p . x /= v ; p . y /= v ; p . z /= v ; }


Input:
 Changes the sign of the vector :

 T = - T
 [CODESPLIT] public static void changeSign ( GeoTuple3D_F64 t ) { t . x = - t . x ; t . y = - t . y ; t . z = - t . z ; }


Input: Converts a GeoTuple3D_F64 into DMatrixRMaj [CODESPLIT] public static DMatrixRMaj toMatrix ( GeoTuple3D_F64 in , DMatrixRMaj out ) { if ( out == null ) out = new DMatrixRMaj ( 3 , 1 ) ; else if ( out . getNumElements ( ) != 3 ) throw new IllegalArgumentException ( "Vector with 3 elements expected" ) ; out . data [ 0 ] = in . x ; out . data [ 1 ] = in . y ; out . data [ 2 ] = in . z ; return out ; }


Input: Converts a DMatrixRMaj into GeoTuple3D_F64 [CODESPLIT] public static void toTuple3D ( DMatrixRMaj in , GeoTuple3D_F64 out ) { out . x = ( double ) in . get ( 0 ) ; out . y = ( double ) in . get ( 1 ) ; out . z = ( double ) in . get ( 2 ) ; }


Input: Assign the Rodrigues coordinates using a 3 element vector . Theta is the vector s magnitude and the axis of rotation is the unit vector . [CODESPLIT] public void setParamVector ( double x , double y , double z ) { double ax = Math . abs ( x ) ; double ay = Math . abs ( y ) ; double az = Math . abs ( z ) ; double max = Math . max ( ax , ay ) ; max = Math . max ( max , az ) ; if ( max == 0 ) { theta = 0 ; unitAxisRotation . set ( 1 , 0 , 0 ) ; } else { x /= max ; y /= max ; z /= max ; theta = Math . sqrt ( x * x + y * y + z * z ) ; unitAxisRotation . x = x / theta ; unitAxisRotation . y = y / theta ; unitAxisRotation . z = z / theta ; theta *= max ; } }


Input: Converts an angle which is ( - pi to pi ) into a half circle angle ( - pi / 2 to pi / 2 ) . [CODESPLIT] public static double toHalfCircle ( double angle ) { if ( angle < 0 ) angle += Math . PI ; if ( angle > Math . PI / 2.0 ) angle -= Math . PI ; return angle ; }


Input: Returns an angle which is equivalent to the one provided but between ( inclusive ) - &pi ; and &pi ; . [CODESPLIT] public static double bound ( double ang ) { ang %= GrlConstants . PI2 ; if ( ang > PI ) { return ang - GrlConstants . PI2 ; } else if ( ang < - PI ) { return ang + GrlConstants . PI2 ; } return ang ; }


Input: Returns an angle which is equivalent to the one provided but between ( inclusive ) - &pi ; and &pi ; . [CODESPLIT] public static float bound ( float ang ) { ang %= GrlConstants . F_PI2 ; if ( ang > GrlConstants . F_PI ) { return ang - GrlConstants . F_PI2 ; } else if ( ang < - GrlConstants . F_PI ) { return ang + GrlConstants . F_PI2 ; } return ang ; }


Input: Bounds the angle between - &pi ; / 2 and &pi ; / 2 [CODESPLIT] public static double boundHalf ( double angle ) { angle = bound ( angle ) ; if ( angle > GrlConstants . PId2 ) { angle -= Math . PI ; } else if ( angle < - GrlConstants . PId2 ) { angle += Math . PI ; } return angle ; }


Input: Bounds the angle between - &pi ; / 2 and &pi ; / 2 [CODESPLIT] public static float boundHalf ( float angle ) { angle = bound ( angle ) ; if ( angle > GrlConstants . F_PId2 ) { angle -= GrlConstants . F_PI ; } else if ( angle < - GrlConstants . F_PId2 ) { angle += GrlConstants . F_PI ; } return angle ; }


Input: 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 ; . [CODESPLIT] public static double distanceCCW ( double angA , double angB ) { if ( angB >= angA ) return angB - angA ; else return GrlConstants . PI2 - ( angA - angB ) ; }


Input: 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 ; . [CODESPLIT] public static float distanceCCW ( float angA , float angB ) { if ( angB >= angA ) return angB - angA ; else return GrlConstants . F_PI2 - ( angA - angB ) ; }


Input: Angular distance in radians to go from angA to angB in clock - wise direction . The resulting angle will be from 0 to 2&pi ; . [CODESPLIT] public static double distanceCW ( double angA , double angB ) { if ( angA >= angB ) return angA - angB ; else return GrlConstants . PI2 - ( angB - angA ) ; }


Input: Angular distance in radians to go from angA to angB in clock - wise direction . The resulting angle will be from 0 to 2&pi ; . [CODESPLIT] public static float distanceCW ( float angA , float angB ) { if ( angA >= angB ) return angA - angB ; else return GrlConstants . F_PI2 - ( angB - angA ) ; }


Input:
 Returns the difference between two angles and bounds the result between - pi and pi :
 result = angA - angB
 and takes in account boundary conditions .
 [CODESPLIT] public static double minus ( double angA , double angB ) { double diff = angA - angB ; if ( diff > Math . PI ) { return GrlConstants . PI2 - diff ; } else if ( diff < - Math . PI ) return - GrlConstants . PI2 - diff ; return diff ; }


Input:
 Returns the difference between two angles and bounds the result between - pi and pi :
 result = angA - angB
 and takes in account boundary conditions .
 [CODESPLIT] public static float minus ( float angA , float angB ) { float diff = angA - angB ; if ( diff > GrlConstants . F_PI ) { return GrlConstants . F_PI2 - diff ; } else if ( diff < - GrlConstants . F_PI ) return - GrlConstants . F_PI2 - diff ; return diff ; }


Input: Angular distance between two half circle angles . [CODESPLIT] public static double distHalf ( double angA , double angB ) { double a = Math . abs ( angA - angB ) ; if ( a <= Math . PI / 2 ) return a ; else return Math . PI - a ; }


Input: 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 > [CODESPLIT] public static double reflectZeroToOne ( double value ) { if ( value < 0 ) value = - value ; value = value % 2.0 ; if ( value > 1.0 ) return 2.0 - value ; return value ; }


Input: 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 . [CODESPLIT] public static boolean intersect ( PlaneNormal3D_F64 plane , LineParametric3D_F64 line , Point3D_F64 intersection ) { double dx = plane . p . x - line . p . x ; double dy = plane . p . y - line . p . y ; double dz = plane . p . z - line . p . z ; double top = dx * plane . n . x + dy * plane . n . y + dz * plane . n . z ; double bottom = line . slope . dot ( plane . n ) ; if ( bottom == 0 ) return false ; double d = top / bottom ; intersection . x = line . p . x + d * line . slope . x ; intersection . y = line . p . y + d * line . slope . y ; intersection . z = line . p . z + d * line . slope . z ; return true ; }


Input: 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 . [CODESPLIT] public static boolean intersect ( PlaneGeneral3D_F64 plane , LineParametric3D_F64 line , Point3D_F64 intersection ) { double top = plane . D - plane . A * line . p . x - plane . B * line . p . y - plane . C * line . p . z ; double bottom = plane . A * line . slope . x + plane . B * line . slope . y + plane . C * line . slope . z ; if ( bottom == 0 ) return false ; double d = top / bottom ; intersection . x = line . p . x + d * line . slope . x ; intersection . y = line . p . y + d * line . slope . y ; intersection . z = line . p . z + d * line . slope . z ; return true ; }


Input: 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 . [CODESPLIT] public static boolean intersect ( PlaneGeneral3D_F64 a , PlaneGeneral3D_F64 b , LineParametric3D_F64 line ) { // Line's slope is the cross product of the two normal vectors GeometryMath_F64 . cross ( a . A , a . B , a . C , b . A , b . B , b . C , line . slope ) ; if ( line . slope . normSq ( ) == 0 ) return false ; // Closest point on plane 'a' to origin (0,0,0) double n2 = a . A * a . A + a . B * a . B + a . C * a . C ; double closestX = a . A * a . D / n2 ; double closestY = a . B * a . D / n2 ; double closestZ = a . C * a . D / n2 ; // Cross product between normal of 'a' and the line's slope.  This points towards the intersection double slopeX = a . B * line . slope . z - a . C * line . slope . y ; double slopeY = a . C * line . slope . x - a . A * line . slope . z ; double slopeZ = a . A * line . slope . y - a . B * line . slope . x ; // Now find the intersection of the plane and a line containing point 'closest' and pointing towards the // the intersection. double top = b . D - b . A * closestX - b . B * closestY - b . C * closestZ ; double bottom = b . A * slopeX + b . B * slopeY + b . C * slopeZ ; double d = top / bottom ; line . p . x = closestX + d * slopeX ; line . p . y = closestY + d * slopeY ; line . p . z = closestZ + d * slopeZ ; return true ; }


Input: <p > Finds the intersection between a 3D triangle and a line - segment . Code ported from [ 1 ] . < / p > [CODESPLIT] public static int intersect ( Triangle3D_F64 T , LineSegment3D_F64 R , Point3D_F64 output ) { return intersect ( T , R , output , new Vector3D_F64 ( ) , new Vector3D_F64 ( ) , new Vector3D_F64 ( ) , new Vector3D_F64 ( ) , new Vector3D_F64 ( ) ) ; }


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


Input: <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 > [CODESPLIT] public static int intersect ( Triangle3D_F64 T , LineParametric3D_F64 R , Point3D_F64 output ) { return intersect ( T , R , output , new Vector3D_F64 ( ) , new Vector3D_F64 ( ) , new Vector3D_F64 ( ) , new Vector3D_F64 ( ) ) ; }


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


Input: Determines if the point on the same plane as T is contained inside of T . [CODESPLIT] private static boolean containedPlane ( Point3D_F64 T_v0 , Point3D_F64 output , Vector3D_F64 u , Vector3D_F64 v , Vector3D_F64 w0 ) { double uu , uv , vv , wu , wv , D ; uu = u . dot ( u ) ; uv = u . dot ( v ) ; vv = v . dot ( v ) ; w0 . minus ( output , T_v0 ) ; wu = w0 . dot ( u ) ; wv = w0 . dot ( v ) ; D = uv * uv - uu * vv ; // get and test parametric coords double s , t ; s = ( uv * wv - vv * wu ) / D ; if ( s < 0.0 || s > 1.0 ) // I is outside T return false ; t = ( uv * wu - uu * wv ) / D ; return ! ( t < 0.0 ) && ! ( ( s + t ) > 1.0 ) ; // I is outside T }


Input: 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 [CODESPLIT] public static boolean contained ( BoxLength3D_F64 box , Point3D_F64 point ) { return ( box . p . x <= point . x && point . x < box . p . x + box . lengthX && box . p . y <= point . y && point . y < box . p . y + box . lengthY && box . p . z <= point . z && point . z < box . p . z + box . lengthZ ) ; }


Input:
 Returns true if the point is contained inside the box with an exclusive upper extent . The point is considered to be inside the box if the following test passes :
 box . p0 . x &le ; point . x { @code < } box . p1 . x
 box . p0 . y &le ; point . y { @code < } box . p1 . y
 box . p0 . z &le ; point . z { @code < } box . p1 . z

 [CODESPLIT] public static boolean contained ( Box3D_F64 box , Point3D_F64 point ) { return ( box . p0 . x <= point . x && point . x < box . p1 . x && box . p0 . y <= point . y && point . y < box . p1 . y && box . p0 . z <= point . z && point . z < box . p1 . z ) ; }


Input: 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 . [CODESPLIT] public static boolean intersect ( LineParametric3D_F64 line , Sphere3D_F64 sphere , Point3D_F64 a , Point3D_F64 b ) { // this equation was found by solving for l: // ||(P + V*l) - X0|| == r double r2 = sphere . radius * sphere . radius ; double PP = GeometryMath_F64 . dot ( line . p , line . p ) ; double PV = GeometryMath_F64 . dot ( line . p , line . slope ) ; double PX = GeometryMath_F64 . dot ( line . p , sphere . center ) ; double VV = GeometryMath_F64 . dot ( line . slope , line . slope ) ; double VX = GeometryMath_F64 . dot ( line . slope , sphere . center ) ; double XX = GeometryMath_F64 . dot ( sphere . center , sphere . center ) ; // Coefficients in the quadratic equation double A = VV ; double B = 2.0 * ( PV - VX ) ; double C = PP + XX - 2.0 * PX - r2 ; // solve for the quadratic equation double inner = B * B - 4.0 * A * C ; if ( inner < 0 ) return false ; double sqrt = Math . sqrt ( inner ) ; double t0 = ( - B + sqrt ) / ( 2.0 * A ) ; double t1 = ( - B - sqrt ) / ( 2.0 * A ) ; line . setPointOnLine ( t0 , a ) ; line . setPointOnLine ( t1 , b ) ; return true ; }


Input: Perform linear interpolation [CODESPLIT] public static void interpolate ( Se2_F64 a , Se2_F64 b , double where , Se2_F64 output ) { double w0 = 1.0 - where ; output . T . x = a . T . x * w0 + b . T . x * where ; output . T . y = a . T . y * w0 + b . T . y * where ; // interpolating rotation is more difficult // This only works well if the difference between the two angles is small double yaw0 = a . getYaw ( ) ; double yaw1 = b . getYaw ( ) ; double cw = UtilAngle . distanceCW ( yaw0 , yaw1 ) ; double ccw = UtilAngle . distanceCCW ( yaw0 , yaw1 ) ; double yaw ; if ( cw > ccw ) { yaw = yaw0 + ccw * where ; } else { yaw = yaw0 - cw * where ; } output . setYaw ( yaw ) ; }


Input: The unit eigenvector corresponding to the maximum eigenvalue of Q is the rotation parameterized as a quaternion . [CODESPLIT] private void extractQuaternionFromQ ( SimpleMatrix q ) { SimpleEVD < SimpleMatrix > evd = q . eig ( ) ; int indexMax = evd . getIndexMax ( ) ; SimpleMatrix v_max = evd . getEigenVector ( indexMax ) ; quat . w = ( double ) v_max . get ( 0 ) ; quat . x = ( double ) v_max . get ( 1 ) ; quat . y = ( double ) v_max . get ( 2 ) ; quat . z = ( double ) v_max . get ( 3 ) ; quat . normalize ( ) ; ConvertRotation3D_F64 . quaternionToMatrix ( quat , motion . getR ( ) ) ; }


Input: Computes the convex hull . The output will be in counter - clockwise order . [CODESPLIT] public void process ( Point2D_F64 [ ] input , int length , Polygon2D_F64 hull ) { // ahdnle special cases if ( length == 2 ) { hull . vertexes . resize ( length ) ; for ( int i = 0 ; i < length ; i ++ ) { hull . get ( i ) . set ( input [ i ] ) ; } return ; } sorter . sort ( input , length ) ; work . reset ( ) ; // construct the lower hull for ( int i = 0 ; i < length ; i ++ ) { Point2D_F64 p = input [ i ] ; //Contains at least 2 points and the last two points and 'p' do not make a counter-clockwise turn while ( work . size ( ) >= 2 && subtractThenCross ( p , work . getTail ( 0 ) , work . getTail ( 1 ) ) >= 0 ) { // remove the last points from the hull work . removeTail ( ) ; } // append p to the end work . add ( p ) ; } work . removeTail ( ) ; int minSize = work . size + 2 ; // construct upper hull for ( int i = length - 1 ; i >= 0 ; i -- ) // Finding top layer from hull { //Contains at least 2 points and the last two points and 'p' do not make a counter-clockwise turn Point2D_F64 p = input [ i ] ; while ( work . size ( ) >= minSize && subtractThenCross ( p , work . getTail ( 0 ) , work . getTail ( 1 ) ) >= 0 ) { work . removeTail ( ) ; } // append p to the end work . add ( p ) ; } work . removeTail ( ) ; // create a copy for the output // the work buffer contains references to the input points, but to be safe the output should have its // own instances hull . vertexes . resize ( work . size ) ; for ( int i = 0 ; i < work . size ( ) ; i ++ ) { hull . vertexes . data [ i ] . set ( work . get ( i ) ) ; } }


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


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


Input: Sets the provided transform so that it does not transform any points . [CODESPLIT] public static void setToNoMotion ( Se3_F64 se ) { CommonOps_DDRM . setIdentity ( se . getR ( ) ) ; se . getT ( ) . set ( 0 , 0 , 0 ) ; }


Input: Converts { [CODESPLIT] public static Affine2D_F64 toAffine ( Se2_F64 se , Affine2D_F64 affine ) { if ( affine == null ) affine = new Affine2D_F64 ( ) ; affine . a11 = se . c ; affine . a12 = - se . s ; affine . a21 = se . s ; affine . a22 = se . c ; affine . tx = se . T . x ; affine . ty = se . T . y ; return affine ; }


Input: Converts it into a 4 by 4 homogeneous matrix . [CODESPLIT] public static DMatrixRMaj toHomogeneous ( Se3_F64 se , DMatrixRMaj ret ) { if ( ret == null ) ret = new DMatrixRMaj ( 4 , 4 ) ; else { ret . set ( 3 , 0 , 0 ) ; ret . set ( 3 , 1 , 0 ) ; ret . set ( 3 , 2 , 0 ) ; } CommonOps_DDRM . insert ( se . getR ( ) , ret , 0 , 0 ) ; Vector3D_F64 T = se . getT ( ) ; ret . set ( 0 , 3 , T . x ) ; ret . set ( 1 , 3 , T . y ) ; ret . set ( 2 , 3 , T . z ) ; ret . set ( 3 , 3 , 1 ) ; return ret ; }


Input: Converts a homogeneous representation into { @link Se3_F64 } . [CODESPLIT] public static Se3_F64 toSe3 ( DMatrixRMaj H , Se3_F64 ret ) { if ( H . numCols != 4 || H . numRows != 4 ) throw new IllegalArgumentException ( "The homogeneous matrix must be 4 by 4 by definition." ) ; if ( ret == null ) ret = new Se3_F64 ( ) ; ret . setTranslation ( ( double ) H . get ( 0 , 3 ) , ( double ) H . get ( 1 , 3 ) , ( double ) H . get ( 2 , 3 ) ) ; CommonOps_DDRM . extract ( H , 0 , 3 , 0 , 3 , ret . getR ( ) , 0 , 0 ) ; return ret ; }


Input: Converts it into a 3 by 3 homogeneous matrix . [CODESPLIT] public static DMatrixRMaj toHomogeneous ( Se2_F64 se , DMatrixRMaj ret ) { if ( ret == null ) ret = new DMatrixRMaj ( 3 , 3 ) ; else { ret . set ( 2 , 0 , 0 ) ; ret . set ( 2 , 1 , 0 ) ; } final double c = se . getCosineYaw ( ) ; final double s = se . getSineYaw ( ) ; ret . set ( 0 , 0 , c ) ; ret . set ( 0 , 1 , - s ) ; ret . set ( 1 , 0 , s ) ; ret . set ( 1 , 1 , c ) ; ret . set ( 0 , 2 , se . getX ( ) ) ; ret . set ( 1 , 2 , se . getY ( ) ) ; ret . set ( 2 , 2 , 1 ) ; return ret ; }


Input: Converts a homogeneous representation into { @link Se2_F64 } . [CODESPLIT] public static Se2_F64 toSe2 ( DMatrixRMaj H , Se2_F64 ret ) { if ( H . numCols != 3 || H . numRows != 3 ) throw new IllegalArgumentException ( "The homogeneous matrix must be 3 by 3 by definition." ) ; if ( ret == null ) ret = new Se2_F64 ( ) ; ret . setTranslation ( ( double ) H . get ( 0 , 2 ) , ( double ) H . get ( 1 , 2 ) ) ; double c = ( double ) H . get ( 0 , 0 ) ; double s = ( double ) H . get ( 1 , 0 ) ; ret . setYaw ( Math . atan2 ( s , c ) ) ; return ret ; }


Input: Sets the value of an { @link Se3_F64 } using Euler XYZ coordinates for the rotation and a translation vector . [CODESPLIT] public static Se3_F64 eulerXyz ( double dx , double dy , double dz , double rotX , double rotY , double rotZ , Se3_F64 se ) { return eulerXyz ( dx , dy , dz , EulerType . XYZ , rotX , rotY , rotZ , se ) ; }


Input: Create SE3 using axis - angle for rotation and XYZ tanslation [CODESPLIT] public static Se3_F64 axisXyz ( double dx , double dy , double dz , double rotX , double rotY , double rotZ , Se3_F64 se ) { if ( se == null ) se = new Se3_F64 ( ) ; double theta = Math . sqrt ( rotX * rotX + rotY + rotY + rotZ * rotZ ) ; if ( theta == 0 ) { CommonOps_DDRM . setIdentity ( se . R ) ; } else { ConvertRotation3D_F64 . rodriguesToMatrix ( rotX / theta , rotY / theta , rotZ / theta , theta , se . getR ( ) ) ; } Vector3D_F64 T = se . getT ( ) ; T . x = dx ; T . y = dy ; T . z = dz ; return se ; }


Input: Can be used to see if two transforms are identical to within tolerance [CODESPLIT] public static boolean isIdentical ( Se3_F64 a , Se3_F64 b , double tolT , double tolR ) { if ( Math . abs ( a . T . x - b . T . x ) > tolT ) return false ; if ( Math . abs ( a . T . y - b . T . y ) > tolT ) return false ; if ( Math . abs ( a . T . z - b . T . z ) > tolT ) return false ; DMatrixRMaj D = new DMatrixRMaj ( 3 , 3 ) ; CommonOps_DDRM . multTransA ( a . R , b . R , D ) ; Rodrigues_F64 rod = new Rodrigues_F64 ( ) ; ConvertRotation3D_F64 . matrixToRodrigues ( D , rod ) ; return rod . theta <= tolR ; }


Input: Finds the best fit projection of a onto SE ( 3 ) . This is useful when a was estimated using a linear algorithm . [CODESPLIT] public static boolean bestFit ( Se3_F64 a ) { SingularValueDecomposition_F64 < DMatrixRMaj > svd = DecompositionFactory_DDRM . svd ( true , true , true ) ; if ( ! svd . decompose ( a . R ) ) throw new RuntimeException ( "SVD Failed" ) ; CommonOps_DDRM . multTransB ( svd . getU ( null , false ) , svd . getV ( null , false ) , a . R ) ; // determinant should be +1 double det = CommonOps_DDRM . det ( a . R ) ; if ( det < 0 ) { CommonOps_DDRM . scale ( - 1 , a . R ) ; } // compute the determinant of the singular matrix double b = 1.0 ; double s [ ] = svd . getSingularValues ( ) ; for ( int i = 0 ; i < svd . numberOfSingularValues ( ) ; i ++ ) { b *= s [ i ] ; } b = Math . signum ( det ) / Math . pow ( b , 1.0 / 3.0 ) ; GeometryMath_F64 . scale ( a . T , b ) ; return true ; }


Input: <p > 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 . < / p > [CODESPLIT] public static double distance ( LineSegment2D_I32 line , Point2D_I32 p ) { int a = line . b . x - line . a . x ; int b = line . b . y - line . a . y ; double t = a * ( p . x - line . a . x ) + b * ( p . y - line . a . y ) ; t /= ( a * a + b * b ) ; // if the point of intersection is past the end points return the distance // from the closest end point if ( t < 0 ) { return UtilPoint2D_I32 . distance ( line . a . x , line . a . y , p . x , p . y ) ; } else if ( t > 1.0 ) return UtilPoint2D_I32 . distance ( line . b . x , line . b . y , p . x , p . y ) ; // return the distance of the closest point on the line return UtilPoint2D_F64 . distance ( line . a . x + t * a , line . a . y + t * b , p . x , p . y ) ; }


Input: <p > Returns the distance of a point to the closest point on a line . < / p > [CODESPLIT] public static double distance ( LineParametric2D_I32 line , Point2D_I32 p ) { int a = line . slopeX ; int b = line . slopeY ; double t = a * ( p . x - line . p . x ) + b * ( p . y - line . p . y ) ; t /= ( a * a + b * b ) ; // return the distance of the closest point on the line return UtilPoint2D_F64 . distance ( line . p . x + t * a , line . p . y + t * b , p . x , p . y ) ; }


Input: Converts latitude and longitude coordinates into a unit vector [CODESPLIT] public static < T extends GeoTuple3D_F64 < T > > T latlonToUnitVector ( double lat , double lon , T vector ) { if ( vector == null ) vector = ( T ) new Vector3D_F64 ( ) ; vector . x = Math . cos ( lat ) * Math . cos ( lon ) ; vector . y = Math . cos ( lat ) * Math . sin ( lon ) ; vector . z = - Math . sin ( lat ) ; return vector ; }


Input: Computes ( x - x_c ) ** 2 + ( y - y_c ) ** 2 - r . If ( x y ) lies on the circle then it should be 0 . [CODESPLIT] public static double evaluate ( double x , double y , Circle2D_F64 circle ) { x -= circle . center . x ; y -= circle . center . y ; return x * x + y * y - circle . radius * circle . radius ; }


Input: 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 . [CODESPLIT] public static boolean circle ( Point2D_F64 x0 , Point2D_F64 x1 , Point2D_F64 x2 , Circle2D_F64 circle ) { // points that lie on line a and b double xa = ( x0 . x + x1 . x ) / 2.0 ; double ya = ( x0 . y + x1 . y ) / 2.0 ; double xb = ( x1 . x + x2 . x ) / 2.0 ; double yb = ( x1 . y + x2 . y ) / 2.0 ; // slopes of lines a and b double m2 = x0 . x - x1 . x ; double m1 = x1 . y - x0 . y ; double n2 = x2 . x - x1 . x ; double n1 = x1 . y - x2 . y ; // find the intersection of the lines double bottom = m2 * n1 - n2 * m1 ; if ( bottom == 0 ) return false ; double alpha = ( - m2 * ( xb - xa ) + m1 * ( yb - ya ) ) / bottom ; circle . center . x = xb + n1 * alpha ; circle . center . y = yb + n2 * alpha ; circle . radius = circle . center . distance ( x0 ) ; return true ; }


Input: Radius squares of the circle that passes through these three points . [CODESPLIT] public static double circleRadiusSq ( Point2D_F64 x0 , Point2D_F64 x1 , Point2D_F64 x2 ) { // points that lie on line a and b double xa = ( x0 . x + x1 . x ) / 2.0 ; double ya = ( x0 . y + x1 . y ) / 2.0 ; double xb = ( x1 . x + x2 . x ) / 2.0 ; double yb = ( x1 . y + x2 . y ) / 2.0 ; // slopes of lines a and b double m2 = x0 . x - x1 . x ; double m1 = x1 . y - x0 . y ; double n2 = x2 . x - x1 . x ; double n1 = x1 . y - x2 . y ; // find the intersection of the lines double bottom = m2 * n1 - n2 * m1 ; if ( bottom == 0 ) return Double . NaN ; double alpha = ( - m2 * ( xb - xa ) + m1 * ( yb - ya ) ) / bottom ; double dx = xb + n1 * alpha - x0 . x ; double dy = yb + n2 * alpha - x0 . y ; return dx * dx + dy * dy ; }


Input: Returns the point which minimizes the distance between the two lines in 3D . If the two lines are parallel the result is undefined . [CODESPLIT] public static Point3D_F64 closestPoint ( LineParametric3D_F64 l0 , LineParametric3D_F64 l1 , Point3D_F64 ret ) { if ( ret == null ) { ret = new Point3D_F64 ( ) ; } ret . x = l0 . p . x - l1 . p . x ; ret . y = l0 . p . y - l1 . p . y ; ret . z = l0 . p . z - l1 . p . z ; // this solution is from: http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline3d/ double dv01v1 = MiscOps . dot ( ret , l1 . slope ) ; double dv1v0 = MiscOps . dot ( l1 . slope , l0 . slope ) ; double dv1v1 = MiscOps . dot ( l1 . slope , l1 . slope ) ; double t0 = dv01v1 * dv1v0 - MiscOps . dot ( ret , l0 . slope ) * dv1v1 ; double bottom = MiscOps . dot ( l0 . slope , l0 . slope ) * dv1v1 - dv1v0 * dv1v0 ; if ( bottom == 0 ) return null ; t0 /= bottom ; // ( d1343 + mua d4321 ) / d4343 double t1 = ( dv01v1 + t0 * dv1v0 ) / dv1v1 ; ret . x = ( double ) 0.5 * ( ( l0 . p . x + t0 * l0 . slope . x ) + ( l1 . p . x + t1 * l1 . slope . x ) ) ; ret . y = ( double ) 0.5 * ( ( l0 . p . y + t0 * l0 . slope . y ) + ( l1 . p . y + t1 * l1 . slope . y ) ) ; ret . z = ( double ) 0.5 * ( ( l0 . p . z + t0 * l0 . slope . z ) + ( l1 . p . z + t1 * l1 . slope . z ) ) ; return ret ; }


Input: <p > 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 . < / p > <p > point on l0 = l0 . a + param [ 0 ] * l0 . slope<br > point on l1 = l1 . a + param [ 1 ] * l1 . slope < / p > @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 . [CODESPLIT] public static boolean closestPoints ( LineParametric3D_F64 l0 , LineParametric3D_F64 l1 , double param [ ] ) { 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 ; }


Input: Finds the closest point on a line to the specified point . [CODESPLIT] public static Point3D_F64 closestPoint ( LineParametric3D_F64 line , Point3D_F64 pt , Point3D_F64 ret ) { if ( ret == null ) { ret = new Point3D_F64 ( ) ; } double dx = pt . x - line . p . x ; double dy = pt . y - line . p . y ; double dz = pt . z - line . p . z ; double n2 = line . slope . normSq ( ) ; double d = ( line . slope . x * dx + line . slope . y * dy + line . slope . z * dz ) ; ret . x = line . p . x + d * line . slope . x / n2 ; ret . y = line . p . y + d * line . slope . y / n2 ; ret . z = line . p . z + d * line . slope . z / n2 ; return ret ; }


Input: 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 . [CODESPLIT] public static double closestPoint ( LineParametric3D_F64 line , Point3D_F64 pt ) { 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 ( ) ; }


Input: Finds the closest point on the plane to the specified point . [CODESPLIT] public static Point3D_F64 closestPoint ( PlaneNormal3D_F64 plane , Point3D_F64 point , Point3D_F64 found ) { 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 ; }


Input: Finds the closest point on the plane to the specified point . [CODESPLIT] public static Point3D_F64 closestPoint ( PlaneGeneral3D_F64 plane , Point3D_F64 point , Point3D_F64 found ) { 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 ; }


Input: Finds the closest point on the plane to the origin . [CODESPLIT] public static Point3D_F64 closestPointOrigin ( PlaneGeneral3D_F64 plane , Point3D_F64 found ) { if ( found == null ) found = new Point3D_F64 ( ) ; double n2 = plane . A * plane . A + plane . B * plane . B + plane . C * plane . C ; found . x = plane . A * plane . D / n2 ; found . y = plane . B * plane . D / n2 ; found . z = plane . C * plane . D / n2 ; return found ; }


Input: Finds the closest point on a line segment to the specified point . [CODESPLIT] public static Point3D_F64 closestPoint ( LineSegment3D_F64 line , Point3D_F64 pt , Point3D_F64 ret ) { if ( ret == null ) { ret = new Point3D_F64 ( ) ; } double dx = pt . x - line . a . x ; double dy = pt . y - line . a . y ; double dz = pt . z - line . a . z ; double slope_x = line . b . x - line . a . x ; double slope_y = line . b . y - line . a . y ; double slope_z = line . b . z - line . a . z ; double n = ( double ) Math . sqrt ( slope_x * slope_x + slope_y * slope_y + slope_z * slope_z ) ; double d = ( slope_x * dx + slope_y * dy + slope_z * dz ) / n ; // if it is past the end points just return one of the end points if ( d <= 0 ) { ret . set ( line . a ) ; } else if ( d >= n ) { ret . set ( line . b ) ; } else { ret . x = line . a . x + d * slope_x / n ; ret . y = line . a . y + d * slope_y / n ; ret . z = line . a . z + d * slope_z / n ; } return ret ; }


Input: Find the point which minimizes its distance from the two line segments . [CODESPLIT] public static Point3D_F64 closestPoint ( LineSegment3D_F64 l0 , LineSegment3D_F64 l1 , Point3D_F64 ret ) { 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 ; }


Input: Closest point from a 3D triangle to a point . [CODESPLIT] public static Point3D_F64 closestPoint ( Point3D_F64 vertexA , Point3D_F64 vertexB , Point3D_F64 vertexC , Point3D_F64 point , Point3D_F64 ret ) { if ( ret == null ) { ret = new Point3D_F64 ( ) ; } DistancePointTriangle3D_F64 alg = new DistancePointTriangle3D_F64 ( ) ; alg . setTriangle ( vertexA , vertexB , vertexC ) ; alg . closestPoint ( point , ret ) ; return ret ; }


Input: <p > 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 ] < / p > [CODESPLIT] public static double closestPointT ( LineParametric3D_F64 line , PlaneNormal3D_F64 plane ) { 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 ; }


Input: 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 . [CODESPLIT] public static Point2D_F64 transform ( Homography2D_F64 H , Point2D_F64 orig , Point2D_F64 result ) { 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 ; }


Input: Applies a 2D special euclidean transform to the point and stores the results in another variable . [CODESPLIT] public static Point2D_F64 transform ( Se2_F64 se , Point2D_F64 orig , Point2D_F64 result ) { if ( result == null ) { result = new Point2D_F64 ( ) ; } final double c = se . getCosineYaw ( ) ; final double s = se . getSineYaw ( ) ; // copy the values so that no errors happen if orig and result are the same instance double x = orig . x ; double y = orig . y ; result . x = se . getX ( ) + x * c - y * s ; result . y = se . getY ( ) + x * s + y * c ; return result ; }


Input: Applies a 2D special euclidean transform to an array of points . [CODESPLIT] public static void transform ( Se2_F64 se , Point2D_F64 points [ ] , int length ) { double tranX = se . getX ( ) ; double tranY = se . getY ( ) ; final double c = se . getCosineYaw ( ) ; final double s = se . getSineYaw ( ) ; for ( int i = 0 ; i < length ; i ++ ) { Point2D_F64 pt = points [ i ] ; double x = pt . x ; double y = pt . y ; pt . x = tranX + x * c - y * s ; pt . y = tranY + x * s + y * c ; } }


Input: Applies a 3D special euclidean transform to a list of points . [CODESPLIT] public static void transform ( Se3_F64 se , Point3D_F64 [ ] points , int start , int length ) { for ( int i = 0 ; i < length ; i ++ ) { Point3D_F64 p = points [ i + start ] ; transform ( se , p , p ) ; } }


Input: Applies a 3D special euclidean transform to a list of points . [CODESPLIT] public static void transform ( Se3_F64 se , List < Point3D_F64 > points ) { for ( Point3D_F64 p : points ) { transform ( se , p , p ) ; } }


Input:
 . Applies the transform specified by SpecialEuclidean to a point .

 p = R * p + T

 Both origPt and tranPt can be the same instance .
 [CODESPLIT] public static Point3D_F64 transform ( Se3_F64 se , Point3D_F64 origPt , Point3D_F64 tranPt ) { 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 ; }


Input:
 . Applies the transform specified by SpecialEuclidean to a homogenous point .

 p = [ R t ] * p

 Both origPt and tranPt can be the same instance .
 [CODESPLIT] public static Point3D_F64 transform ( Se3_F64 se , Point4D_F64 origPt , Point3D_F64 tranPt ) { 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 ; }


Input:
 . Applies the transform in the reverse direction

 p = R T * ( p - T )

 Both origPt and tranPt can be the same instance .
 [CODESPLIT] public static Point3D_F64 transformReverse ( Se3_F64 se , Point3D_F64 origPt , Point3D_F64 tranPt ) { 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 ; }


Input: Converts the polygon into a list . [CODESPLIT] public List < Point2D_F64 > convert ( @ Nullable List < Point2D_F64 > storage , boolean copy ) { if ( storage == null ) storage = new ArrayList <> ( ) ; else storage . clear ( ) ; if ( copy ) { for ( int i = 0 ; i < 4 ; i ++ ) { storage . add ( get ( i ) . copy ( ) ) ; } } else { for ( int i = 0 ; i < 4 ; i ++ ) { storage . add ( get ( i ) ) ; } } return storage ; }


Input: 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 . [CODESPLIT] public void set ( List < Point2D_F64 > list ) { 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 ) ) ; }


Input: 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 . [CODESPLIT] public boolean isEquals ( Quadrilateral_F64 quad , double tol ) { tol *= tol ; if ( a . distance2 ( quad . a ) > tol ) return false ; if ( b . distance2 ( quad . b ) > tol ) return false ; if ( c . distance2 ( quad . c ) > tol ) return false ; return d . distance2 ( quad . d ) <= tol ; }


Input: Returns the acute angle between the slope of two lines . Lines do not need to ever intersect . Found using the dot product . [CODESPLIT] public static double acuteAngle ( LineGeneral2D_F64 a , LineGeneral2D_F64 b ) { double la = Math . sqrt ( a . A * a . A + a . B * a . B ) ; double lb = Math . sqrt ( b . A * b . A + b . B * b . B ) ; // numerical round off error can cause it to be barely greater than 1, which is outside the allowed // domain of acos() double value = ( a . A * b . A + a . B * b . B ) / ( la * lb ) ; if ( value < - 1.0 ) value = - 1.0 ; else if ( value > 1.0 ) value = 1.0 ; return Math . acos ( value ) ; }


Input: 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 . [CODESPLIT] public static double acuteAngleN ( LineGeneral2D_F64 a , LineGeneral2D_F64 b ) { 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 ) ; }


Input: Converts a line from polar form to parametric . [CODESPLIT] public static LineParametric2D_F64 convert ( LinePolar2D_F64 src , LineParametric2D_F64 ret ) { if ( ret == null ) ret = new LineParametric2D_F64 ( ) ; double c = ( double ) Math . cos ( src . angle ) ; double s = ( double ) Math . sin ( src . angle ) ; ret . p . set ( c * src . distance , s * src . distance ) ; ret . slope . set ( - s , c ) ; return ret ; }


Input: Converts a line from polar form to general . After conversion the line will be normalized e . g . A * A + B * B == 1 . [CODESPLIT] public static LineGeneral2D_F64 convert ( LinePolar2D_F64 src , LineGeneral2D_F64 ret ) { 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 ; }


Input: Converts a line from general to polar . [CODESPLIT] public static LinePolar2D_F64 convert ( LineGeneral2D_F64 src , LinePolar2D_F64 ret ) { if ( ret == null ) ret = new LinePolar2D_F64 ( ) ; double r = Math . sqrt ( src . A * src . A + src . B * src . B ) ; double sign = src . C < 0 ? - 1 : 1 ; ret . angle = Math . atan2 ( - sign * src . B / r , - sign * src . A / r ) ; ret . distance = sign * src . C / r ; return ret ; }


Input: 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 [CODESPLIT] public static LineParametric2D_F64 convert ( LineSegment2D_F64 src , LineParametric2D_F64 ret ) { if ( ret == null ) ret = new LineParametric2D_F64 ( ) ; ret . p . set ( src . a ) ; ret . slope . set ( src . slopeX ( ) , src . slopeY ( ) ) ; return ret ; }


Input: Converts a line segment into a general line . [CODESPLIT] public static LineGeneral2D_F64 convert ( LineSegment2D_F64 src , LineGeneral2D_F64 ret ) { return convert ( src . a , src . b , ret ) ; }


Input: Converts a line segment into a general line . Line segment is defined by two points . [CODESPLIT] public static LineGeneral2D_F64 convert ( Point2D_F64 a , Point2D_F64 b , LineGeneral2D_F64 ret ) { if ( ret == null ) ret = new LineGeneral2D_F64 ( ) ; ret . A = a . y - b . y ; ret . B = b . x - a . x ; ret . C = - ( ret . A * a . x + ret . B * a . y ) ; return ret ; }


Input: Converts a line segment into a line in parametric format . It will point from a to b . Point a and b must be unique . [CODESPLIT] public static LineParametric2D_F64 convert ( Point2D_F64 a , Point2D_F64 b , LineParametric2D_F64 ret ) { if ( ret == null ) ret = new LineParametric2D_F64 ( ) ; ret . p . set ( a ) ; ret . slope . x = b . x - a . x ; ret . slope . y = b . y - a . y ; return ret ; }


Input: Converts a line from parametric to polar . [CODESPLIT] public static LinePolar2D_F64 convert ( LineParametric2D_F64 src , LinePolar2D_F64 ret ) { if ( ret == null ) ret = new LinePolar2D_F64 ( ) ; double top = src . slope . y * src . p . x - src . slope . x * src . p . y ; ret . distance = top / src . slope . norm ( ) ; ret . angle = Math . atan2 ( - src . slope . x , src . slope . y ) ; if ( ret . distance < 0 ) { ret . distance = - ret . distance ; ret . angle = UtilAngle . bound ( ret . angle + Math . PI ) ; } return ret ; }


Input: Converts a line from parametric to general [CODESPLIT] public static LineGeneral2D_F64 convert ( LineParametric2D_F64 src , LineGeneral2D_F64 ret ) { if ( ret == null ) { ret = new LineGeneral2D_F64 ( ) ; } ret . A = - src . slope . y ; ret . B = src . slope . x ; ret . C = - ret . A * src . p . x - ret . B * src . p . y ; return ret ; }


Input: Converts a line from general to parametric [CODESPLIT] public static LineParametric2D_F64 convert ( LineGeneral2D_F64 src , LineParametric2D_F64 ret ) { if ( ret == null ) { ret = new LineParametric2D_F64 ( ) ; } ret . slope . x = src . B ; ret . slope . y = - src . A ; // find a point on the line if ( Math . abs ( src . B ) > Math . abs ( src . A ) ) { ret . p . y = - src . C / src . B ; ret . p . x = 0 ; } else { ret . p . x = - src . C / src . A ; ret . p . y = 0 ; } return ret ; }


Input: Converts a plane in normal form into a general equation [CODESPLIT] public static PlaneGeneral3D_F64 convert ( PlaneNormal3D_F64 input , PlaneGeneral3D_F64 output ) { if ( output == null ) output = new PlaneGeneral3D_F64 ( ) ; Vector3D_F64 n = input . n ; Point3D_F64 p = input . p ; output . A = n . x ; output . B = n . y ; output . C = n . z ; output . D = n . x * p . x + n . y * p . y + n . z * p . z ; return output ; }


Input: <p > 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 > [CODESPLIT] public static PlaneNormal3D_F64 convert ( PlaneGeneral3D_F64 input , PlaneNormal3D_F64 output ) { 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 ; }


Input: Converts a plane in tangent form into a plane in normal form [CODESPLIT] public static PlaneNormal3D_F64 convert ( PlaneTangent3D_F64 input , PlaneNormal3D_F64 output ) { if ( output == null ) output = new PlaneNormal3D_F64 ( ) ; // the value of input is a vector normal to the plane and a point on the plane. output . n . x = input . x ; output . n . y = input . y ; output . n . z = input . z ; output . p . set ( input ) ; return output ; }


Input: 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 . [CODESPLIT] public static PlaneNormal3D_F64 convert ( Se3_F64 planeToWorld , PlaneNormal3D_F64 output ) { if ( output == null ) output = new PlaneNormal3D_F64 ( ) ; // the value of input is a vector normal to the plane and a point on the plane. output . n . x = planeToWorld . R . unsafe_get ( 0 , 2 ) ; output . n . y = planeToWorld . R . unsafe_get ( 1 , 2 ) ; output . n . z = planeToWorld . R . unsafe_get ( 2 , 2 ) ; output . p . set ( planeToWorld . T . x , planeToWorld . T . y , planeToWorld . T . z ) ; return output ; }


Input: Converts the plane into Hessian normal form . This is done by dividing each coefficient by the Euclidean norm of ( A B C ) . [CODESPLIT] public static void hessianNormalForm ( PlaneGeneral3D_F64 plane ) { 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 ; }


Input: Applies the plane s definition to test to see if a point is one the plane [CODESPLIT] public static double evaluate ( PlaneGeneral3D_F64 plane , Point3D_F64 point ) { return plane . A * point . x + plane . B * point . y + plane . C * point . z - plane . D ; }


Input: Applies the plane s definition to test to see if a point is one the plane [CODESPLIT] public static double evaluate ( PlaneNormal3D_F64 plane , Point3D_F64 point ) { 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 ; }


Input: 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 . [CODESPLIT] public static void selectAxis2D ( Vector3D_F64 normal , Vector3D_F64 axisX , Vector3D_F64 axisY ) { UtilVector3D_F64 . perpendicularCanonical ( normal , axisX ) ; axisX . normalize ( ) ; axisY . cross ( normal , axisX ) ; axisY . normalize ( ) ; }


Input: 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 [CODESPLIT] public static void point3Dto2D ( Point3D_F64 pointOnPlane , Vector3D_F64 axisX , Vector3D_F64 axisY , Point3D_F64 A , Point2D_F64 output ) { 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 ; }


Input: Given a point on the plane s 2D coordinate system convert it back into a 3D point . [CODESPLIT] public static void point2Dto3D ( Point3D_F64 origin , Vector3D_F64 axisX , Vector3D_F64 axisY , Point2D_F64 A , Point3D_F64 output ) { output . x = origin . x + axisX . x * A . x + axisY . y * A . y ; output . y = origin . y + axisX . y * A . x + axisY . y * A . y ; output . z = origin . z + axisX . z * A . x + axisY . y * A . y ; }


Input: 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 . [CODESPLIT] public static Se3_F64 planeToWorld ( PlaneGeneral3D_F64 plane , Se3_F64 planeToWorld ) { if ( planeToWorld == null ) planeToWorld = new Se3_F64 ( ) ; Vector3D_F64 axisZ = new Vector3D_F64 ( plane . A , plane . B , plane . C ) ; axisZ . normalize ( ) ; Vector3D_F64 axisX = new Vector3D_F64 ( ) ; Vector3D_F64 axisY = new Vector3D_F64 ( ) ; UtilPlane3D_F64 . selectAxis2D ( axisZ , axisX , axisY ) ; return planeToWorld ( plane , axisX , axisY , axisZ , planeToWorld ) ; }


Input: Checks to see if the two GeoTuple have values which are nearly the same . False is always returned if the dimension is different . [CODESPLIT] public boolean isIdentical ( T t , double tol ) { if ( t . getDimension ( ) != getDimension ( ) ) return false ; int N = getDimension ( ) ; for ( int i = 0 ; i < N ; i ++ ) { double diff = Math . abs ( getIdx ( i ) - t . getIdx ( i ) ) ; if ( diff > tol ) return false ; } return true ; }


Input: Generic copy routine . It is recommended that this be overridden with a faster implementation . [CODESPLIT] @ Override public T copy ( ) { T ret = createNewInstance ( ) ; int N = getDimension ( ) ; for ( int i = 0 ; i < N ; i ++ ) { ret . setIdx ( i , getIdx ( i ) ) ; } return ret ; }


Input: Computes the square of the Euclidean norm . [CODESPLIT] public double normSq ( ) { double total = 0 ; int N = getDimension ( ) ; for ( int i = 0 ; i < N ; i ++ ) { double a = getIdx ( i ) ; total += a * a ; } return total ; }


Input: Converts a { @link LineSegment3D_F64 } into { @link LineParametric3D_F64 } . [CODESPLIT] public static LineParametric3D_F64 convert ( LineSegment3D_F64 line , LineParametric3D_F64 output ) { 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 ; }


Input: Computes the value of T for a point on the parametric line [CODESPLIT] public static double computeT ( LineParametric3D_F64 line , Point3D_F64 pointOnLine ) { double dx = pointOnLine . x - line . p . x ; double dy = pointOnLine . y - line . p . y ; double dz = pointOnLine . z - line . p . z ; double adx = Math . abs ( dx ) ; double ady = Math . abs ( dy ) ; double adz = Math . abs ( dz ) ; double t ; if ( adx > ady ) { if ( adx > adz ) { t = dx / line . slope . x ; } else { t = dz / line . slope . z ; } } else if ( ady > adz ) { t = dy / line . slope . y ; } else { t = dz / line . slope . z ; } return t ; }


Input: Returns true if any of its parameters have an uncountable number [CODESPLIT] public boolean hasUncountable ( ) { return UtilEjml . isUncountable ( A ) || UtilEjml . isUncountable ( C ) || UtilEjml . isUncountable ( D ) || UtilEjml . isUncountable ( E ) || UtilEjml . isUncountable ( F ) ; }


Input: Determines if they are equivalent up to a scale factor [CODESPLIT] public boolean isEquivalent ( ParabolaGeneral_F64 parabola , double tol ) { double scale = relativeScale ( parabola ) ; if ( Math . abs ( A * scale - parabola . A ) > tol ) return false ; if ( Math . abs ( C * scale - parabola . C ) > tol ) return false ; if ( Math . abs ( D * scale - parabola . D ) > tol ) return false ; if ( Math . abs ( E * scale - parabola . E ) > tol ) return false ; if ( Math . abs ( F * scale - parabola . F ) > tol ) return false ; return true ; }


Input: <p > 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 . < / p > [CODESPLIT] public static LinePolar2D_F32 polar ( List < Point2D_I32 > points , int start , int length , LinePolar2D_F32 ret ) { 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 ; }


Input: The box s area . area = lengthX * lengthY * lengthZ [CODESPLIT] public double area ( ) { return ( p1 . x - p0 . x ) * ( p1 . y - p0 . y ) * ( p1 . z - p0 . z ) ; }


Input: Computes and return the center of the cube . [CODESPLIT] public Point3D_F64 center ( Point3D_F64 storage ) { if ( storage == null ) storage = new Point3D_F64 ( ) ; storage . x = ( p0 . x + p1 . x ) / 2.0 ; storage . y = ( p0 . y + p1 . y ) / 2.0 ; storage . z = ( p0 . z + p1 . z ) / 2.0 ; return storage ; }


Input: Computes the acute angle between the two lines . Does not check for intersection [CODESPLIT] public static double acuteAngle ( LineSegment2D_I32 line0 , LineSegment2D_I32 line1 ) { int dx0 = line0 . b . x - line0 . a . x ; int dy0 = line0 . b . y - line0 . a . y ; int dx1 = line1 . b . x - line1 . a . x ; int dy1 = line1 . b . y - line1 . a . y ; double bottom = Math . sqrt ( dx0 * dx0 + dy0 * dy0 ) * Math . sqrt ( dx1 * dx1 + dy1 * dy1 ) ; return Math . acos ( ( dx0 * dx1 + dy0 * dy1 ) / bottom ) ; }


Input: Given two sets of corresponding points compute the { @link Se2_F64 } transform which minimizes the difference between the two sets of points . [CODESPLIT] public static Se2_F64 fitPoints2D ( List < Point2D_F64 > from , List < Point2D_F64 > to ) { MotionTransformPoint < Se2_F64 , Point2D_F64 > alg = fitPoints2D ( ) ; alg . process ( from , to ) ; return alg . getTransformSrcToDst ( ) ; }


Input: Given two sets of corresponding points compute the { @link Se3_F64 } transform which minimizes the difference between the two sets of points . [CODESPLIT] public static Se3_F64 fitPoints3D ( List < Point3D_F64 > from , List < Point3D_F64 > to ) { MotionTransformPoint < Se3_F64 , Point3D_F64 > alg = fitPoints3D ( ) ; alg . process ( from , to ) ; return alg . getTransformSrcToDst ( ) ; }


Input: Checks to see if the homogenous 3D point lies on the plane at infinity [CODESPLIT] public static boolean isInfiniteH ( Point4D_F64 p , double tol ) { double n = Math . sqrt ( p . x * p . x + p . y * p . y + p . z * p . z ) ; return Math . abs ( p . w ) <= n * tol ; }


Input: Normally distributed homogenous 3D point . w is fixed [CODESPLIT] public static List < Point4D_F64 > randomN ( Point3D_F64 center , double w , double stdev , int num , Random rand ) { List < Point4D_F64 > ret = new ArrayList <> ( ) ; for ( int i = 0 ; i < num ; i ++ ) { Point4D_F64 p = new Point4D_F64 ( ) ; p . x = center . x + rand . nextGaussian ( ) * stdev ; p . y = center . y + rand . nextGaussian ( ) * stdev ; p . z = center . z + rand . nextGaussian ( ) * stdev ; p . w = w ; ret . add ( p ) ; } return ret ; }


Input: Converts a point from homogenous coordinates into Euclidean [CODESPLIT] public static Point3D_F64 h_to_e ( Point4D_F64 p ) { Point3D_F64 out = new Point3D_F64 ( ) ; h_to_e ( p , out ) ; return out ; }


Input: Applies a 2D affine transform to the point and stores the results in another variable . [CODESPLIT] public static Vector2D_F64 transform ( Affine2D_F64 se , Vector2D_F64 orig , Vector2D_F64 result ) { 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 ; }


Input: Applies a 2D affine transform to the point and stores the results in another variable . [CODESPLIT] public static Point2D_F64 transform ( Affine2D_F64 se , Point2D_F64 orig , Point2D_F64 result ) { 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 ; }


Input: Returns the acute angle between the two vectors . Computed using the dot product . [CODESPLIT] public static double acute ( Vector2D_F64 a , Vector2D_F64 b ) { 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 ) ; }


Input: Sets the vector equal to a - b . [CODESPLIT] public static Vector2D_F64 minus ( Point2D_F64 a , Point2D_F64 b , Vector2D_F64 output ) { if ( output == null ) output = new Vector2D_F64 ( ) ; output . x = a . x - b . x ; output . y = a . y - b . y ; return output ; }


Input: Tests to see if the two vectors are identical up to a sign difference [CODESPLIT] public static boolean identicalSign ( double xa , double ya , double xb , double yb , double tol ) { double dx0 = xb - xa ; double dy0 = yb - ya ; double dx1 = xb + xa ; double dy1 = yb + ya ; double error0 = dx0 * dx0 + dy0 * dy0 ; double error1 = dx1 * dx1 + dy1 * dy1 ; if ( error0 < error1 ) { return error0 <= tol * tol ; } else { return error1 <= tol * tol ; } }


Input: Sets this rectangle to be equal to the passed in rectangle . [CODESPLIT] public void set ( RectangleLength2D_I32 r ) { this . x0 = r . x0 ; this . y0 = r . y0 ; this . width = r . width ; this . height = r . height ; }


Input: In - place minus operation . this = a - b . [CODESPLIT] public void minus ( Point3D_F64 a , Point3D_F64 b ) { x = a . x - b . x ; y = a . y - b . y ; z = a . z - b . z ; }


Input: Dot product between this and a = this . x * a . x + this . y * a . y + this . z * a . z [CODESPLIT] public double dot ( Vector3D_F64 a ) { return x * a . x + y * a . y + z * a . z ; }


Input: <p > Convert from quadratic to rotated formats . Equations taken from [ 1 ] . < / p > [CODESPLIT] public static EllipseRotated_F64 convert ( EllipseQuadratic_F64 input , EllipseRotated_F64 output ) { 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 ; }


Input: Convert from rotated to quadratic . [CODESPLIT] public static EllipseQuadratic_F64 convert ( EllipseRotated_F64 input , EllipseQuadratic_F64 output ) { if ( output == null ) output = new EllipseQuadratic_F64 ( ) ; double x0 = input . center . x ; double y0 = input . center . y ; double a = input . a ; double b = input . b ; double phi = input . phi ; double cphi = Math . cos ( phi ) ; double sphi = Math . sin ( phi ) ; double cphi2 = cphi * cphi ; double sphi2 = sphi * sphi ; double a2 = a * a ; double b2 = b * b ; double x02 = x0 * x0 ; double y02 = y0 * y0 ; // TODO simplfy using more trig identities output . A = cphi2 / a2 + sphi2 / b2 ; output . B = sphi * cphi / a2 - sphi * cphi / b2 ; output . C = sphi2 / a2 + cphi2 / b2 ; output . D = - x0 * cphi2 / a2 - y0 * sphi * cphi / a2 - x0 * sphi2 / b2 + y0 * sphi * cphi / b2 ; output . E = - x0 * sphi * cphi / a2 - y0 * sphi2 / a2 + x0 * sphi * cphi / b2 - y0 * cphi2 / b2 ; output . F = x02 * cphi2 / a2 + 2 * x0 * y0 * sphi * cphi / a2 + y02 * sphi2 / a2 + x02 * sphi2 / b2 - 2 * x0 * y0 * sphi * cphi / b2 + y02 * cphi2 / b2 - 1 ; return output ; }


Input: Computes the value of the quadratic ellipse function at point ( x y ) . Should equal 0 if the point is along the ellipse . [CODESPLIT] public static double evaluate ( double x , double y , EllipseQuadratic_F64 ellipse ) { return ellipse . A * x * x + 2 * ellipse . B * x * y + ellipse . C * y * y + 2 * ellipse . D * x + 2 * ellipse . E * y + ellipse . F ; }


Input: Computes the value of the quadratic ellipse function at point ( x y ) . Should equal 1 if the point is on the ellipse . [CODESPLIT] public static double evaluate ( double x , double y , EllipseRotated_F64 ellipse ) { double cphi = Math . cos ( ellipse . phi ) ; double sphi = Math . sin ( ellipse . phi ) ; x -= ellipse . center . x ; y -= ellipse . center . y ; double left = ( x * cphi + y * sphi ) ; double right = ( - x * sphi + y * cphi ) ; double ll = left / ellipse . a ; double rr = right / ellipse . b ; return ll * ll + rr * rr ; }


Input: Computes the point on the ellipse at location t where t is an angle in radians [CODESPLIT] public static Point2D_F64 computePoint ( double t , EllipseRotated_F64 ellipse , Point2D_F64 output ) { if ( output == null ) output = new Point2D_F64 ( ) ; double ct = Math . cos ( t ) ; double st = Math . sin ( t ) ; double cphi = Math . cos ( ellipse . phi ) ; double sphi = Math . sin ( ellipse . phi ) ; // coordinate in ellipse frame double x = ellipse . a * ct ; double y = ellipse . b * st ; // put into global frame output . x = ellipse . center . x + x * cphi - y * sphi ; output . y = ellipse . center . y + x * sphi + y * cphi ; return output ; }


Input: Computes the value of t used to specify a point s location [CODESPLIT] public static double computeAngle ( Point2D_F64 p , EllipseRotated_F64 ellipse ) { // put point into ellipse's reference frame double ce = Math . cos ( ellipse . phi ) ; double se = Math . sin ( ellipse . phi ) ; // world into ellipse frame double xc = p . x - ellipse . center . x ; double yc = p . y - ellipse . center . y ; double x = ce * xc + se * yc ; double y = - se * xc + ce * yc ; return Math . atan2 ( y / ellipse . b , x / ellipse . a ) ; }


Input: Computes the tangent to the ellipse at the specified location [CODESPLIT] public static Vector2D_F64 computeTangent ( double t , EllipseRotated_F64 ellipse , Vector2D_F64 output ) { if ( output == null ) output = new Vector2D_F64 ( ) ; double ct = Math . cos ( t ) ; double st = Math . sin ( t ) ; double cphi = Math . cos ( ellipse . phi ) ; double sphi = Math . sin ( ellipse . phi ) ; // point in ellipse frame multiplied by b^2 and a^2 double x = ellipse . a * ct * ellipse . b * ellipse . b ; double y = ellipse . b * st * ellipse . a * ellipse . a ; // rotate vector normal into world frame double rx = x * cphi - y * sphi ; double ry = x * sphi + y * cphi ; // normalize and change into tangent double r = Math . sqrt ( rx * rx + ry * ry ) ; output . x = - ry / r ; output . y = rx / r ; return output ; }


Input: <p > Finds two points on the ellipse that in combination with point pt each define a line that is tangent to the ellipse . < / p > [CODESPLIT] public static boolean tangentLines ( Point2D_F64 pt , EllipseRotated_F64 ellipse , Point2D_F64 tangentA , Point2D_F64 tangentB ) { // Derivation: // Compute the tangent at only point along the ellipse by computing dy/dx //    x*b^2/(y*a^2) or - x*b^2/(y*a^2)  are the possible solutions for the tangent // The slope of the line and the gradient are the same, so this is true: //   y - y'     -x*b^2 //  -------  =  ------- //   x - x'      y*a^2 // //  (x,y) is point on ellipse, (x',y') is pt that lines pass through // //  that becomes //  y^2*a^2 + x^2*b^2 = x'*x*b^2 + y'*y*a^2 //  use the equation for the ellipse (centered and aligned at origin) //  a^2*b^2 =  x'*x*b^2 + y'*y*a^2 // // solve for y // plug into ellipse equation // solve for x, which is a quadratic equation // translate and rotate into ellipse reference frame double cphi = Math . cos ( ellipse . phi ) ; double sphi = Math . sin ( ellipse . phi ) ; double tmpx = pt . x - ellipse . center . x ; double tmpy = pt . y - ellipse . center . y ; double xt = tmpx * cphi + tmpy * sphi ; double yt = - tmpx * sphi + tmpy * cphi ; // solve double a2 = ellipse . a * ellipse . a ; double b2 = ellipse . b * ellipse . b ; // quadratic equation for the two variants. // solving for x double aa0 = yt * yt / b2 + xt * xt / a2 ; double bb0 = - 2.0 * xt ; double cc0 = a2 * ( 1.0 - yt * yt / b2 ) ; double descriminant0 = bb0 * bb0 - 4.0 * aa0 * cc0 ; // solving for y double aa1 = xt * xt / a2 + yt * yt / b2 ; double bb1 = - 2.0 * yt ; double cc1 = b2 * ( 1.0 - xt * xt / a2 ) ; double descriminant1 = bb1 * bb1 - 4.0 * aa1 * cc1 ; double x0 , y0 , x1 , y1 ; if ( descriminant0 < 0 && descriminant1 < 0 ) { return false ; } else if ( descriminant0 > descriminant1 ) { if ( yt == 0 ) return false ; double right = Math . sqrt ( descriminant0 ) ; x0 = ( - bb0 + right ) / ( 2.0 * aa0 ) ; x1 = ( - bb0 - right ) / ( 2.0 * aa0 ) ; y0 = b2 / yt - xt * x0 * b2 / ( yt * a2 ) ; y1 = b2 / yt - xt * x1 * b2 / ( yt * a2 ) ; } else { if ( xt == 0 ) return false ; double right = Math . sqrt ( descriminant1 ) ; y0 = ( - bb1 + right ) / ( 2.0 * aa1 ) ; y1 = ( - bb1 - right ) / ( 2.0 * aa1 ) ; x0 = a2 / xt - yt * y0 * a2 / ( xt * b2 ) ; x1 = a2 / xt - yt * y1 * a2 / ( xt * b2 ) ; } // convert the lines back into world space tangentA . x = x0 * cphi - y0 * sphi + ellipse . center . x ; tangentA . y = x0 * sphi + y0 * cphi + ellipse . center . y ; tangentB . x = x1 * cphi - y1 * sphi + ellipse . center . x ; tangentB . y = x1 * sphi + y1 * cphi + ellipse . center . y ; return true ; }


Input: <p > Finds four lines which are tangent to both ellipses . Both ellipses must not intersect . Line 0 and line 3 will not intersect the line joining the center of the two ellipses while line 1 and 2 will . < / p > [CODESPLIT] public static boolean tangentLines ( EllipseRotated_F64 ellipseA , EllipseRotated_F64 ellipseB , Point2D_F64 tangentA0 , Point2D_F64 tangentA1 , Point2D_F64 tangentA2 , Point2D_F64 tangentA3 , Point2D_F64 tangentB0 , Point2D_F64 tangentB1 , Point2D_F64 tangentB2 , Point2D_F64 tangentB3 ) { TangentLinesTwoEllipses_F64 alg = new TangentLinesTwoEllipses_F64 ( GrlConstants . TEST_F64 , 10 ) ; return alg . process ( ellipseA , ellipseB , tangentA0 , tangentA1 , tangentA2 , tangentA3 , tangentB0 , tangentB1 , tangentB2 , tangentB3 ) ; }