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