static void backsubstitution0134 ( DMatrixRMaj P_plus , DMatrixRMaj P , DMatrixRMaj X , double H [ ] ) { final int N = P . numRows ; DMatrixRMaj tmp = new DMatrixRMaj ( N * 2 , 1 ) ; double H6 = H [ 6 ] ; double H7 = H [ 7 ] ; double H8 = H [ 8 ] ; for ( int i = 0 , index = 0 ; i < N ; i ++ ) { double x = - X . data [ index ] , y = - X . data [ index + 1 ] ; double sum = P . data [ index ++ ] * H6 + P . data [ index ++ ] * H7 + H8 ; tmp . data [ i ] = x * sum ; tmp . data [ i + N ] = y * sum ; } double h0 = 0 , h1 = 0 , h3 = 0 , h4 = 0 ; for ( int i = 0 ; i < N ; i ++ ) { double P_pls_0 = P_plus . data [ i ] ; double P_pls_1 = P_plus . data [ i + N ] ; double tmp_i = tmp . data [ i ] ; double tmp_j = tmp . data [ i + N ] ; h0 += P_pls_0 * tmp_i ; h1 += P_pls_1 * tmp_i ; h3 += P_pls_0 * tmp_j ; h4 += P_pls_1 * tmp_j ; } H [ 0 ] = - h0 ; H [ 1 ] = - h1 ; H [ 3 ] = - h3 ; H [ 4 ] = - h4 ; }
Backsubstitution for solving for 0 1 and 3 4 using solution for 6 7 8