static OPUS_INLINE void silk_LDL_FLP(
silk_float *A, /* I/O Pointer to Symetric Square Matrix */
opus_int M, /* I Size of Matrix */
silk_float *L, /* I/O Pointer to Square Upper triangular Matrix */
silk_float *Dinv /* I/O Pointer to vector holding the inverse diagonal elements of D */
)
{
opus_int i, j, k, loop_count, err = 1;
silk_float *ptr1, *ptr2;
double temp, diag_min_value;
silk_float v[ MAX_MATRIX_SIZE ] = { 0 }, D[ MAX_MATRIX_SIZE ]; /* temp arrays*/
silk_assert(M <= MAX_MATRIX_SIZE);
diag_min_value = FIND_LTP_COND_FAC * 0.5f * (A[ 0 ] + A[ M * M - 1 ]);
for(loop_count = 0; loop_count < M && err == 1; loop_count++) {
err = 0;
for(j = 0; j < M; j++) {
ptr1 = matrix_adr(L, j, 0, M);
temp = matrix_ptr(A, j, j, M); /* element in row j column j*/
for(i = 0; i < j; i++) {
v[ i ] = ptr1[ i ] * D[ i ];
temp -= ptr1[ i ] * v[ i ];
}
if(temp < diag_min_value) {
/* Badly conditioned matrix: add white noise and run again */
temp = (loop_count + 1) * diag_min_value - temp;
for(i = 0; i < M; i++) {
matrix_ptr(A, i, i, M) += (silk_float)temp;
}
err = 1;
break;
}
D[ j ] = (silk_float)temp;
Dinv[ j ] = (silk_float)(1.0f / temp);
matrix_ptr(L, j, j, M) = 1.0f;
ptr1 = matrix_adr(A, j, 0, M);
ptr2 = matrix_adr(L, j + 1, 0, M);
for(i = j + 1; i < M; i++) {
temp = 0.0;
for(k = 0; k < j; k++) {
temp += ptr2[ k ] * v[ k ];
}
matrix_ptr(L, i, j, M) = (silk_float)((ptr1[ i ] - temp) * Dinv[ j ]);
ptr2 += M; /* go to next column*/
}
}
}
silk_assert(err == 0);
}
/* Solve L^t*x = b, where L is lower triangular with ones on the diagonal */
SKP_INLINE void SKP_Silk_LS_SolveLast_FIX(
const SKP_int32 *L_Q16, /* I Pointer to Lower Triangular Matrix */
const SKP_int M, /* I Dim of Matrix equation */
const SKP_int32 *b, /* I b Vector */
SKP_int32 *x_Q16 /* O x Vector */
)
{
SKP_int i, j;
const SKP_int32 *ptr32;
SKP_int32 tmp_32;
for( i = M - 1; i >= 0; i-- ) {
ptr32 = matrix_adr( L_Q16, 0, i, M );
tmp_32 = 0;
for( j = M - 1; j > i; j-- ) {
tmp_32 = SKP_SMLAWW( tmp_32, ptr32[ SKP_SMULBB( j, M ) ], x_Q16[ j ] );
}
x_Q16[ i ] = SKP_SUB32( b[ i ], tmp_32 );
}
}
/* Solve Lx = b, when L is lower triangular and has ones on the diagonal */
SKP_INLINE void SKP_Silk_LS_SolveFirst_FIX(
const SKP_int32 *L_Q16, /* I Pointer to Lower Triangular Matrix */
SKP_int M, /* I Dim of Matrix equation */
const SKP_int32 *b, /* I b Vector */
SKP_int32 *x_Q16 /* O x Vector */
)
{
SKP_int i, j;
const SKP_int32 *ptr32;
SKP_int32 tmp_32;
for( i = 0; i < M; i++ ) {
ptr32 = matrix_adr( L_Q16, i, 0, M );
tmp_32 = 0;
for( j = 0; j < i; j++ ) {
tmp_32 = SKP_SMLAWW( tmp_32, ptr32[ j ], x_Q16[ j ] );
}
x_Q16[ i ] = SKP_SUB32( b[ i ], tmp_32 );
}
}
/* Solve L^t*x = b, where L is lower triangular with ones on the diagonal */
static inline void silk_LS_SolveLast_FIX(
const opus_int32 *L_Q16, /* I Pointer to Lower Triangular Matrix */
const opus_int M, /* I Dim of Matrix equation */
const opus_int32 *b, /* I b Vector */
opus_int32 *x_Q16 /* O x Vector */
)
{
opus_int i, j;
const opus_int32 *ptr32;
opus_int32 tmp_32;
for( i = M - 1; i >= 0; i-- ) {
ptr32 = matrix_adr( L_Q16, 0, i, M );
tmp_32 = 0;
for( j = M - 1; j > i; j-- ) {
tmp_32 = silk_SMLAWW( tmp_32, ptr32[ silk_SMULBB( j, M ) ], x_Q16[ j ] );
}
x_Q16[ i ] = silk_SUB32( b[ i ], tmp_32 );
}
}
static OPUS_INLINE void silk_SolveWithLowerTriangularWdiagOnes_FLP(
const silk_float *L, /* I Pointer to Lower Triangular Matrix */
opus_int M, /* I Dim of Matrix equation */
const silk_float *b, /* I b Vector */
silk_float *x /* O x Vector */
)
{
opus_int i, j;
silk_float temp;
const silk_float *ptr1;
for(i = 0; i < M; i++) {
ptr1 = matrix_adr(L, i, 0, M);
temp = 0;
for(j = 0; j < i; j++) {
temp += ptr1[ j ] * x[ j ];
}
temp = b[ i ] - temp;
x[ i ] = temp;
}
}
static OPUS_INLINE void silk_SolveWithUpperTriangularFromLowerWdiagOnes_FLP(
const silk_float *L, /* I Pointer to Lower Triangular Matrix */
opus_int M, /* I Dim of Matrix equation */
const silk_float *b, /* I b Vector */
silk_float *x /* O x Vector */
)
{
opus_int i, j;
silk_float temp;
const silk_float *ptr1;
for(i = M - 1; i >= 0; i--) {
ptr1 = matrix_adr(L, 0, i, M);
temp = 0;
for(j = M - 1; j > i ; j--) {
temp += ptr1[ j * M ] * x[ j ];
}
temp = b[ i ] - temp;
x[ i ] = temp;
}
}
void SKP_Silk_SolveWithLowerTriangularWdiagOnes_FLP(
const SKP_float *L, /* (I) Pointer to Lower Triangular Matrix */
SKP_int M, /* (I) Dim of Matrix equation */
const SKP_float *b, /* (I) b Vector */
SKP_float *x /* (O) x Vector */
)
{
SKP_int i, j;
SKP_float temp;
const SKP_float *ptr1;
for( i = 0; i < M; i++ ) {
ptr1 = matrix_adr( L, i, 0, M );
temp = 0;
for( j = 0; j < i; j++ ) {
temp += ptr1[ j ] * x[ j ];
}
temp = b[ i ] - temp;
x[ i ] = temp;
}
}
void SKP_Silk_SolveWithUpperTriangularFromLowerWdiagOnes_FLP(
const SKP_float *L, /* (I) Pointer to Lower Triangular Matrix */
SKP_int M, /* (I) Dim of Matrix equation */
const SKP_float *b, /* (I) b Vector */
SKP_float *x /* (O) x Vector */
)
{
SKP_int i, j;
SKP_float temp;
const SKP_float *ptr1;
for( i = M - 1; i >= 0; i-- ) {
ptr1 = matrix_adr( L, 0, i, M );
temp = 0;
for( j = M - 1; j > i ; j-- ) {
temp += ptr1[ j * M ] * x[ j ];
}
temp = b[ i ] - temp;
x[ i ] = temp;
}
}
SKP_INLINE void SKP_Silk_LDL_factorize_FIX(
SKP_int32 *A, /* I Pointer to Symetric Square Matrix */
SKP_int M, /* I Size of Matrix */
SKP_int32 *L_Q16, /* I/O Pointer to Square Upper triangular Matrix */
inv_D_t *inv_D /* I/O Pointer to vector holding inverted diagonal elements of D */
)
{
SKP_int i, j, k, status, loop_count;
const SKP_int32 *ptr1, *ptr2;
SKP_int32 diag_min_value, tmp_32, err;
SKP_int32 v_Q0[ MAX_MATRIX_SIZE ], D_Q0[ MAX_MATRIX_SIZE ];
SKP_int32 one_div_diag_Q36, one_div_diag_Q40, one_div_diag_Q48;
SKP_assert( M <= MAX_MATRIX_SIZE );
status = 1;
diag_min_value = SKP_max_32( SKP_SMMUL( SKP_ADD_SAT32( A[ 0 ], A[ SKP_SMULBB( M, M ) - 1 ] ), SKP_FIX_CONST( FIND_LTP_COND_FAC, 31 ) ), 1 << 9 );
for( loop_count = 0; loop_count < M && status == 1; loop_count++ ) {
status = 0;
for( j = 0; j < M; j++ ) {
ptr1 = matrix_adr( L_Q16, j, 0, M );
tmp_32 = 0;
for( i = 0; i < j; i++ ) {
v_Q0[ i ] = SKP_SMULWW( D_Q0[ i ], ptr1[ i ] ); /* Q0 */
tmp_32 = SKP_SMLAWW( tmp_32, v_Q0[ i ], ptr1[ i ] ); /* Q0 */
}
tmp_32 = SKP_SUB32( matrix_ptr( A, j, j, M ), tmp_32 );
if( tmp_32 < diag_min_value ) {
tmp_32 = SKP_SUB32( SKP_SMULBB( loop_count + 1, diag_min_value ), tmp_32 );
/* Matrix not positive semi-definite, or ill conditioned */
for( i = 0; i < M; i++ ) {
matrix_ptr( A, i, i, M ) = SKP_ADD32( matrix_ptr( A, i, i, M ), tmp_32 );
}
status = 1;
break;
}
D_Q0[ j ] = tmp_32; /* always < max(Correlation) */
/* two-step division */
one_div_diag_Q36 = SKP_INVERSE32_varQ( tmp_32, 36 ); /* Q36 */
one_div_diag_Q40 = SKP_LSHIFT( one_div_diag_Q36, 4 ); /* Q40 */
err = SKP_SUB32( 1 << 24, SKP_SMULWW( tmp_32, one_div_diag_Q40 ) ); /* Q24 */
one_div_diag_Q48 = SKP_SMULWW( err, one_div_diag_Q40 ); /* Q48 */
/* Save 1/Ds */
inv_D[ j ].Q36_part = one_div_diag_Q36;
inv_D[ j ].Q48_part = one_div_diag_Q48;
matrix_ptr( L_Q16, j, j, M ) = 65536; /* 1.0 in Q16 */
ptr1 = matrix_adr( A, j, 0, M );
ptr2 = matrix_adr( L_Q16, j + 1, 0, M );
for( i = j + 1; i < M; i++ ) {
tmp_32 = 0;
for( k = 0; k < j; k++ ) {
tmp_32 = SKP_SMLAWW( tmp_32, v_Q0[ k ], ptr2[ k ] ); /* Q0 */
}
tmp_32 = SKP_SUB32( ptr1[ i ], tmp_32 ); /* always < max(Correlation) */
/* tmp_32 / D_Q0[j] : Divide to Q16 */
matrix_ptr( L_Q16, i, j, M ) = SKP_ADD32( SKP_SMMUL( tmp_32, one_div_diag_Q48 ),
SKP_RSHIFT( SKP_SMULWW( tmp_32, one_div_diag_Q36 ), 4 ) );
/* go to next column */
ptr2 += M;
}
}
}
SKP_assert( status == 0 );
}
