@ -954,7 +954,7 @@ double cv::invert( InputArray _src, OutputArray _dst, int method )
size_t esz = CV_ELEM_SIZE ( type ) ;
int m = src . rows , n = src . cols ;
if ( method = = DECOMP_SVD )
if ( method = = DECOMP_SVD )
{
int nm = std : : min ( m , n ) ;
@ -1101,62 +1101,21 @@ double cv::invert( InputArray _src, OutputArray _dst, int method )
result = true ;
d = 1. / d ;
# if CV_SSE2
if ( USE_SSE2 )
{
__m128 det = _mm_set1_ps ( ( float ) d ) ;
__m128 s0 = _mm_loadu_ps ( ( const float * ) srcdata ) ; //s0 = Sf(0,0) Sf(0,1) Sf(0,2) ***
__m128 s1 = _mm_loadu_ps ( ( const float * ) ( srcdata + srcstep ) ) ; //s1 = Sf(1,0) Sf(1,1) Sf(1,2) ***
__m128 s2 = _mm_set_ps ( 0.f , Sf ( 2 , 2 ) , Sf ( 2 , 1 ) , Sf ( 2 , 0 ) ) ; //s2 = Sf(2,0) Sf(2,1) Sf(2,2) ***
__m128 r0 = _mm_shuffle_ps ( s1 , s1 , _MM_SHUFFLE ( 3 , 0 , 2 , 1 ) ) ; //r0 = Sf(1,1) Sf(1,2) Sf(1,0) ***
__m128 r1 = _mm_shuffle_ps ( s2 , s2 , _MM_SHUFFLE ( 3 , 1 , 0 , 2 ) ) ; //r1 = Sf(2,2) Sf(2,0) Sf(2,1) ***
__m128 r2 = _mm_shuffle_ps ( s2 , s2 , _MM_SHUFFLE ( 3 , 0 , 2 , 1 ) ) ; //r2 = Sf(2,1) Sf(2,2) Sf(2,0) ***
__m128 t0 = _mm_mul_ps ( s0 , r0 ) ; //t0 = Sf(0,0)*Sf(1,1) Sf(0,1)*Sf(1,2) Sf(0,2)*Sf(1,0) ***
__m128 t1 = _mm_mul_ps ( s0 , r1 ) ; //t1 = Sf(0,0)*Sf(2,2) Sf(0,1)*Sf(2,0) Sf(0,2)*Sf(2,1) ***
__m128 t2 = _mm_mul_ps ( s1 , r2 ) ; //t2 = Sf(1,0)*Sf(2,1) Sf(1,1)*Sf(2,2) Sf(1,2)*Sf(2,0) ***
__m128 r3 = _mm_shuffle_ps ( s0 , s0 , _MM_SHUFFLE ( 3 , 0 , 2 , 1 ) ) ; //r3 = Sf(0,1) Sf(0,2) Sf(0,0) ***
__m128 r4 = _mm_shuffle_ps ( s0 , s0 , _MM_SHUFFLE ( 3 , 1 , 0 , 2 ) ) ; //r4 = Sf(0,2) Sf(0,0) Sf(0,1) ***
__m128 t00 = _mm_mul_ps ( s1 , r3 ) ; //t00 = Sf(1,0)*Sf(0,1) Sf(1,1)*Sf(0,2) Sf(1,2)*Sf(0,0) ***
__m128 t11 = _mm_mul_ps ( s2 , r4 ) ; //t11 = Sf(2,0)*Sf(0,2) Sf(2,1)*Sf(0,0) Sf(2,2)*Sf(0,1) ***
__m128 t22 = _mm_mul_ps ( s2 , r0 ) ; //t22 = Sf(2,0)*Sf(1,1) Sf(2,1)*Sf(1,2) Sf(2,2)*Sf(1,0) ***
t0 = _mm_mul_ps ( _mm_sub_ps ( t0 , t00 ) , det ) ; //Sf(0,0)*Sf(1,1) Sf(0,1)*Sf(1,2) Sf(0,2)*Sf(1,0) ***
//-Sf(1,0)*Sf(0,1) -Sf(1,1)*Sf(0,2) -Sf(1,2)*Sf(0,0)
t1 = _mm_mul_ps ( _mm_sub_ps ( t1 , t11 ) , det ) ; //Sf(0,0)*Sf(2,2) Sf(0,1)*Sf(2,0) Sf(0,2)*Sf(2,1) ***
//-Sf(2,0)*Sf(0,2) -Sf(2,1)*Sf(0,0) -Sf(2,2)*Sf(0,1)
t2 = _mm_mul_ps ( _mm_sub_ps ( t2 , t22 ) , det ) ; //Sf(1,0)*Sf(2,1) Sf(1,1)*Sf(2,2) Sf(1,2)*Sf(2,0) ***
//-Sf(2,0)*Sf(1,1) -Sf(2,1)*Sf(1,2) -Sf(2,2)*Sf(1,0)
_mm_store_ps ( t , t0 ) ;
_mm_store_ps ( t + 4 , t1 ) ;
_mm_store_ps ( t + 8 , t2 ) ;
Df ( 0 , 0 ) = t [ 9 ] ; Df ( 0 , 1 ) = t [ 6 ] ; Df ( 0 , 2 ) = t [ 1 ] ;
Df ( 1 , 0 ) = t [ 10 ] ; Df ( 1 , 1 ) = t [ 4 ] ; Df ( 1 , 2 ) = t [ 2 ] ;
Df ( 2 , 0 ) = t [ 8 ] ; Df ( 2 , 1 ) = t [ 5 ] ; Df ( 2 , 2 ) = t [ 0 ] ;
}
else
# endif
{
t [ 0 ] = ( float ) ( ( ( double ) Sf ( 1 , 1 ) * Sf ( 2 , 2 ) - ( double ) Sf ( 1 , 2 ) * Sf ( 2 , 1 ) ) * d ) ;
t [ 1 ] = ( float ) ( ( ( double ) Sf ( 0 , 2 ) * Sf ( 2 , 1 ) - ( double ) Sf ( 0 , 1 ) * Sf ( 2 , 2 ) ) * d ) ;
t [ 2 ] = ( float ) ( ( ( double ) Sf ( 0 , 1 ) * Sf ( 1 , 2 ) - ( double ) Sf ( 0 , 2 ) * Sf ( 1 , 1 ) ) * d ) ;
t [ 0 ] = ( float ) ( ( ( double ) Sf ( 1 , 1 ) * Sf ( 2 , 2 ) - ( double ) Sf ( 1 , 2 ) * Sf ( 2 , 1 ) ) * d ) ;
t [ 1 ] = ( float ) ( ( ( double ) Sf ( 0 , 2 ) * Sf ( 2 , 1 ) - ( double ) Sf ( 0 , 1 ) * Sf ( 2 , 2 ) ) * d ) ;
t [ 2 ] = ( float ) ( ( ( double ) Sf ( 0 , 1 ) * Sf ( 1 , 2 ) - ( double ) Sf ( 0 , 2 ) * Sf ( 1 , 1 ) ) * d ) ;
t [ 3 ] = ( float ) ( ( ( double ) Sf ( 1 , 2 ) * Sf ( 2 , 0 ) - ( double ) Sf ( 1 , 0 ) * Sf ( 2 , 2 ) ) * d ) ;
t [ 4 ] = ( float ) ( ( ( double ) Sf ( 0 , 0 ) * Sf ( 2 , 2 ) - ( double ) Sf ( 0 , 2 ) * Sf ( 2 , 0 ) ) * d ) ;
t [ 5 ] = ( float ) ( ( ( double ) Sf ( 0 , 2 ) * Sf ( 1 , 0 ) - ( double ) Sf ( 0 , 0 ) * Sf ( 1 , 2 ) ) * d ) ;
t [ 3 ] = ( float ) ( ( ( double ) Sf ( 1 , 2 ) * Sf ( 2 , 0 ) - ( double ) Sf ( 1 , 0 ) * Sf ( 2 , 2 ) ) * d ) ;
t [ 4 ] = ( float ) ( ( ( double ) Sf ( 0 , 0 ) * Sf ( 2 , 2 ) - ( double ) Sf ( 0 , 2 ) * Sf ( 2 , 0 ) ) * d ) ;
t [ 5 ] = ( float ) ( ( ( double ) Sf ( 0 , 2 ) * Sf ( 1 , 0 ) - ( double ) Sf ( 0 , 0 ) * Sf ( 1 , 2 ) ) * d ) ;
t [ 6 ] = ( float ) ( ( ( double ) Sf ( 1 , 0 ) * Sf ( 2 , 1 ) - ( double ) Sf ( 1 , 1 ) * Sf ( 2 , 0 ) ) * d ) ;
t [ 7 ] = ( float ) ( ( ( double ) Sf ( 0 , 1 ) * Sf ( 2 , 0 ) - ( double ) Sf ( 0 , 0 ) * Sf ( 2 , 1 ) ) * d ) ;
t [ 8 ] = ( float ) ( ( ( double ) Sf ( 0 , 0 ) * Sf ( 1 , 1 ) - ( double ) Sf ( 0 , 1 ) * Sf ( 1 , 0 ) ) * d ) ;
t [ 6 ] = ( float ) ( ( ( double ) Sf ( 1 , 0 ) * Sf ( 2 , 1 ) - ( double ) Sf ( 1 , 1 ) * Sf ( 2 , 0 ) ) * d ) ;
t [ 7 ] = ( float ) ( ( ( double ) Sf ( 0 , 1 ) * Sf ( 2 , 0 ) - ( double ) Sf ( 0 , 0 ) * Sf ( 2 , 1 ) ) * d ) ;
t [ 8 ] = ( float ) ( ( ( double ) Sf ( 0 , 0 ) * Sf ( 1 , 1 ) - ( double ) Sf ( 0 , 1 ) * Sf ( 1 , 0 ) ) * d ) ;
Df ( 0 , 0 ) = t [ 0 ] ; Df ( 0 , 1 ) = t [ 1 ] ; Df ( 0 , 2 ) = t [ 2 ] ;
Df ( 1 , 0 ) = t [ 3 ] ; Df ( 1 , 1 ) = t [ 4 ] ; Df ( 1 , 2 ) = t [ 5 ] ;
Df ( 2 , 0 ) = t [ 6 ] ; Df ( 2 , 1 ) = t [ 7 ] ; Df ( 2 , 2 ) = t [ 8 ] ;
}
Df ( 0 , 0 ) = t [ 0 ] ; Df ( 0 , 1 ) = t [ 1 ] ; Df ( 0 , 2 ) = t [ 2 ] ;
Df ( 1 , 0 ) = t [ 3 ] ; Df ( 1 , 1 ) = t [ 4 ] ; Df ( 1 , 2 ) = t [ 5 ] ;
Df ( 2 , 0 ) = t [ 6 ] ; Df ( 2 , 1 ) = t [ 7 ] ; Df ( 2 , 2 ) = t [ 8 ] ;
}
}
else