void conv_Float2ToFloat1(void* dst, const void* s, s32 numSamples)
{
LSfloat* d = reinterpret_cast<LSfloat*>(dst);
const LSfloat* src = reinterpret_cast<const LSfloat*>(s);
s32 num = numSamples >> 2; //4個のfloatをまとめて処理
s32 offset = num << 2;
s32 rem = numSamples - offset;
__m128 coff = _mm_set1_ps(0.5f);
const LSfloat* p = src;
LSfloat* q = d;
for(s32 i=0; i<num; ++i){
__m128 f32_0 = _mm_loadu_ps(p);
__m128 f32_1 = _mm_shuffle_ps(f32_0, f32_0, _MM_SHUFFLE(2, 3, 1, 0));
__m128 f32_2 = _mm_mul_ps(_mm_add_ps(f32_0, f32_1), coff);
__m128 f32_3 = _mm_shuffle_ps(f32_2, f32_2, _MM_SHUFFLE(2, 0, 2, 0));
_mm_storel_pi((__m64*)q, f32_3);
p += 4;
q += 2;
}
for(s32 i=0; i<rem; ++i){
s32 j=i<<1;
q[i] = 0.5f*(p[j+0]+p[j+1]);
}
}
void mpeg2_idct_copy_sse2(int16_t* block, uint8_t* dest, const int stride)
{
idct_M128ASM(block);
/*
for(int i = 0; i < 8; i++)
{
dest[0] = CLIP(block[0]);
dest[1] = CLIP(block[1]);
dest[2] = CLIP(block[2]);
dest[3] = CLIP(block[3]);
dest[4] = CLIP(block[4]);
dest[5] = CLIP(block[5]);
dest[6] = CLIP(block[6]);
dest[7] = CLIP(block[7]);
memset(block, 0, sizeof(short)*8);
dest += stride;
block += 8;
}
*/
__m128i* src = (__m128i*)block;
__m128i zero = _mm_setzero_si128();
__m128i r0 = _mm_packus_epi16(_mm_load_si128(&src[0]), _mm_load_si128(&src[1]));
__m128i r1 = _mm_packus_epi16(_mm_load_si128(&src[2]), _mm_load_si128(&src[3]));
__m128i r2 = _mm_packus_epi16(_mm_load_si128(&src[4]), _mm_load_si128(&src[5]));
__m128i r3 = _mm_packus_epi16(_mm_load_si128(&src[6]), _mm_load_si128(&src[7]));
_mm_storel_pi((__m64*)&dest[0*stride], *(__m128*)&r0);
_mm_storeh_pi((__m64*)&dest[1*stride], *(__m128*)&r0);
_mm_storel_pi((__m64*)&dest[2*stride], *(__m128*)&r1);
_mm_storeh_pi((__m64*)&dest[3*stride], *(__m128*)&r1);
_mm_storel_pi((__m64*)&dest[4*stride], *(__m128*)&r2);
_mm_storeh_pi((__m64*)&dest[5*stride], *(__m128*)&r2);
_mm_storel_pi((__m64*)&dest[6*stride], *(__m128*)&r3);
_mm_storeh_pi((__m64*)&dest[7*stride], *(__m128*)&r3);
_mm_store_si128(&src[0], zero);
_mm_store_si128(&src[1], zero);
_mm_store_si128(&src[2], zero);
_mm_store_si128(&src[3], zero);
_mm_store_si128(&src[4], zero);
_mm_store_si128(&src[5], zero);
_mm_store_si128(&src[6], zero);
_mm_store_si128(&src[7], zero);
}
void mpeg2_idct_copy_sse2(int16_t* block, uint8_t* dest, const int stride)
{
__m128i &src0=*(__m128i*)(block+0*16/2);
__m128i &src1=*(__m128i*)(block+1*16/2);
__m128i &src2=*(__m128i*)(block+2*16/2);
__m128i &src3=*(__m128i*)(block+3*16/2);
__m128i &src4=*(__m128i*)(block+4*16/2);
__m128i &src5=*(__m128i*)(block+5*16/2);
__m128i &src6=*(__m128i*)(block+6*16/2);
__m128i &src7=*(__m128i*)(block+7*16/2);
idct_M128ASM (src0,src1,src2,src3,src4,src5,src6,src7);
__m128i zero = _mm_setzero_si128();
__m128i r0 = _mm_packus_epi16(_mm_load_si128(&src0), _mm_load_si128(&src1));
__m128i r1 = _mm_packus_epi16(_mm_load_si128(&src2), _mm_load_si128(&src3));
__m128i r2 = _mm_packus_epi16(_mm_load_si128(&src4), _mm_load_si128(&src5));
__m128i r3 = _mm_packus_epi16(_mm_load_si128(&src6), _mm_load_si128(&src7));
_mm_storel_pi((__m64*)&dest[0*stride], *(__m128*)&r0);
_mm_storeh_pi((__m64*)&dest[1*stride], *(__m128*)&r0);
_mm_storel_pi((__m64*)&dest[2*stride], *(__m128*)&r1);
_mm_storeh_pi((__m64*)&dest[3*stride], *(__m128*)&r1);
_mm_storel_pi((__m64*)&dest[4*stride], *(__m128*)&r2);
_mm_storeh_pi((__m64*)&dest[5*stride], *(__m128*)&r2);
_mm_storel_pi((__m64*)&dest[6*stride], *(__m128*)&r3);
_mm_storeh_pi((__m64*)&dest[7*stride], *(__m128*)&r3);
_mm_store_si128(&src0, zero);
_mm_store_si128(&src1, zero);
_mm_store_si128(&src2, zero);
_mm_store_si128(&src3, zero);
_mm_store_si128(&src4, zero);
_mm_store_si128(&src5, zero);
_mm_store_si128(&src6, zero);
_mm_store_si128(&src7, zero);
}
void
intrin_sse_add_su3_vector(su3_vectorf *aa, su3_vectorf *bb, su3_vectorf *cc)
{
/* XMM Variables */
__m128 xmm2, xmm3, xmm0, xmm1;
xmm0 = _mm_loadu_ps((float *)&((aa)->c[0]) );
xmm1 = _mm_loadl_pi(xmm1, (__m64 *)&((aa)->c[2]) );
xmm1 = _mm_shuffle_ps( xmm1, xmm1, 0x44 );
xmm2 = _mm_loadu_ps((float *)&((bb)->c[0]) );
xmm3 = _mm_loadl_pi(xmm3, (__m64 *)&((bb)->c[2]) );
xmm3 = _mm_shuffle_ps( xmm3, xmm3, 0x44 );
xmm0 = _mm_add_ps( xmm0, xmm2 );
xmm1 = _mm_add_ps( xmm1, xmm3 );
_mm_storeu_ps((float *)&((cc)->c[0]), xmm0 );
_mm_storel_pi((__m64 *)&((cc)->c[2]), xmm1 );
}
void
intrin_sse_scalar_mult_add_su3_vector(su3_vectorf* aa, su3_vectorf* bb, float cc, su3_vectorf* dd)
{
/* XMM Variables */
__m128 xmm2, xmm3, xmm0, xmm1, xmm4;
xmm4 = _mm_load_ss((float *)&((cc)) );
xmm4 = _mm_shuffle_ps( xmm4, xmm4, 0x00 );
xmm0 = _mm_loadu_ps((float *)&((aa)->c[0]) );
xmm1 = _mm_loadl_pi(xmm1, (__m64 *)&((aa)->c[2]) );
xmm1 = _mm_shuffle_ps( xmm1, xmm1, 0x44 );
xmm2 = _mm_loadu_ps((float *)&((bb)->c[0]) );
xmm3 = _mm_loadl_pi(xmm3, (__m64 *)&((bb)->c[2]) );
xmm3 = _mm_shuffle_ps( xmm3, xmm3, 0x44 );
xmm2 = _mm_mul_ps( xmm2, xmm4 );
xmm3 = _mm_mul_ps( xmm3, xmm4 );
xmm0 = _mm_add_ps( xmm0, xmm2 );
xmm1 = _mm_add_ps( xmm1, xmm3 );
_mm_storeu_ps((float *)&((dd)->c[0]), xmm0 );
_mm_storel_pi((__m64 *)&((dd)->c[2]), xmm1 );
}
static void
mdct_bitreverse_sse(MDCTContext *mdct, FLOAT *x)
{
int n = mdct->n;
int *bit = mdct->bitrev;
float *w0 = x;
float *w1 = x = w0+(n>>1);
float *T = mdct->trig_bitreverse;
do {
float *x0 = x+bit[0];
float *x1 = x+bit[1];
float *x2 = x+bit[2];
float *x3 = x+bit[3];
__m128 XMM0, XMM1, XMM2, XMM3, XMM4, XMM5, XMM6, XMM7;
w1 -= 4;
XMM0 = _mm_lddqu_ps(x0);
XMM1 = _mm_lddqu_ps(x1);
XMM4 = _mm_lddqu_ps(x2);
XMM7 = _mm_lddqu_ps(x3);
XMM2 = XMM0;
XMM3 = XMM1;
XMM5 = XMM0;
XMM6 = XMM1;
XMM0 = _mm_shuffle_ps(XMM0, XMM4, _MM_SHUFFLE(0,1,0,1));
XMM1 = _mm_shuffle_ps(XMM1, XMM7, _MM_SHUFFLE(0,1,0,1));
XMM2 = _mm_shuffle_ps(XMM2, XMM4, _MM_SHUFFLE(0,0,0,0));
XMM3 = _mm_shuffle_ps(XMM3, XMM7, _MM_SHUFFLE(0,0,0,0));
XMM5 = _mm_shuffle_ps(XMM5, XMM4, _MM_SHUFFLE(1,1,1,1));
XMM6 = _mm_shuffle_ps(XMM6, XMM7, _MM_SHUFFLE(1,1,1,1));
XMM4 = _mm_load_ps(T );
XMM7 = _mm_load_ps(T+4);
XMM1 = _mm_xor_ps(XMM1, PCS_RNRN.v);
XMM2 = _mm_add_ps(XMM2, XMM3);
XMM5 = _mm_sub_ps(XMM5, XMM6);
XMM0 = _mm_add_ps(XMM0, XMM1);
XMM2 = _mm_mul_ps(XMM2, XMM4);
XMM5 = _mm_mul_ps(XMM5, XMM7);
XMM0 = _mm_mul_ps(XMM0, PFV_0P5.v);
XMM2 = _mm_add_ps(XMM2, XMM5);
XMM1 = XMM0;
XMM3 = XMM2;
XMM1 = _mm_xor_ps(XMM1, PCS_RNRN.v);
XMM3 = _mm_xor_ps(XMM3, PCS_RNRN.v);
XMM0 = _mm_add_ps(XMM0, XMM2);
XMM1 = _mm_sub_ps(XMM1, XMM3);
_mm_store_ps(w0, XMM0);
_mm_storeh_pi((__m64*)(w1 ), XMM1);
_mm_storel_pi((__m64*)(w1+2), XMM1);
T += 8;
bit += 4;
w0 += 4;
} while (w0 < w1);
}
void
intrin_sse_mult_su3_mat_vec(su3_matrixf *aa, su3_vectorf* bb, su3_vectorf* cc)
{
/* XMM Variables */
__m128 xmm2, xmm3, xmm0, xmm1, xmm6, xmm7, xmm4, xmm5;
xmm0 = _mm_loadl_pi(xmm0, (__m64 *)&((bb)->c[0]) );
xmm1 = _mm_loadl_pi(xmm1, (__m64 *)&((bb)->c[1]) );
xmm2 = _mm_loadl_pi(xmm2, (__m64 *)&((bb)->c[2]) );
xmm0 = _mm_shuffle_ps( xmm0, xmm0, 0x44 );
xmm1 = _mm_shuffle_ps( xmm1, xmm1, 0x44 );
xmm2 = _mm_shuffle_ps( xmm2, xmm2, 0x44 );
xmm3 = _mm_load_ss((float *)&((aa)->e[0][0].real) );
xmm7 = _mm_load_ss((float *)&((aa)->e[1][0].real) );
xmm3 = _mm_shuffle_ps( xmm3, xmm7, 0x00 );
xmm4 = _mm_load_ss((float *)&((aa)->e[0][1].real) );
xmm7 = _mm_load_ss((float *)&((aa)->e[1][1].real) );
xmm4 = _mm_shuffle_ps( xmm4, xmm7, 0x00 );
xmm3 = _mm_mul_ps( xmm3, xmm0 );
xmm4 = _mm_mul_ps( xmm4, xmm1 );
xmm3 = _mm_add_ps( xmm3, xmm4 );
xmm5 = _mm_load_ss((float *)&((aa)->e[0][2].real) );
xmm7 = _mm_load_ss((float *)&((aa)->e[1][2].real) );
xmm5 = _mm_shuffle_ps( xmm5, xmm7, 0x00 );
xmm5 = _mm_mul_ps( xmm5, xmm2 );
xmm3 = _mm_add_ps( xmm3, xmm5 );
xmm1 = _mm_shuffle_ps( xmm1, xmm0, 0x44 );
xmm7 = _mm_load_ss((float *)&((aa)->e[2][0].real) );
xmm6 = _mm_load_ss((float *)&((aa)->e[2][1].real) );
xmm6 = _mm_shuffle_ps( xmm6, xmm7, 0x00 );
xmm6 = _mm_mul_ps( xmm6, xmm1 );
xmm0 = _mm_shuffle_ps( xmm0, xmm0, 0xB1 );
xmm0 = _mm_xor_ps( xmm0, _sse_sgn13.xmm );
xmm1 = _mm_shuffle_ps( xmm1, xmm1, 0x11 );
xmm1 = _mm_xor_ps( xmm1, _sse_sgn13.xmm );
xmm2 = _mm_shuffle_ps( xmm2, xmm2, 0xB1 );
xmm2 = _mm_xor_ps( xmm2, _sse_sgn13.xmm );
xmm4 = _mm_load_ss((float *)&((aa)->e[0][0].imag) );
xmm7 = _mm_load_ss((float *)&((aa)->e[1][0].imag) );
xmm4 = _mm_shuffle_ps( xmm4, xmm7, 0x00 );
xmm4 = _mm_mul_ps( xmm4, xmm0 );
xmm3 = _mm_add_ps( xmm3, xmm4 );
xmm5 = _mm_load_ss((float *)&((aa)->e[0][1].imag) );
xmm7 = _mm_load_ss((float *)&((aa)->e[1][1].imag) );
xmm5 = _mm_shuffle_ps( xmm5, xmm7, 0x00 );
xmm5 = _mm_mul_ps( xmm5, xmm1 );
xmm3 = _mm_add_ps( xmm3, xmm5 );
xmm5 = _mm_load_ss((float *)&((aa)->e[0][2].imag) );
xmm7 = _mm_load_ss((float *)&((aa)->e[1][2].imag) );
xmm5 = _mm_shuffle_ps( xmm5, xmm7, 0x00 );
xmm5 = _mm_mul_ps( xmm5, xmm2 );
xmm3 = _mm_add_ps( xmm3, xmm5 );
_mm_storeu_ps((float *)&((cc)->c[0]), xmm3 );
xmm1 = _mm_shuffle_ps( xmm1, xmm0, 0x44 );
xmm7 = _mm_load_ss((float *)&((aa)->e[2][0].imag) );
xmm5 = _mm_load_ss((float *)&((aa)->e[2][1].imag) );
xmm5 = _mm_shuffle_ps( xmm5, xmm7, 0x00 );
xmm5 = _mm_mul_ps( xmm5, xmm1 );
xmm6 = _mm_add_ps( xmm6, xmm5 );
xmm2 = _mm_shuffle_ps( xmm2, xmm2, 0xB4 );
xmm2 = _mm_xor_ps( xmm2, _sse_sgn4.xmm );
xmm7 = _mm_loadl_pi(xmm7, (__m64 *)&((aa)->e[2][2]) );
xmm7 = _mm_shuffle_ps( xmm7, xmm7, 0x05 );
xmm7 = _mm_mul_ps( xmm7, xmm2 );
xmm6 = _mm_add_ps( xmm6, xmm7 );
xmm7 = xmm6 ;
xmm7 = _mm_shuffle_ps( xmm7, xmm7, 0xEE );
xmm6 = _mm_add_ps( xmm6, xmm7 );
_mm_storel_pi((__m64 *)&((cc)->c[2]), xmm6 );
}
// Does inverse according to Cramers Rule
// See ftp://download.intel.com/design/PentiumIII/sml/24504301.pdf
void Mat44::Cramers_Inverse_SSE(const Mat44 *out, f32 &detv) const
{
f32 *src = (f32*)&mat;
__m128 minor0=_mm_setzero_ps(), minor1=_mm_setzero_ps(), minor2=_mm_setzero_ps(), minor3=_mm_setzero_ps();
__m128 row0=_mm_setzero_ps(), row1=_mm_setzero_ps(), row2=_mm_setzero_ps(), row3=_mm_setzero_ps();
__m128 det=_mm_setzero_ps(), tmp1=_mm_setzero_ps();
tmp1 = _mm_loadh_pi(_mm_loadl_pi(tmp1, (__m64*)(src)), (__m64*)(src+ 4));
row1 = _mm_loadh_pi(_mm_loadl_pi(row1, (__m64*)(src+8)), (__m64*)(src+12));
row0 = _mm_shuffle_ps(tmp1, row1, 0x88);
row1 = _mm_shuffle_ps(row1, tmp1, 0xDD);
tmp1 = _mm_loadh_pi(_mm_loadl_pi(tmp1, (__m64*)(src+ 2)), (__m64*)(src+ 6));
row3 = _mm_loadh_pi(_mm_loadl_pi(row3, (__m64*)(src+10)), (__m64*)(src+14));
row2 = _mm_shuffle_ps(tmp1, row3, 0x88);
row3 = _mm_shuffle_ps(row3, tmp1, 0xDD);
tmp1 = _mm_mul_ps(row2, row3);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor0 = _mm_mul_ps(row1, tmp1);
minor1 = _mm_mul_ps(row0, tmp1);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor0 = _mm_sub_ps(_mm_mul_ps(row1, tmp1), minor0);
minor1 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor1);
minor1 = _mm_shuffle_ps(minor1, minor1, 0x4E);
tmp1 = _mm_mul_ps(row1, row2);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor0 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor0);
minor3 = _mm_mul_ps(row0, tmp1);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row3, tmp1));
minor3 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor3);
minor3 = _mm_shuffle_ps(minor3, minor3, 0x4E);
tmp1 = _mm_mul_ps(_mm_shuffle_ps(row1, row1, 0x4E), row3);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
row2 = _mm_shuffle_ps(row2, row2, 0x4E);
minor0 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor0);
minor2 = _mm_mul_ps(row0, tmp1);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row2, tmp1));
minor2 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor2);
minor2 = _mm_shuffle_ps(minor2, minor2, 0x4E);
tmp1 = _mm_mul_ps(row0, row1);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor2 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor2);
minor3 = _mm_sub_ps(_mm_mul_ps(row2, tmp1), minor3);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor2 = _mm_sub_ps(_mm_mul_ps(row3, tmp1), minor2);
minor3 = _mm_sub_ps(minor3, _mm_mul_ps(row2, tmp1));
tmp1 = _mm_mul_ps(row0, row3);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row2, tmp1));
minor2 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor2);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor1 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor1);
minor2 = _mm_sub_ps(minor2, _mm_mul_ps(row1, tmp1));
tmp1 = _mm_mul_ps(row0, row2);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor1 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor1);
minor3 = _mm_sub_ps(minor3, _mm_mul_ps(row1, tmp1));
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row3, tmp1));
minor3 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor3);
det = _mm_mul_ps(row0, minor0);
det = _mm_add_ps(_mm_shuffle_ps(det, det, 0x4E), det);
det = _mm_add_ss(_mm_shuffle_ps(det, det, 0xB1), det);
tmp1 = _mm_rcp_ss(det);
det = _mm_sub_ss(_mm_add_ss(tmp1, tmp1), _mm_mul_ss(det, _mm_mul_ss(tmp1, tmp1)));
det = _mm_shuffle_ps(det, det, 0x00);
_mm_store_ss(&detv, det);
Mat44 t;
if(out)
{
src = (f32*)out->mat;
}
else
{
src = t.mat;
}
minor0 = _mm_mul_ps(det, minor0);
_mm_storel_pi((__m64*)(src), minor0);
_mm_storeh_pi((__m64*)(src+2), minor0);
minor1 = _mm_mul_ps(det, minor1);
_mm_storel_pi((__m64*)(src+4), minor1);
_mm_storeh_pi((__m64*)(src+6), minor1);
minor2 = _mm_mul_ps(det, minor2);
_mm_storel_pi((__m64*)(src+ 8), minor2);
_mm_storeh_pi((__m64*)(src+10), minor2);
minor3 = _mm_mul_ps(det, minor3);
_mm_storel_pi((__m64*)(src+12), minor3);
_mm_storeh_pi((__m64*)(src+14), minor3);
};
int dist(const float* xyz, const int* pairs, float* distance_out,
float* displacement_out, const int n_frames, const int n_atoms,
const int n_pairs) {
/* Compute the distance/displacement between pairs of atoms in every frame
of xyz.
Parameters
----------
xyz : array, shape=(n_frames, n_atoms, 3)
Cartesian coordinates of the atoms in every frame, in contiguous C order.
pairs : array, shape=(n_pairs, 2)
The specific pairs of atoms whose distance you want to compute. A 2d
array of pairs, in C order.
distance_out : array, shape=(n_frames, n_pairs), optional
Array where the distances between pairs will be stored, in contiguous
C order. If NULL is passed in, this return value will not be saved
displacement_out : array, shaoe=(n_frames, n_pairs, 3), optional
An optional return value: if you'd also like to save the displacement
vectors between the pairs, you can pass a pointer here. If
displacement_out is NULL, then this variable will not be saved back
to memory.
All of the arrays are assumed to be contiguous. This code will
segfault if they're not.
*/
int i, j;
int store_displacement = displacement_out == NULL ? 0 : 1;
int store_distance = distance_out == NULL ? 0 : 1;
__m128 x1, x2, r12, r12_2, s;
for (i = 0; i < n_frames; i++) {
for (j = 0; j < n_pairs; j++) {
// Load the two vectors whos distance we want to compute
// x1 = xyz[i, pairs[j,0], 0:3]
// x2 = xyz[i, pairs[j,1], 0:3]
x1 = load_float3(xyz + 3*pairs[2*j + 0]);
x2 = load_float3(xyz + 3*pairs[2*j + 1]);
// r12 = x2 - x1
r12 = _mm_sub_ps(x2, x1);
// r12_2 = r12*r12
r12_2 = _mm_mul_ps(r12, r12);
if (store_displacement) {
// store the two lower entries (x,y) in memory
_mm_storel_pi((__m64*)(displacement_out), r12);
displacement_out += 2;
// swap high-low and then store the z entry in the memory
_mm_store_ss(displacement_out++, _mm_movehl_ps(r12, r12));
}
if (store_distance) {
// horizontal add the components of d2 with
// two instructions. note: it's critical
// here that the last entry of x1 and x2 was 0
// so that d2.w = 0
s = _mm_hadd_ps(r12_2, r12_2);
s = _mm_hadd_ps(s, s);
// sqrt our final answer
s = _mm_sqrt_ps(s);
// s now contains our answer in all four elements, because
// of the way the hadd works. we only want to store one
// element.
_mm_store_ss(distance_out++, s);
}
}
// advance to the next frame
xyz += n_atoms*3;
}
return 1;
}
// from intel
Matrix4x4SSE &Matrix4x4SSE::Invert(void)
{
float *src = &m_Vec0[0];
__m128 minor0, minor1, minor2, minor3;
__m128 det;
// fool compiler only..
__m128 tmp1 = m_Vec0.m_Vec;
__m128 row0 = m_Vec0.m_Vec;
__m128 row1 = m_Vec1.m_Vec;
__m128 row2 = m_Vec2.m_Vec;
__m128 row3 = m_Vec3.m_Vec;
tmp1 = _mm_loadh_pi(_mm_loadl_pi(tmp1, (__m64*)(src)), (__m64*)(src+ 4));
row1 = _mm_loadh_pi(_mm_loadl_pi(row1, (__m64*)(src+8)), (__m64*)(src+12));
row0 = _mm_shuffle_ps(tmp1, row1, 0x88);
row1 = _mm_shuffle_ps(row1, tmp1, 0xDD);
tmp1 = _mm_loadh_pi(_mm_loadl_pi(tmp1, (__m64*)(src+ 2)), (__m64*)(src+ 6));
row3 = _mm_loadh_pi(_mm_loadl_pi(row3, (__m64*)(src+10)), (__m64*)(src+14));
row2 = _mm_shuffle_ps(tmp1, row3, 0x88);
row3 = _mm_shuffle_ps(row3, tmp1, 0xDD);
// -----------------------------------------------
tmp1 = _mm_mul_ps(row2, row3);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor0 = _mm_mul_ps(row1, tmp1);
minor1 = _mm_mul_ps(row0, tmp1);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor0 = _mm_sub_ps(_mm_mul_ps(row1, tmp1), minor0);
minor1 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor1);
minor1 = _mm_shuffle_ps(minor1, minor1, 0x4E);
// -----------------------------------------------
tmp1 = _mm_mul_ps(row1, row2);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor0 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor0);
minor3 = _mm_mul_ps(row0, tmp1);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row3, tmp1));
minor3 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor3);
minor3 = _mm_shuffle_ps(minor3, minor3, 0x4E);
// -----------------------------------------------
tmp1 = _mm_mul_ps(_mm_shuffle_ps(row1, row1, 0x4E), row3);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
row2 = _mm_shuffle_ps(row2, row2, 0x4E);
minor0 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor0);
minor2 = _mm_mul_ps(row0, tmp1);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row2, tmp1));
minor2 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor2);
minor2 = _mm_shuffle_ps(minor2, minor2, 0x4E);
// -----------------------------------------------
tmp1 = _mm_mul_ps(row0, row1);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor2 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor2);
minor3 = _mm_sub_ps(_mm_mul_ps(row2, tmp1), minor3);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor2 = _mm_sub_ps(_mm_mul_ps(row3, tmp1), minor2);
minor3 = _mm_sub_ps(minor3, _mm_mul_ps(row2, tmp1));
// -----------------------------------------------
tmp1 = _mm_mul_ps(row0, row3);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row2, tmp1));
minor2 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor2);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor1 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor1);
minor2 = _mm_sub_ps(minor2, _mm_mul_ps(row1, tmp1));
// -----------------------------------------------
tmp1 = _mm_mul_ps(row0, row2);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor1 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor1);
minor3 = _mm_sub_ps(minor3, _mm_mul_ps(row1, tmp1));
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row3, tmp1));
minor3 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor3);
// -----------------------------------------------
det = _mm_mul_ps(row0, minor0);
det = _mm_add_ps(_mm_shuffle_ps(det, det, 0x4E), det);
det = _mm_add_ss(_mm_shuffle_ps(det, det, 0xB1), det);
tmp1 = _mm_rcp_ss(det);
det = _mm_sub_ss(_mm_add_ss(tmp1, tmp1), _mm_mul_ss(det, _mm_mul_ss(tmp1, tmp1)));
det = _mm_shuffle_ps(det, det, 0x00);
minor0 = _mm_mul_ps(det, minor0);
_mm_storel_pi((__m64*)(src), minor0);
_mm_storeh_pi((__m64*)(src+2), minor0);
minor1 = _mm_mul_ps(det, minor1);
_mm_storel_pi((__m64*)(src+4), minor1);
_mm_storeh_pi((__m64*)(src+6), minor1);
minor2 = _mm_mul_ps(det, minor2);
_mm_storel_pi((__m64*)(src+ 8), minor2);
_mm_storeh_pi((__m64*)(src+10), minor2);
minor3 = _mm_mul_ps(det, minor3);
_mm_storel_pi((__m64*)(src+12), minor3);
_mm_storeh_pi((__m64*)(src+14), minor3);
return *this;
}
// Inverts a 4x4 matrix and returns the determinate
inline float invert_44_matrix(float* src)
{
// Code pulled from "Streaming SIMD Extensions - Inverse of 4x4 Matrix"
// by Intel.
// ftp://download.intel.com/design/PentiumIII/sml/24504301.pdf
__m128 minor0;
__m128 minor1;
__m128 minor2;
__m128 minor3;
__m128 row0;
__m128 row1;
__m128 row2;
__m128 row3;
__m128 det;
__m128 tmp1;
tmp1 = _mm_loadh_pi(_mm_loadl_pi(tmp1, (__m64*)(src)), (__m64*)(src+ 4));
row1 = _mm_loadh_pi(_mm_loadl_pi(row1, (__m64*)(src+8)), (__m64*)(src+12));
row0 = _mm_shuffle_ps(tmp1, row1, 0x88);
row1 = _mm_shuffle_ps(row1, tmp1, 0xDD);
tmp1 = _mm_loadh_pi(_mm_loadl_pi(tmp1, (__m64*)(src+ 2)), (__m64*)(src+ 6));
row3 = _mm_loadh_pi(_mm_loadl_pi(row3, (__m64*)(src+10)), (__m64*)(src+14));
row2 = _mm_shuffle_ps(tmp1, row3, 0x88);
row3 = _mm_shuffle_ps(row3, tmp1, 0xDD);
// -----------------------------------------------
tmp1 = _mm_mul_ps(row2, row3);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor0 = _mm_mul_ps(row1, tmp1);
minor1 = _mm_mul_ps(row0, tmp1);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor0 = _mm_sub_ps(_mm_mul_ps(row1, tmp1), minor0);
minor1 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor1);
minor1 = _mm_shuffle_ps(minor1, minor1, 0x4E);
// -----------------------------------------------
tmp1 = _mm_mul_ps(row1, row2);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor0 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor0);
minor3 = _mm_mul_ps(row0, tmp1);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row3, tmp1));
minor3 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor3);
minor3 = _mm_shuffle_ps(minor3, minor3, 0x4E);
// -----------------------------------------------
tmp1 = _mm_mul_ps(_mm_shuffle_ps(row1, row1, 0x4E), row3);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
row2 = _mm_shuffle_ps(row2, row2, 0x4E);
minor0 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor0);
minor2 = _mm_mul_ps(row0, tmp1);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row2, tmp1));
minor2 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor2);
minor2 = _mm_shuffle_ps(minor2, minor2, 0x4E);
// -----------------------------------------------
tmp1 = _mm_mul_ps(row0, row1);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor2 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor2);
minor3 = _mm_sub_ps(_mm_mul_ps(row2, tmp1), minor3);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor2 = _mm_sub_ps(_mm_mul_ps(row3, tmp1), minor2);
minor3 = _mm_sub_ps(minor3, _mm_mul_ps(row2, tmp1));
// -----------------------------------------------
tmp1 = _mm_mul_ps(row0, row3);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row2, tmp1));
minor2 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor2);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor1 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor1);
minor2 = _mm_sub_ps(minor2, _mm_mul_ps(row1, tmp1));
// -----------------------------------------------
tmp1 = _mm_mul_ps(row0, row2);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor1 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor1);
minor3 = _mm_sub_ps(minor3, _mm_mul_ps(row1, tmp1));
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row3, tmp1));
minor3 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor3);
// -----------------------------------------------
det = _mm_mul_ps(row0, minor0);
det = _mm_add_ps(_mm_shuffle_ps(det, det, 0x4E), det);
det = _mm_add_ss(_mm_shuffle_ps(det, det, 0xB1), det);
tmp1 = _mm_rcp_ss(det);
det = _mm_sub_ss(_mm_add_ss(tmp1, tmp1), _mm_mul_ss(det, _mm_mul_ss(tmp1, tmp1)));
det = _mm_shuffle_ps(det, det, 0x00);
minor0 = _mm_mul_ps(det, minor0);
_mm_storel_pi((__m64*)(src), minor0);
_mm_storeh_pi((__m64*)(src+2), minor0);
minor1 = _mm_mul_ps(det, minor1);
_mm_storel_pi((__m64*)(src+4), minor1);
_mm_storeh_pi((__m64*)(src+6), minor1);
minor2 = _mm_mul_ps(det, minor2);
_mm_storel_pi((__m64*)(src+ 8), minor2);
_mm_storeh_pi((__m64*)(src+10), minor2);
minor3 = _mm_mul_ps(det, minor3);
_mm_storel_pi((__m64*)(src+12), minor3);
_mm_storeh_pi((__m64*)(src+14), minor3);
return det[0];
}
int main()
{
float *arr = get_arr(); // [4, 3, 2, 1]
float *uarr = get_uarr(); // [5, 4, 3, 2]
float *arr2 = get_arr2(); // [4, 3, 2, 1]
float *uarr2 = get_uarr2(); // [5, 4, 3, 2]
__m128 a = get_a(); // [8, 6, 4, 2]
__m128 b = get_b(); // [1, 2, 3, 4]
// Check that test data is like expected.
Assert(((uintptr_t)arr & 0xF) == 0); // arr must be aligned by 16.
Assert(((uintptr_t)uarr & 0xF) != 0); // uarr must be unaligned.
Assert(((uintptr_t)arr2 & 0xF) == 0); // arr must be aligned by 16.
Assert(((uintptr_t)uarr2 & 0xF) != 0); // uarr must be unaligned.
// Test that aeq itself works and does not trivially return true on everything.
Assert(aeq_("",_mm_load_ps(arr), 4.f, 3.f, 2.f, 0.f, false) == false);
#ifdef TEST_M64
Assert(aeq64(u64castm64(0x22446688AACCEEFFULL), 0xABABABABABABABABULL, false) == false);
#endif
// SSE1 Load instructions:
aeq(_mm_load_ps(arr), 4.f, 3.f, 2.f, 1.f); // 4-wide load from aligned address.
aeq(_mm_load_ps1(uarr), 2.f, 2.f, 2.f, 2.f); // Load scalar from unaligned address and populate 4-wide.
aeq(_mm_load_ss(uarr), 0.f, 0.f, 0.f, 2.f); // Load scalar from unaligned address to lowest, and zero all highest.
aeq(_mm_load1_ps(uarr), 2.f, 2.f, 2.f, 2.f); // _mm_load1_ps == _mm_load_ps1
aeq(_mm_loadh_pi(a, (__m64*)uarr), 3.f, 2.f, 4.f, 2.f); // Load two highest addresses, preserve two lowest.
aeq(_mm_loadl_pi(a, (__m64*)uarr), 8.f, 6.f, 3.f, 2.f); // Load two lowest addresses, preserve two highest.
aeq(_mm_loadr_ps(arr), 1.f, 2.f, 3.f, 4.f); // 4-wide load from an aligned address, but reverse order.
aeq(_mm_loadu_ps(uarr), 5.f, 4.f, 3.f, 2.f); // 4-wide load from an unaligned address.
// SSE1 Set instructions:
aeq(_mm_set_ps(uarr[3], 2.f, 3.f, 4.f), 5.f, 2.f, 3.f, 4.f); // 4-wide set by specifying four immediate or memory operands.
aeq(_mm_set_ps1(uarr[3]), 5.f, 5.f, 5.f, 5.f); // 4-wide set by specifying one scalar that is expanded.
aeq(_mm_set_ss(uarr[3]), 0.f, 0.f, 0.f, 5.f); // Set scalar at lowest index, zero all higher.
aeq(_mm_set1_ps(uarr[3]), 5.f, 5.f, 5.f, 5.f); // _mm_set1_ps == _mm_set_ps1
aeq(_mm_setr_ps(uarr[3], 2.f, 3.f, 4.f), 4.f, 3.f, 2.f, 5.f); // 4-wide set by specifying four immediate or memory operands, but reverse order.
aeq(_mm_setzero_ps(), 0.f, 0.f, 0.f, 0.f); // Returns a new zero register.
// SSE1 Move instructions:
aeq(_mm_move_ss(a, b), 8.f, 6.f, 4.f, 4.f); // Copy three highest elements from a, and lowest from b.
aeq(_mm_movehl_ps(a, b), 8.f, 6.f, 1.f, 2.f); // Copy two highest elements from a, and take two highest from b and place them to the two lowest in output.
aeq(_mm_movelh_ps(a, b), 3.f, 4.f, 4.f, 2.f); // Copy two lowest elements from a, and take two lowest from b and place them to the two highest in output.
// SSE1 Store instructions:
#ifdef TEST_M64
/*M64*/*(uint64_t*)uarr = 0xCDCDCDCDCDCDCDCDULL; _mm_maskmove_si64(u64castm64(0x00EEDDCCBBAA9988ULL), u64castm64(0x0080FF7F01FEFF40ULL), (char*)uarr); Assert(*(uint64_t*)uarr == 0xCDEEDDCDCDAA99CDULL); // _mm_maskmove_si64: Conditionally store bytes of a 64-bit value.
/*M64*/*(uint64_t*)uarr = 0xABABABABABABABABULL; _m_maskmovq(u64castm64(0x00EEDDCCBBAA9988ULL), u64castm64(0x0080FF7F01FEFF40ULL), (char*)uarr); Assert(*(uint64_t*)uarr == 0xABEEDDABABAA99ABULL); // _m_maskmovq is an alias to _mm_maskmove_si64.
#endif
_mm_store_ps(arr2, a); aeq(_mm_load_ps(arr2), 8.f, 6.f, 4.f, 2.f); // _mm_store_ps: 4-wide store to aligned memory address.
_mm_store_ps1(arr2, a); aeq(_mm_load_ps(arr2), 2.f, 2.f, 2.f, 2.f); // _mm_store_ps1: Store lowest scalar to aligned address, duplicating the element 4 times.
_mm_storeu_ps(uarr2, _mm_set1_ps(100.f)); _mm_store_ss(uarr2, b); aeq(_mm_loadu_ps(uarr2), 100.f, 100.f, 100.f, 4.f); // _mm_store_ss: Store lowest scalar to unaligned address. Don't adjust higher addresses in memory.
_mm_store_ps(arr2, _mm_set1_ps(100.f)); _mm_store1_ps(arr2, a); aeq(_mm_load_ps(arr2), 2.f, 2.f, 2.f, 2.f); // _mm_store1_ps == _mm_store_ps1
_mm_storeu_ps(uarr2, _mm_set1_ps(100.f)); _mm_storeh_pi((__m64*)uarr2, a); aeq(_mm_loadu_ps(uarr2), 100.f, 100.f, 8.f, 6.f); // _mm_storeh_pi: Store two highest elements to memory.
_mm_storeu_ps(uarr2, _mm_set1_ps(100.f)); _mm_storel_pi((__m64*)uarr2, a); aeq(_mm_loadu_ps(uarr2), 100.f, 100.f, 4.f, 2.f); // _mm_storel_pi: Store two lowest elements to memory.
_mm_storer_ps(arr2, a); aeq(_mm_load_ps(arr2), 2.f, 4.f, 6.f, 8.f); // _mm_storer_ps: 4-wide store to aligned memory address, but reverse the elements on output.
_mm_storeu_ps(uarr2, a); aeq(_mm_loadu_ps(uarr2), 8.f, 6.f, 4.f, 2.f); // _mm_storeu_ps: 4-wide store to unaligned memory address.
#ifdef TEST_M64
/*M64*/_mm_stream_pi((__m64*)uarr, u64castm64(0x0080FF7F01FEFF40ULL)); Assert(*(uint64_t*)uarr == 0x0080FF7F01FEFF40ULL); // _mm_stream_pi: 2-wide store, but with a non-temporal memory cache hint.
#endif
_mm_store_ps(arr2, _mm_set1_ps(100.f)); _mm_stream_ps(arr2, a); aeq(_mm_load_ps(arr2), 8.f, 6.f, 4.f, 2.f); // _mm_stream_ps: 4-wide store, but with a non-temporal memory cache hint.
// SSE1 Arithmetic instructions:
aeq(_mm_add_ps(a, b), 9.f, 8.f, 7.f, 6.f); // 4-wide add.
aeq(_mm_add_ss(a, b), 8.f, 6.f, 4.f, 6.f); // Add lowest element, preserve three highest unchanged from a.
aeq(_mm_div_ps(a, _mm_set_ps(2.f, 3.f, 8.f, 2.f)), 4.f, 2.f, 0.5f, 1.f); // 4-wide div.
aeq(_mm_div_ss(a, _mm_set_ps(2.f, 3.f, 8.f, 8.f)), 8.f, 6.f, 4.f, 0.25f); // Div lowest element, preserve three highest unchanged from a.
aeq(_mm_mul_ps(a, b), 8.f, 12.f, 12.f, 8.f); // 4-wide mul.
aeq(_mm_mul_ss(a, b), 8.f, 6.f, 4.f, 8.f); // Mul lowest element, preserve three highest unchanged from a.
#ifdef TEST_M64
__m64 m1 = get_m1();
/*M64*/aeq64(_mm_mulhi_pu16(m1, u64castm64(0x22446688AACCEEFFULL)), 0x002233440B4C33CFULL); // Multiply u16 channels, and store high parts.
/*M64*/aeq64( _m_pmulhuw(m1, u64castm64(0x22446688AACCEEFFULL)), 0x002233440B4C33CFULL); // _m_pmulhuw is an alias to _mm_mulhi_pu16.
__m64 m2 = get_m2();
/*M64*/aeq64(_mm_sad_pu8(m1, m2), 0x368ULL); // Compute abs. differences of u8 channels, and sum those up to a single 16-bit scalar.
/*M64*/aeq64( _m_psadbw(m1, m2), 0x368ULL); // _m_psadbw is an alias to _mm_sad_pu8.
#endif
aeq(_mm_sub_ps(a, b), 7.f, 4.f, 1.f, -2.f); // 4-wide sub.
aeq(_mm_sub_ss(a, b), 8.f, 6.f, 4.f, -2.f); // Sub lowest element, preserve three highest unchanged from a.
// SSE1 Elementary Math functions:
#ifndef __EMSCRIPTEN__ // TODO: Enable support for this to pass.
aeq(_mm_rcp_ps(a), 0.124969f, 0.166626f, 0.249939f, 0.499878f); // Compute 4-wide 1/x.
aeq(_mm_rcp_ss(a), 8.f, 6.f, 4.f, 0.499878f); // Compute 1/x of lowest element, pass higher elements unchanged.
aeq(_mm_rsqrt_ps(a), 0.353455f, 0.408203f, 0.499878f, 0.706909f); // Compute 4-wide 1/sqrt(x).
aeq(_mm_rsqrt_ss(a), 8.f, 6.f, 4.f, 0.706909f); // Compute 1/sqrt(x) of lowest element, pass higher elements unchanged.
#endif
aeq(_mm_sqrt_ps(a), 2.82843f, 2.44949f, 2.f, 1.41421f); // Compute 4-wide sqrt(x).
aeq(_mm_sqrt_ss(a), 8.f, 6.f, 4.f, 1.41421f); // Compute sqrt(x) of lowest element, pass higher elements unchanged.
__m128 i1 = get_i1();
__m128 i2 = get_i2();
// SSE1 Logical instructions:
#ifndef __EMSCRIPTEN__ // TODO: The polyfill currently does NaN canonicalization and breaks these.
aeqi(_mm_and_ps(i1, i2), 0x83200100, 0x0fecc988, 0x80244021, 0x13458a88); // 4-wide binary AND
aeqi(_mm_andnot_ps(i1, i2), 0x388a9888, 0xf0021444, 0x7000289c, 0x00121046); // 4-wide binary (!i1) & i2
aeqi(_mm_or_ps(i1, i2), 0xbfefdba9, 0xffefdfed, 0xf7656bbd, 0xffffdbef); // 4-wide binary OR
aeqi(_mm_xor_ps(i1, i2), 0x3ccfdaa9, 0xf0031665, 0x77412b9c, 0xecba5167); // 4-wide binary XOR
#endif
// SSE1 Compare instructions:
// a = [8, 6, 4, 2], b = [1, 2, 3, 4]
aeqi(_mm_cmpeq_ps(a, _mm_set_ps(8.f, 0.f, 4.f, 0.f)), 0xFFFFFFFF, 0, 0xFFFFFFFF, 0); // 4-wide cmp ==
aeqi(_mm_cmpeq_ss(a, _mm_set_ps(8.f, 0.f, 4.f, 2.f)), fcastu(8.f), fcastu(6.f), fcastu(4.f), 0xFFFFFFFF); // scalar cmp ==, pass three highest unchanged.
aeqi(_mm_cmpge_ps(a, _mm_set_ps(8.f, 7.f, 3.f, 5.f)), 0xFFFFFFFF, 0, 0xFFFFFFFF, 0); // 4-wide cmp >=
aeqi(_mm_cmpge_ss(a, _mm_set_ps(8.f, 7.f, 3.f, 0.f)), fcastu(8.f), fcastu(6.f), fcastu(4.f), 0xFFFFFFFF); // scalar cmp >=, pass three highest unchanged.
aeqi(_mm_cmpgt_ps(a, _mm_set_ps(8.f, 7.f, 3.f, 5.f)), 0, 0, 0xFFFFFFFF, 0); // 4-wide cmp >
aeqi(_mm_cmpgt_ss(a, _mm_set_ps(8.f, 7.f, 3.f, 2.f)), fcastu(8.f), fcastu(6.f), fcastu(4.f), 0); // scalar cmp >, pass three highest unchanged.
aeqi(_mm_cmple_ps(a, _mm_set_ps(8.f, 7.f, 3.f, 5.f)), 0xFFFFFFFF, 0xFFFFFFFF, 0, 0xFFFFFFFF); // 4-wide cmp <=
aeqi(_mm_cmple_ss(a, _mm_set_ps(8.f, 7.f, 3.f, 0.f)), fcastu(8.f), fcastu(6.f), fcastu(4.f), 0); // scalar cmp <=, pass three highest unchanged.
aeqi(_mm_cmplt_ps(a, _mm_set_ps(8.f, 7.f, 3.f, 5.f)), 0, 0xFFFFFFFF, 0, 0xFFFFFFFF); // 4-wide cmp <
aeqi(_mm_cmplt_ss(a, _mm_set_ps(8.f, 7.f, 3.f, 2.f)), fcastu(8.f), fcastu(6.f), fcastu(4.f), 0); // scalar cmp <, pass three highest unchanged.
aeqi(_mm_cmpneq_ps(a, _mm_set_ps(8.f, 0.f, 4.f, 0.f)), 0, 0xFFFFFFFF, 0, 0xFFFFFFFF); // 4-wide cmp !=
aeqi(_mm_cmpneq_ss(a, _mm_set_ps(8.f, 0.f, 4.f, 2.f)), fcastu(8.f), fcastu(6.f), fcastu(4.f), 0); // scalar cmp !=, pass three highest unchanged.
aeqi(_mm_cmpnge_ps(a, _mm_set_ps(8.f, 7.f, 3.f, 5.f)), 0, 0xFFFFFFFF, 0, 0xFFFFFFFF); // 4-wide cmp not >=
aeqi(_mm_cmpnge_ss(a, _mm_set_ps(8.f, 7.f, 3.f, 0.f)), fcastu(8.f), fcastu(6.f), fcastu(4.f), 0); // scalar cmp not >=, pass three highest unchanged.
aeqi(_mm_cmpngt_ps(a, _mm_set_ps(8.f, 7.f, 3.f, 5.f)), 0xFFFFFFFF, 0xFFFFFFFF, 0, 0xFFFFFFFF); // 4-wide cmp not >
aeqi(_mm_cmpngt_ss(a, _mm_set_ps(8.f, 7.f, 3.f, 2.f)), fcastu(8.f), fcastu(6.f), fcastu(4.f), 0xFFFFFFFF); // scalar cmp not >, pass three highest unchanged.
aeqi(_mm_cmpnle_ps(a, _mm_set_ps(8.f, 7.f, 3.f, 5.f)), 0, 0, 0xFFFFFFFF, 0); // 4-wide cmp not <=
aeqi(_mm_cmpnle_ss(a, _mm_set_ps(8.f, 7.f, 3.f, 0.f)), fcastu(8.f), fcastu(6.f), fcastu(4.f), 0xFFFFFFFF); // scalar cmp not <=, pass three highest unchanged.
aeqi(_mm_cmpnlt_ps(a, _mm_set_ps(8.f, 7.f, 3.f, 5.f)), 0xFFFFFFFF, 0, 0xFFFFFFFF, 0); // 4-wide cmp not <
aeqi(_mm_cmpnlt_ss(a, _mm_set_ps(8.f, 7.f, 3.f, 2.f)), fcastu(8.f), fcastu(6.f), fcastu(4.f), 0xFFFFFFFF); // scalar cmp not <, pass three highest unchanged.
__m128 nan1 = get_nan1(); // [NAN, 0, 0, NAN]
__m128 nan2 = get_nan2(); // [NAN, NAN, 0, 0]
aeqi(_mm_cmpord_ps(nan1, nan2), 0, 0, 0xFFFFFFFF, 0); // 4-wide test if both operands are not nan.
aeqi(_mm_cmpord_ss(nan1, nan2), fcastu(NAN), 0, 0, 0); // scalar test if both operands are not nan, pass three highest unchanged.
// Intel Intrinsics Guide documentation is wrong on _mm_cmpunord_ps and _mm_cmpunord_ss. MSDN is right: http://msdn.microsoft.com/en-us/library/khy6fk1t(v=vs.90).aspx
aeqi(_mm_cmpunord_ps(nan1, nan2), 0xFFFFFFFF, 0xFFFFFFFF, 0, 0xFFFFFFFF); // 4-wide test if one of the operands is nan.
#ifndef __EMSCRIPTEN__ // TODO: The polyfill currently does NaN canonicalization and breaks these.
aeqi(_mm_cmpunord_ss(nan1, nan2), fcastu(NAN), 0, 0, 0xFFFFFFFF); // scalar test if one of the operands is nan, pass three highest unchanged.
#endif
Assert(_mm_comieq_ss(a, b) == 0); Assert(_mm_comieq_ss(a, a) == 1); // Scalar cmp == of lowest element, return int.
Assert(_mm_comige_ss(a, b) == 0); Assert(_mm_comige_ss(a, a) == 1); // Scalar cmp >= of lowest element, return int.
Assert(_mm_comigt_ss(b, a) == 1); Assert(_mm_comigt_ss(a, a) == 0); // Scalar cmp > of lowest element, return int.
Assert(_mm_comile_ss(b, a) == 0); Assert(_mm_comile_ss(a, a) == 1); // Scalar cmp <= of lowest element, return int.
Assert(_mm_comilt_ss(a, b) == 1); Assert(_mm_comilt_ss(a, a) == 0); // Scalar cmp < of lowest element, return int.
Assert(_mm_comineq_ss(a, b) == 1); Assert(_mm_comineq_ss(a, a) == 0); // Scalar cmp != of lowest element, return int.
// The ucomi versions are identical to comi, except that ucomi signal a FP exception only if one of the input operands is a SNaN, whereas the comi versions signal a FP
// exception when one of the input operands is either a QNaN or a SNaN.
#ifndef __EMSCRIPTEN__ // TODO: Fix ucomi support in SSE to treat NaNs properly.
Assert(_mm_ucomieq_ss(a, b) == 0); Assert(_mm_ucomieq_ss(a, a) == 1); Assert(_mm_ucomieq_ss(a, nan1) == 1);
#endif
Assert(_mm_ucomige_ss(a, b) == 0); Assert(_mm_ucomige_ss(a, a) == 1); Assert(_mm_ucomige_ss(a, nan1) == 0);
Assert(_mm_ucomigt_ss(b, a) == 1); Assert(_mm_ucomigt_ss(a, a) == 0); Assert(_mm_ucomigt_ss(a, nan1) == 0);
Assert(_mm_ucomile_ss(b, a) == 0); Assert(_mm_ucomile_ss(a, a) == 1); Assert(_mm_ucomile_ss(a, nan1) == 1);
Assert(_mm_ucomilt_ss(a, b) == 1); Assert(_mm_ucomilt_ss(a, a) == 0); Assert(_mm_ucomilt_ss(a, nan1) == 1);
#ifndef __EMSCRIPTEN__ // TODO: Fix ucomi support in SSE to treat NaNs properly.
Assert(_mm_ucomineq_ss(a, b) == 1); Assert(_mm_ucomineq_ss(a, a) == 0); Assert(_mm_ucomineq_ss(a, nan1) == 0);
#endif
// SSE1 Convert instructions:
__m128 c = get_c(); // [1.5, 2.5, 3.5, 4.5]
__m128 e = get_e(); // [INF, -INF, 2.5, 3.5]
__m128 f = get_f(); // [-1.5, 1.5, -2.5, -9223372036854775808]
#ifdef TEST_M64
/*M64*/aeq(_mm_cvt_pi2ps(a, m2), 8.f, 6.f, -19088744.f, 1985229312.f); // 2-way int32 to float conversion to two lowest channels of m128.
/*M64*/aeq64(_mm_cvt_ps2pi(c), 0x400000004ULL); // 2-way two lowest floats from m128 to integer, return as m64.
#endif
aeq(_mm_cvtsi32_ss(c, -16777215), 1.5f, 2.5f, 3.5f, -16777215.f); // Convert int to float, store in lowest channel of m128.
aeq( _mm_cvt_si2ss(c, -16777215), 1.5f, 2.5f, 3.5f, -16777215.f); // _mm_cvt_si2ss is an alias to _mm_cvtsi32_ss.
#ifndef __EMSCRIPTEN__ // TODO: Fix banker's rounding in cvt functions.
Assert(_mm_cvtss_si32(c) == 4); Assert(_mm_cvtss_si32(e) == 4); // Convert lowest channel of m128 from float to int.
Assert( _mm_cvt_ss2si(c) == 4); Assert( _mm_cvt_ss2si(e) == 4); // _mm_cvt_ss2si is an alias to _mm_cvtss_si32.
#endif
#ifdef TEST_M64
/*M64*/aeq(_mm_cvtpi16_ps(m1), 255.f , -32767.f, 4336.f, 14207.f); // 4-way convert int16s to floats, return in a m128.
/*M64*/aeq(_mm_cvtpi32_ps(a, m1), 8.f, 6.f, 16744449.f, 284178304.f); // 2-way convert int32s to floats, return in two lowest channels of m128, pass two highest unchanged.
/*M64*/aeq(_mm_cvtpi32x2_ps(m1, m2), -19088744.f, 1985229312.f, 16744449.f, 284178304.f); // 4-way convert int32s from two different m64s to float.
/*M64*/aeq(_mm_cvtpi8_ps(m1), 16.f, -16.f, 55.f, 127.f); // 4-way convert int8s from lowest end of m64 to float in a m128.
/*M64*/aeq64(_mm_cvtps_pi16(c), 0x0002000200040004ULL); // 4-way convert floats to int16s in a m64.
/*M64*/aeq64(_mm_cvtps_pi32(c), 0x0000000400000004ULL); // 2-way convert two lowest floats to int32s in a m64.
/*M64*/aeq64(_mm_cvtps_pi8(c), 0x0000000002020404ULL); // 4-way convert floats to int8s in a m64, zero higher half of the returned m64.
/*M64*/aeq(_mm_cvtpu16_ps(m1), 255.f , 32769.f, 4336.f, 14207.f); // 4-way convert uint16s to floats, return in a m128.
/*M64*/aeq(_mm_cvtpu8_ps(m1), 16.f, 240.f, 55.f, 127.f); // 4-way convert uint8s from lowest end of m64 to float in a m128.
#endif
aeq(_mm_cvtsi64_ss(c, -9223372036854775808ULL), 1.5f, 2.5f, 3.5f, -9223372036854775808.f); // Convert single int64 to float, store in lowest channel of m128, and pass three higher channel unchanged.
Assert(_mm_cvtss_f32(c) == 4.5f); // Extract lowest channel of m128 to a plain old float.
Assert(_mm_cvtss_si64(f) == -9223372036854775808ULL); // Convert lowest channel of m128 from float to int64.
#ifdef TEST_M64
/*M64*/aeq64(_mm_cvtt_ps2pi(e), 0x0000000200000003ULL); aeq64(_mm_cvtt_ps2pi(f), 0xfffffffe80000000ULL); // Truncating conversion from two lowest floats of m128 to int32s, return in a m64.
#endif
Assert(_mm_cvttss_si32(e) == 3); // Truncating conversion from the lowest float of a m128 to int32.
Assert( _mm_cvtt_ss2si(e) == 3); // _mm_cvtt_ss2si is an alias to _mm_cvttss_si32.
#ifdef TEST_M64
/*M64*/aeq64(_mm_cvttps_pi32(c), 0x0000000300000004ULL); // Truncating conversion from two lowest floats of m128 to m64.
#endif
Assert(_mm_cvttss_si64(f) == -9223372036854775808ULL); // Truncating conversion from lowest channel of m128 from float to int64.
#ifndef __EMSCRIPTEN__ // TODO: Not implemented.
// SSE1 General support:
unsigned int mask = _MM_GET_EXCEPTION_MASK();
_MM_SET_EXCEPTION_MASK(mask);
unsigned int flushZeroMode = _MM_GET_FLUSH_ZERO_MODE();
_MM_SET_FLUSH_ZERO_MODE(flushZeroMode);
unsigned int roundingMode = _MM_GET_ROUNDING_MODE();
_MM_SET_ROUNDING_MODE(roundingMode);
unsigned int csr = _mm_getcsr();
_mm_setcsr(csr);
unsigned char dummyData[4096];
_mm_prefetch(dummyData, _MM_HINT_T0);
_mm_prefetch(dummyData, _MM_HINT_T1);
_mm_prefetch(dummyData, _MM_HINT_T2);
_mm_prefetch(dummyData, _MM_HINT_NTA);
_mm_sfence();
#endif
// SSE1 Misc instructions:
#ifdef TEST_M64
/*M64*/Assert(_mm_movemask_pi8(m1) == 100); // Return int with eight lowest bits set depending on the highest bits of the 8 uint8 input channels of the m64.
/*M64*/Assert( _m_pmovmskb(m1) == 100); // _m_pmovmskb is an alias to _mm_movemask_pi8.
#endif
Assert(_mm_movemask_ps(_mm_set_ps(-1.f, 0.f, 1.f, NAN)) == 8); Assert(_mm_movemask_ps(_mm_set_ps(-INFINITY, -0.f, INFINITY, -INFINITY)) == 13); // Return int with four lowest bits set depending on the highest bits of the 4 m128 input channels.
// SSE1 Probability/Statistics instructions:
#ifdef TEST_M64
/*M64*/aeq64(_mm_avg_pu16(m1, m2), 0x7FEE9D4D43A234C8ULL); // 4-way average uint16s.
/*M64*/aeq64( _m_pavgw(m1, m2), 0x7FEE9D4D43A234C8ULL); // _m_pavgw is an alias to _mm_avg_pu16.
/*M64*/aeq64(_mm_avg_pu8(m1, m2), 0x7FEE9D4D43A23548ULL); // 8-way average uint8s.
/*M64*/aeq64( _m_pavgb(m1, m2), 0x7FEE9D4D43A23548ULL); // _m_pavgb is an alias to _mm_avg_pu8.
// SSE1 Special Math instructions:
/*M64*/aeq64(_mm_max_pi16(m1, m2), 0xFFBA987654377FULL); // 4-way average uint16s.
/*M64*/aeq64( _m_pmaxsw(m1, m2), 0xFFBA987654377FULL); // _m_pmaxsw is an alias to _mm_max_pi16.
/*M64*/aeq64(_mm_max_pu8(m1, m2), 0xFEFFBA9876F0377FULL); // 4-way average uint16s.
/*M64*/aeq64( _m_pmaxub(m1, m2), 0xFEFFBA9876F0377FULL); // _m_pmaxub is an alias to _mm_max_pu8.
/*M64*/aeq64(_mm_min_pi16(m1, m2), 0xFEDC800110F03210ULL); // 4-way average uint16s.
/*M64*/aeq64( _m_pminsw(m1, m2), 0xFEDC800110F03210ULL); // is an alias to _mm_min_pi16.
/*M64*/aeq64(_mm_min_pu8(m1, m2), 0xDC800110543210ULL); // 4-way average uint16s.
/*M64*/aeq64( _m_pminub(m1, m2), 0xDC800110543210ULL); // is an alias to _mm_min_pu8.
#endif
// a = [8, 6, 4, 2], b = [1, 2, 3, 4]
aeq(_mm_max_ps(a, b), 8.f, 6.f, 4.f, 4.f); // 4-wide max.
aeq(_mm_max_ss(a, _mm_set1_ps(100.f)), 8.f, 6.f, 4.f, 100.f); // Scalar max, pass three highest unchanged.
aeq(_mm_min_ps(a, b), 1.f, 2.f, 3.f, 2.f); // 4-wide min.
aeq(_mm_min_ss(a, _mm_set1_ps(-100.f)), 8.f, 6.f, 4.f, -100.f); // Scalar min, pass three highest unchanged.
// SSE1 Swizzle instructions:
#ifdef TEST_M64
/*M64*/Assert(_mm_extract_pi16(m1, 1) == 4336); // Extract the given int16 channel from a m64.
/*M64*/Assert( _m_pextrw(m1, 1) == 4336); // _m_pextrw is an alias to _mm_extract_pi16.
/*M64*/aeq64(_mm_insert_pi16(m1, 0xABCD, 1), 0xFF8001ABCD377FULL); // Insert a int16 to a specific channel of a m64.
/*M64*/aeq64( _m_pinsrw(m1, 0xABCD, 1), 0xFF8001ABCD377FULL); // _m_pinsrw is an alias to _mm_insert_pi16.
/*M64*/aeq64(_mm_shuffle_pi16(m1, _MM_SHUFFLE(1, 0, 3, 2)), 0x10F0377F00FF8001ULL); // Shuffle int16s around in the 4 channels of the m64.
/*M64*/aeq64( _m_pshufw(m1, _MM_SHUFFLE(1, 0, 3, 2)), 0x10F0377F00FF8001ULL); // _m_pshufw is an alias to _mm_shuffle_pi16.
#endif
aeq(_mm_shuffle_ps(a, b, _MM_SHUFFLE(1, 0, 3, 2)), 3.f, 4.f, 8.f, 6.f);
aeq(_mm_unpackhi_ps(a, b), 1.f , 8.f, 2.f, 6.f);
aeq(_mm_unpacklo_ps(a, b), 3.f , 4.f, 4.f, 2.f);
// Transposing a matrix via the xmmintrin.h-provided intrinsic.
__m128 c0 = a; // [8, 6, 4, 2]
__m128 c1 = b; // [1, 2, 3, 4]
__m128 c2 = get_c(); // [1.5, 2.5, 3.5, 4.5]
__m128 c3 = get_d(); // [8.5, 6.5, 4.5, 2.5]
_MM_TRANSPOSE4_PS(c0, c1, c2, c3);
aeq(c0, 2.5f, 4.5f, 4.f, 2.f);
aeq(c1, 4.5f, 3.5f, 3.f, 4.f);
aeq(c2, 6.5f, 2.5f, 2.f, 6.f);
aeq(c3, 8.5f, 1.5f, 1.f, 8.f);
// All done!
if (numFailures == 0)
printf("Success!\n");
else
printf("%d tests failed!\n", numFailures);
}
void mpeg2_idct_add_sse2(const int last, int16_t* block, uint8_t* dest, const int stride)
{
idct_M128ASM(block);
/*
for(int i = 0; i < 8; i++)
{
dest[0] = CLIP(block[0] + dest[0]);
dest[1] = CLIP(block[1] + dest[1]);
dest[2] = CLIP(block[2] + dest[2]);
dest[3] = CLIP(block[3] + dest[3]);
dest[4] = CLIP(block[4] + dest[4]);
dest[5] = CLIP(block[5] + dest[5]);
dest[6] = CLIP(block[6] + dest[6]);
dest[7] = CLIP(block[7] + dest[7]);
memset(block, 0, sizeof(short)*8);
dest += stride;
block += 8;
}
*/
__m128i* src = (__m128i*)block;
__m128i zero = _mm_setzero_si128();
__m128i r0 = _mm_load_si128(&src[0]);
__m128i r1 = _mm_load_si128(&src[1]);
__m128i r2 = _mm_load_si128(&src[2]);
__m128i r3 = _mm_load_si128(&src[3]);
__m128i r4 = _mm_load_si128(&src[4]);
__m128i r5 = _mm_load_si128(&src[5]);
__m128i r6 = _mm_load_si128(&src[6]);
__m128i r7 = _mm_load_si128(&src[7]);
__m128 q0 = _mm_loadl_pi(*(__m128*)&zero, (__m64*)&dest[0*stride]);
__m128 q1 = _mm_loadl_pi(*(__m128*)&zero, (__m64*)&dest[1*stride]);
__m128 q2 = _mm_loadl_pi(*(__m128*)&zero, (__m64*)&dest[2*stride]);
__m128 q3 = _mm_loadl_pi(*(__m128*)&zero, (__m64*)&dest[3*stride]);
__m128 q4 = _mm_loadl_pi(*(__m128*)&zero, (__m64*)&dest[4*stride]);
__m128 q5 = _mm_loadl_pi(*(__m128*)&zero, (__m64*)&dest[5*stride]);
__m128 q6 = _mm_loadl_pi(*(__m128*)&zero, (__m64*)&dest[6*stride]);
__m128 q7 = _mm_loadl_pi(*(__m128*)&zero, (__m64*)&dest[7*stride]);
r0 = _mm_adds_epi16(r0, _mm_unpacklo_epi8(*(__m128i*)&q0, zero));
r1 = _mm_adds_epi16(r1, _mm_unpacklo_epi8(*(__m128i*)&q1, zero));
r2 = _mm_adds_epi16(r2, _mm_unpacklo_epi8(*(__m128i*)&q2, zero));
r3 = _mm_adds_epi16(r3, _mm_unpacklo_epi8(*(__m128i*)&q3, zero));
r4 = _mm_adds_epi16(r4, _mm_unpacklo_epi8(*(__m128i*)&q4, zero));
r5 = _mm_adds_epi16(r5, _mm_unpacklo_epi8(*(__m128i*)&q5, zero));
r6 = _mm_adds_epi16(r6, _mm_unpacklo_epi8(*(__m128i*)&q6, zero));
r7 = _mm_adds_epi16(r7, _mm_unpacklo_epi8(*(__m128i*)&q7, zero));
r0 = _mm_packus_epi16(r0, r1);
r1 = _mm_packus_epi16(r2, r3);
r2 = _mm_packus_epi16(r4, r5);
r3 = _mm_packus_epi16(r6, r7);
_mm_storel_pi((__m64*)&dest[0*stride], *(__m128*)&r0);
_mm_storeh_pi((__m64*)&dest[1*stride], *(__m128*)&r0);
_mm_storel_pi((__m64*)&dest[2*stride], *(__m128*)&r1);
_mm_storeh_pi((__m64*)&dest[3*stride], *(__m128*)&r1);
_mm_storel_pi((__m64*)&dest[4*stride], *(__m128*)&r2);
_mm_storeh_pi((__m64*)&dest[5*stride], *(__m128*)&r2);
_mm_storel_pi((__m64*)&dest[6*stride], *(__m128*)&r3);
_mm_storeh_pi((__m64*)&dest[7*stride], *(__m128*)&r3);
_mm_store_si128(&src[0], zero);
_mm_store_si128(&src[1], zero);
_mm_store_si128(&src[2], zero);
_mm_store_si128(&src[3], zero);
_mm_store_si128(&src[4], zero);
_mm_store_si128(&src[5], zero);
_mm_store_si128(&src[6], zero);
_mm_store_si128(&src[7], zero);
}
void mpeg2_idct_add_sse2(int,int16_t* block, uint8_t* dest, const int stride)
{
__m128i &src0=*(__m128i*)(block+0*16/2);
__m128i &src1=*(__m128i*)(block+1*16/2);
__m128i &src2=*(__m128i*)(block+2*16/2);
__m128i &src3=*(__m128i*)(block+3*16/2);
__m128i &src4=*(__m128i*)(block+4*16/2);
__m128i &src5=*(__m128i*)(block+5*16/2);
__m128i &src6=*(__m128i*)(block+6*16/2);
__m128i &src7=*(__m128i*)(block+7*16/2);
idct_M128ASM (src0,src1,src2,src3,src4,src5,src6,src7);
__m128i zero = _mm_setzero_si128();
__m128i r0 = _mm_load_si128(&src0);
__m128i r1 = _mm_load_si128(&src1);
__m128i r2 = _mm_load_si128(&src2);
__m128i r3 = _mm_load_si128(&src3);
__m128i r4 = _mm_load_si128(&src4);
__m128i r5 = _mm_load_si128(&src5);
__m128i r6 = _mm_load_si128(&src6);
__m128i r7 = _mm_load_si128(&src7);
__m128 q0 = _mm_loadl_pi(*(__m128*)&zero, (__m64*)&dest[0*stride]);
__m128 q1 = _mm_loadl_pi(*(__m128*)&zero, (__m64*)&dest[1*stride]);
__m128 q2 = _mm_loadl_pi(*(__m128*)&zero, (__m64*)&dest[2*stride]);
__m128 q3 = _mm_loadl_pi(*(__m128*)&zero, (__m64*)&dest[3*stride]);
__m128 q4 = _mm_loadl_pi(*(__m128*)&zero, (__m64*)&dest[4*stride]);
__m128 q5 = _mm_loadl_pi(*(__m128*)&zero, (__m64*)&dest[5*stride]);
__m128 q6 = _mm_loadl_pi(*(__m128*)&zero, (__m64*)&dest[6*stride]);
__m128 q7 = _mm_loadl_pi(*(__m128*)&zero, (__m64*)&dest[7*stride]);
r0 = _mm_adds_epi16(r0, _mm_unpacklo_epi8(*(__m128i*)&q0, zero));
r1 = _mm_adds_epi16(r1, _mm_unpacklo_epi8(*(__m128i*)&q1, zero));
r2 = _mm_adds_epi16(r2, _mm_unpacklo_epi8(*(__m128i*)&q2, zero));
r3 = _mm_adds_epi16(r3, _mm_unpacklo_epi8(*(__m128i*)&q3, zero));
r4 = _mm_adds_epi16(r4, _mm_unpacklo_epi8(*(__m128i*)&q4, zero));
r5 = _mm_adds_epi16(r5, _mm_unpacklo_epi8(*(__m128i*)&q5, zero));
r6 = _mm_adds_epi16(r6, _mm_unpacklo_epi8(*(__m128i*)&q6, zero));
r7 = _mm_adds_epi16(r7, _mm_unpacklo_epi8(*(__m128i*)&q7, zero));
r0 = _mm_packus_epi16(r0, r1);
r1 = _mm_packus_epi16(r2, r3);
r2 = _mm_packus_epi16(r4, r5);
r3 = _mm_packus_epi16(r6, r7);
_mm_storel_pi((__m64*)&dest[0*stride], *(__m128*)&r0);
_mm_storeh_pi((__m64*)&dest[1*stride], *(__m128*)&r0);
_mm_storel_pi((__m64*)&dest[2*stride], *(__m128*)&r1);
_mm_storeh_pi((__m64*)&dest[3*stride], *(__m128*)&r1);
_mm_storel_pi((__m64*)&dest[4*stride], *(__m128*)&r2);
_mm_storeh_pi((__m64*)&dest[5*stride], *(__m128*)&r2);
_mm_storel_pi((__m64*)&dest[6*stride], *(__m128*)&r3);
_mm_storeh_pi((__m64*)&dest[7*stride], *(__m128*)&r3);
_mm_store_si128(&src0, zero);
_mm_store_si128(&src1, zero);
_mm_store_si128(&src2, zero);
_mm_store_si128(&src3, zero);
_mm_store_si128(&src4, zero);
_mm_store_si128(&src5, zero);
_mm_store_si128(&src6, zero);
_mm_store_si128(&src7, zero);
}
mlib_status
mlib_ImageColorConvert2_F32(
const mlib_f32 *src,
mlib_s32 slb,
mlib_f32 *dst,
mlib_s32 dlb,
mlib_s32 xsize,
mlib_s32 ysize,
const mlib_d64 *fmat,
const mlib_d64 *offset)
{
/* pointers for pixel and line of source */
mlib_f32 *sa, *sl;
/* pointers for pixel and line of destination */
mlib_f32 *da, *dl;
/* indices */
mlib_s32 i, j;
/* intermediate */
__m128 p0, p1, p2, t0, t1, t2, s0, s1, q;
/* packed kernel */
__m128 k0, k1, k2;
/* packed offset */
__m128 off;
/* load transposed kernel */
k0 = _mm_set_ps(0.0f,
(mlib_f32)fmat[6],
(mlib_f32)fmat[3],
(mlib_f32)fmat[0]);
k1 = _mm_set_ps(0.0f,
(mlib_f32)fmat[7],
(mlib_f32)fmat[4],
(mlib_f32)fmat[1]);
k2 = _mm_set_ps(0.0f,
(mlib_f32)fmat[8],
(mlib_f32)fmat[5],
(mlib_f32)fmat[2]);
/* load offset */
off = _mm_set_ps(0.0f,
(mlib_f32)offset[2],
(mlib_f32)offset[1],
(mlib_f32)offset[0]);
sa = sl = (mlib_f32 *)src;
da = dl = dst;
for (j = 0; j < ysize; j++) {
#ifdef __SUNPRO_C
#pragma pipeloop(0)
#endif /* __SUNPRO_C */
for (i = 0; i < (xsize - 1); i ++) {
p0 = _mm_load1_ps(sa);
sa ++;
p1 = _mm_load1_ps(sa);
sa ++;
p2 = _mm_load1_ps(sa);
sa ++;
t0 = _mm_mul_ps(p0, k0);
t1 = _mm_mul_ps(p1, k1);
t2 = _mm_mul_ps(p2, k2);
s0 = _mm_add_ps(t0, t1);
s1 = _mm_add_ps(t2, off);
q = _mm_add_ps(s0, s1);
_mm_storeu_ps(da, q);
da += 3;
}
/*
* process the last pixel of each row separately
* to avoid out of bound write
*/
p0 = _mm_load1_ps(sa);
sa ++;
p1 = _mm_load1_ps(sa);
sa ++;
p2 = _mm_load1_ps(sa);
sa ++;
t0 = _mm_mul_ps(p0, k0);
t1 = _mm_mul_ps(p1, k1);
t2 = _mm_mul_ps(p2, k2);
s0 = _mm_add_ps(t0, t1);
s1 = _mm_add_ps(t2, off);
q = _mm_add_ps(s0, s1);
_mm_storel_pi((__m64 *)da, q);
da += 2;
q = _mm_shuffle_ps(q, q, 0xaa);
_mm_store_ss(da, q);
/* set src pointer to next row */
sa = sl = sl + slb;
/* set dst pointer to next row */
da = dl = dl + dlb;
}
return (MLIB_SUCCESS);
}
// Calculates bounding rectagnle of a point set or retrieves already calculated
static Rect pointSetBoundingRect( const Mat& points )
{
int npoints = points.checkVector(2);
int depth = points.depth();
CV_Assert(npoints >= 0 && (depth == CV_32F || depth == CV_32S));
int xmin = 0, ymin = 0, xmax = -1, ymax = -1, i;
bool is_float = depth == CV_32F;
if( npoints == 0 )
return Rect();
const Point* pts = (const Point*)points.data;
Point pt = pts[0];
#if CV_SSE4_2
if(cv::checkHardwareSupport(CV_CPU_SSE4_2))
{
if( !is_float )
{
__m128i minval, maxval;
minval = maxval = _mm_loadl_epi64((const __m128i*)(&pt)); //min[0]=pt.x, min[1]=pt.y
for( i = 1; i < npoints; i++ )
{
__m128i ptXY = _mm_loadl_epi64((const __m128i*)&pts[i]);
minval = _mm_min_epi32(ptXY, minval);
maxval = _mm_max_epi32(ptXY, maxval);
}
xmin = _mm_cvtsi128_si32(minval);
ymin = _mm_cvtsi128_si32(_mm_srli_si128(minval, 4));
xmax = _mm_cvtsi128_si32(maxval);
ymax = _mm_cvtsi128_si32(_mm_srli_si128(maxval, 4));
}
else
{
__m128 minvalf, maxvalf, z = _mm_setzero_ps(), ptXY = _mm_setzero_ps();
minvalf = maxvalf = _mm_loadl_pi(z, (const __m64*)(&pt));
for( i = 1; i < npoints; i++ )
{
ptXY = _mm_loadl_pi(ptXY, (const __m64*)&pts[i]);
minvalf = _mm_min_ps(minvalf, ptXY);
maxvalf = _mm_max_ps(maxvalf, ptXY);
}
float xyminf[2], xymaxf[2];
_mm_storel_pi((__m64*)xyminf, minvalf);
_mm_storel_pi((__m64*)xymaxf, maxvalf);
xmin = cvFloor(xyminf[0]);
ymin = cvFloor(xyminf[1]);
xmax = cvFloor(xymaxf[0]);
ymax = cvFloor(xymaxf[1]);
}
}
else
#endif
{
if( !is_float )
{
xmin = xmax = pt.x;
ymin = ymax = pt.y;
for( i = 1; i < npoints; i++ )
{
pt = pts[i];
if( xmin > pt.x )
xmin = pt.x;
if( xmax < pt.x )
xmax = pt.x;
if( ymin > pt.y )
ymin = pt.y;
if( ymax < pt.y )
ymax = pt.y;
}
}
else
{
Cv32suf v;
// init values
xmin = xmax = CV_TOGGLE_FLT(pt.x);
ymin = ymax = CV_TOGGLE_FLT(pt.y);
for( i = 1; i < npoints; i++ )
{
pt = pts[i];
pt.x = CV_TOGGLE_FLT(pt.x);
pt.y = CV_TOGGLE_FLT(pt.y);
if( xmin > pt.x )
xmin = pt.x;
if( xmax < pt.x )
xmax = pt.x;
if( ymin > pt.y )
ymin = pt.y;
if( ymax < pt.y )
ymax = pt.y;
}
v.i = CV_TOGGLE_FLT(xmin); xmin = cvFloor(v.f);
v.i = CV_TOGGLE_FLT(ymin); ymin = cvFloor(v.f);
// because right and bottom sides of the bounding rectangle are not inclusive
// (note +1 in width and height calculation below), cvFloor is used here instead of cvCeil
v.i = CV_TOGGLE_FLT(xmax); xmax = cvFloor(v.f);
v.i = CV_TOGGLE_FLT(ymax); ymax = cvFloor(v.f);
}
}
return Rect(xmin, ymin, xmax - xmin + 1, ymax - ymin + 1);
}
/* Calculates bounding rectagnle of a point set or retrieves already calculated */
CV_IMPL CvRect
cvBoundingRect( CvArr* array, int update )
{
CvSeqReader reader;
CvRect rect = { 0, 0, 0, 0 };
CvContour contour_header;
CvSeq* ptseq = 0;
CvSeqBlock block;
CvMat stub, *mat = 0;
int xmin = 0, ymin = 0, xmax = -1, ymax = -1, i, j, k;
int calculate = update;
if( CV_IS_SEQ( array ))
{
ptseq = (CvSeq*)array;
if( !CV_IS_SEQ_POINT_SET( ptseq ))
CV_Error( CV_StsBadArg, "Unsupported sequence type" );
if( ptseq->header_size < (int)sizeof(CvContour))
{
update = 0;
calculate = 1;
}
}
else
{
mat = cvGetMat( array, &stub );
if( CV_MAT_TYPE(mat->type) == CV_32SC2 ||
CV_MAT_TYPE(mat->type) == CV_32FC2 )
{
ptseq = cvPointSeqFromMat(CV_SEQ_KIND_GENERIC, mat, &contour_header, &block);
mat = 0;
}
else if( CV_MAT_TYPE(mat->type) != CV_8UC1 &&
CV_MAT_TYPE(mat->type) != CV_8SC1 )
CV_Error( CV_StsUnsupportedFormat,
"The image/matrix format is not supported by the function" );
update = 0;
calculate = 1;
}
if( !calculate )
return ((CvContour*)ptseq)->rect;
if( mat )
{
CvSize size = cvGetMatSize(mat);
xmin = size.width;
ymin = -1;
for( i = 0; i < size.height; i++ )
{
uchar* _ptr = mat->data.ptr + i*mat->step;
uchar* ptr = (uchar*)cvAlignPtr(_ptr, 4);
int have_nz = 0, k_min, offset = (int)(ptr - _ptr);
j = 0;
offset = MIN(offset, size.width);
for( ; j < offset; j++ )
if( _ptr[j] )
{
have_nz = 1;
break;
}
if( j < offset )
{
if( j < xmin )
xmin = j;
if( j > xmax )
xmax = j;
}
if( offset < size.width )
{
xmin -= offset;
xmax -= offset;
size.width -= offset;
j = 0;
for( ; j <= xmin - 4; j += 4 )
if( *((int*)(ptr+j)) )
break;
for( ; j < xmin; j++ )
if( ptr[j] )
{
xmin = j;
if( j > xmax )
xmax = j;
have_nz = 1;
break;
}
k_min = MAX(j-1, xmax);
k = size.width - 1;
for( ; k > k_min && (k&3) != 3; k-- )
if( ptr[k] )
break;
if( k > k_min && (k&3) == 3 )
{
for( ; k > k_min+3; k -= 4 )
if( *((int*)(ptr+k-3)) )
break;
}
for( ; k > k_min; k-- )
if( ptr[k] )
{
xmax = k;
have_nz = 1;
break;
}
if( !have_nz )
{
j &= ~3;
for( ; j <= k - 3; j += 4 )
if( *((int*)(ptr+j)) )
break;
for( ; j <= k; j++ )
if( ptr[j] )
{
have_nz = 1;
break;
}
}
xmin += offset;
xmax += offset;
size.width += offset;
}
if( have_nz )
{
if( ymin < 0 )
ymin = i;
ymax = i;
}
}
if( xmin >= size.width )
xmin = ymin = 0;
}
else if( ptseq->total )
{
int is_float = CV_SEQ_ELTYPE(ptseq) == CV_32FC2;
cvStartReadSeq( ptseq, &reader, 0 );
CvPoint pt;
CV_READ_SEQ_ELEM( pt, reader );
#if CV_SSE4_2
if(cv::checkHardwareSupport(CV_CPU_SSE4_2))
{
if( !is_float )
{
__m128i minval, maxval;
minval = maxval = _mm_loadl_epi64((const __m128i*)(&pt)); //min[0]=pt.x, min[1]=pt.y
for( i = 1; i < ptseq->total; i++)
{
__m128i ptXY = _mm_loadl_epi64((const __m128i*)(reader.ptr));
CV_NEXT_SEQ_ELEM(sizeof(pt), reader);
minval = _mm_min_epi32(ptXY, minval);
maxval = _mm_max_epi32(ptXY, maxval);
}
xmin = _mm_cvtsi128_si32(minval);
ymin = _mm_cvtsi128_si32(_mm_srli_si128(minval, 4));
xmax = _mm_cvtsi128_si32(maxval);
ymax = _mm_cvtsi128_si32(_mm_srli_si128(maxval, 4));
}
else
{
__m128 minvalf, maxvalf, z = _mm_setzero_ps(), ptXY = _mm_setzero_ps();
minvalf = maxvalf = _mm_loadl_pi(z, (const __m64*)(&pt));
for( i = 1; i < ptseq->total; i++ )
{
ptXY = _mm_loadl_pi(ptXY, (const __m64*)reader.ptr);
CV_NEXT_SEQ_ELEM(sizeof(pt), reader);
minvalf = _mm_min_ps(minvalf, ptXY);
maxvalf = _mm_max_ps(maxvalf, ptXY);
}
float xyminf[2], xymaxf[2];
_mm_storel_pi((__m64*)xyminf, minvalf);
_mm_storel_pi((__m64*)xymaxf, maxvalf);
xmin = cvFloor(xyminf[0]);
ymin = cvFloor(xyminf[1]);
xmax = cvFloor(xymaxf[0]);
ymax = cvFloor(xymaxf[1]);
}
}
else
#endif
{
if( !is_float )
{
xmin = xmax = pt.x;
ymin = ymax = pt.y;
for( i = 1; i < ptseq->total; i++ )
{
CV_READ_SEQ_ELEM( pt, reader );
if( xmin > pt.x )
xmin = pt.x;
if( xmax < pt.x )
xmax = pt.x;
if( ymin > pt.y )
ymin = pt.y;
if( ymax < pt.y )
ymax = pt.y;
}
}
else
{
Cv32suf v;
// init values
xmin = xmax = CV_TOGGLE_FLT(pt.x);
ymin = ymax = CV_TOGGLE_FLT(pt.y);
for( i = 1; i < ptseq->total; i++ )
{
CV_READ_SEQ_ELEM( pt, reader );
pt.x = CV_TOGGLE_FLT(pt.x);
pt.y = CV_TOGGLE_FLT(pt.y);
if( xmin > pt.x )
xmin = pt.x;
if( xmax < pt.x )
xmax = pt.x;
if( ymin > pt.y )
ymin = pt.y;
if( ymax < pt.y )
ymax = pt.y;
}
v.i = CV_TOGGLE_FLT(xmin); xmin = cvFloor(v.f);
v.i = CV_TOGGLE_FLT(ymin); ymin = cvFloor(v.f);
// because right and bottom sides of the bounding rectangle are not inclusive
// (note +1 in width and height calculation below), cvFloor is used here instead of cvCeil
v.i = CV_TOGGLE_FLT(xmax); xmax = cvFloor(v.f);
v.i = CV_TOGGLE_FLT(ymax); ymax = cvFloor(v.f);
}
}
rect.x = xmin;
rect.y = ymin;
rect.width = xmax - xmin + 1;
rect.height = ymax - ymin + 1;
}
if( update )
((CvContour*)ptseq)->rect = rect;
return rect;
}
ibMtx4& ibMtx4::Invert()
{
f32* src = &data.a[0][0];
__m128 minor0, minor1, minor2, minor3;
__m128 row0, row1, row2, row3;
__m128 det, tmp1;
#if !defined NDEBUG || defined STATIC
// Suppress RTC error for uninit vars
f32 init = 0.f;
row3 = row1 = tmp1 = _mm_load_ps1( &init );
#endif // NDEBUG
tmp1 = _mm_loadh_pi(_mm_loadl_pi(tmp1, (__m64*)(src)), (__m64*)(src+ 4));
row1 = _mm_loadh_pi(_mm_loadl_pi(row1, (__m64*)(src+8)), (__m64*)(src+12));
row0 = _mm_shuffle_ps(tmp1, row1, 0x88);
row1 = _mm_shuffle_ps(row1, tmp1, 0xDD);
tmp1 = _mm_loadh_pi(_mm_loadl_pi(tmp1, (__m64*)(src+ 2)), (__m64*)(src+ 6));
row3 = _mm_loadh_pi(_mm_loadl_pi(row3, (__m64*)(src+10)), (__m64*)(src+14));
row2 = _mm_shuffle_ps(tmp1, row3, 0x88);
row3 = _mm_shuffle_ps(row3, tmp1, 0xDD);
// -----------------------------------------------
tmp1 = _mm_mul_ps(row2, row3);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor0 = _mm_mul_ps(row1, tmp1);
minor1 = _mm_mul_ps(row0, tmp1);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor0 = _mm_sub_ps(_mm_mul_ps(row1, tmp1), minor0);
minor1 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor1);
minor1 = _mm_shuffle_ps(minor1, minor1, 0x4E);
// -----------------------------------------------
tmp1 = _mm_mul_ps(row1, row2);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor0 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor0);
minor3 = _mm_mul_ps(row0, tmp1);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row3, tmp1));
minor3 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor3);
minor3 = _mm_shuffle_ps(minor3, minor3, 0x4E);
// -----------------------------------------------
tmp1 = _mm_mul_ps(_mm_shuffle_ps(row1, row1, 0x4E), row3);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
row2 = _mm_shuffle_ps(row2, row2, 0x4E);
minor0 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor0);
minor2 = _mm_mul_ps(row0, tmp1);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row2, tmp1));
minor2 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor2);
minor2 = _mm_shuffle_ps(minor2, minor2, 0x4E);
// -----------------------------------------------
tmp1 = _mm_mul_ps(row0, row1);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor2 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor2);
minor3 = _mm_sub_ps(_mm_mul_ps(row2, tmp1), minor3);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor2 = _mm_sub_ps(_mm_mul_ps(row3, tmp1), minor2);
minor3 = _mm_sub_ps(minor3, _mm_mul_ps(row2, tmp1));
// -----------------------------------------------
tmp1 = _mm_mul_ps(row0, row3);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row2, tmp1));
minor2 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor2);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor1 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor1);
minor2 = _mm_sub_ps(minor2, _mm_mul_ps(row1, tmp1));
// -----------------------------------------------
tmp1 = _mm_mul_ps(row0, row2);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor1 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor1);
minor3 = _mm_sub_ps(minor3, _mm_mul_ps(row1, tmp1));
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row3, tmp1));
minor3 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor3);
// -----------------------------------------------
det = _mm_mul_ps(row0, minor0);
det = _mm_add_ps(_mm_shuffle_ps(det, det, 0x4E), det);
det = _mm_add_ss(_mm_shuffle_ps(det, det, 0xB1), det);
tmp1 = _mm_rcp_ss(det);
det = _mm_sub_ss(_mm_add_ss(tmp1, tmp1), _mm_mul_ss(det, _mm_mul_ss(tmp1, tmp1)));
det = _mm_shuffle_ps(det, det, 0x00);
minor0 = _mm_mul_ps(det, minor0);
_mm_storel_pi((__m64*)(src), minor0);
_mm_storeh_pi((__m64*)(src+2), minor0);
minor1 = _mm_mul_ps(det, minor1);
_mm_storel_pi((__m64*)(src+4), minor1);
_mm_storeh_pi((__m64*)(src+6), minor1);
minor2 = _mm_mul_ps(det, minor2);
_mm_storel_pi((__m64*)(src+ 8), minor2);
_mm_storeh_pi((__m64*)(src+10), minor2);
minor3 = _mm_mul_ps(det, minor3);
_mm_storel_pi((__m64*)(src+12), minor3);
_mm_storeh_pi((__m64*)(src+14), minor3);
return *this;
}
/*---------------------------------------------------------------------------
// 16Byte Allignment calloc
//-------------------------------------------------------------------------*/
void* xmm_calloc(size_t nitems, size_t size)
{
unsigned char* t_RetPtr = (unsigned char*)_aligned_malloc(nitems*size, 16);
if(t_RetPtr)
{
#ifdef __SSE__
size_t i,j, k;
__m128 XMM0, XMM1, XMM2, XMM3;
XMM0 =
XMM1 =
XMM2 =
XMM3 = _mm_setzero_ps();
k = nitems*size;
j = k&(~127);
for(i=0;i<j;i+=128)
{
_mm_stream_ps((float*)(t_RetPtr+i ), XMM0);
_mm_stream_ps((float*)(t_RetPtr+i+ 16), XMM1);
_mm_stream_ps((float*)(t_RetPtr+i+ 32), XMM2);
_mm_stream_ps((float*)(t_RetPtr+i+ 48), XMM3);
_mm_stream_ps((float*)(t_RetPtr+i+ 64), XMM0);
_mm_stream_ps((float*)(t_RetPtr+i+ 80), XMM1);
_mm_stream_ps((float*)(t_RetPtr+i+ 96), XMM2);
_mm_stream_ps((float*)(t_RetPtr+i+112), XMM3);
}
j = k&(~63);
for(;i<j;i+=64)
{
_mm_stream_ps((float*)(t_RetPtr+i ), XMM0);
_mm_stream_ps((float*)(t_RetPtr+i+ 16), XMM1);
_mm_stream_ps((float*)(t_RetPtr+i+ 32), XMM2);
_mm_stream_ps((float*)(t_RetPtr+i+ 48), XMM3);
}
j = k&(~31);
for(;i<j;i+=32)
{
_mm_stream_ps((float*)(t_RetPtr+i ), XMM0);
_mm_stream_ps((float*)(t_RetPtr+i+ 16), XMM1);
}
j = k&(~15);
for(;i<j;i+=16)
{
_mm_stream_ps((float*)(t_RetPtr+i ), XMM0);
}
j = k&(~7);
for(;i<j;i+=8)
{
_mm_storel_pi((__m64*)(t_RetPtr+i ), XMM0);
}
j = k&(~3);
for(;i<j;i+=4)
{
_mm_store_ss((float*)(t_RetPtr+i) , XMM0);
}
for(;i<k;i++)
*(t_RetPtr+i ) = 0;
_mm_sfence();
#else
memset(t_RetPtr, 0, nitems*size);
#endif
}
return (void*)t_RetPtr;
}
int dist_mic(const float* xyz, const int* pairs, const float* box_matrix,
float* distance_out, float* displacement_out,
const int n_frames, const int n_atoms, const int n_pairs) {
/* Compute the distance/displacement between pairs of atoms in every frame
of xyz following the minimum image convention in periodic boundary
conditions.
The computation follows scheme B.9 in Tukerman, M. "Statistical
Mechanics: Theory and Molecular Simulation", 2010.
Parameters
----------
xyz : array, shape=(n_frames, n_atoms, 3)
Cartesian coordinates of the atoms in every frame, in contiguous C order.
pairs : array, shape=(n_pairs, 2)
The specific pairs of atoms whose distance you want to compute. A 2d
array of pairs, in C order.
box_matrix : array, shape=(3,3)
The box matrix for a single frame. All of the frames are assumed to
use this box vector.
distance_out : array, shape=(n_frames, n_pairs)
Array where the distances between pairs will be stored, in contiguous
C order.
displacement_out : array, shaoe=(n_frames, n_pairs, 3), optional
An optional return value: if you'd also like to save the displacement
vectors between the pairs, you can pass a pointer here. If
displacement_out is NULL, then this variable will not be saved back
to memory.
All of the arrays are assumed to be contiguous. This code will
segfault if they're not.
*/
#ifndef __SSE4_1__
_MM_SET_ROUNDING_MODE(_MM_ROUND_NEAREST);
int rounding_mode = _MM_GET_ROUNDING_MODE();
#endif
int i, j;
int store_displacement = displacement_out == NULL ? 0 : 1;
int store_distance = distance_out == NULL ? 0 : 1;
__m128 r1, r2, s12, r12, s, r12_2;
__m128 hinv[3];
__m128 h[3];
for (i = 0; i < n_frames; i++) {
// Store the columns of the box matrix in three float4s. This format
// is fast for matrix * vector product. See, for example, this S.O. question:
// http://stackoverflow.com/questions/14967969/efficient-4x4-matrix-vector-multiplication-with-sse-horizontal-add-and-dot-prod
h[0] = _mm_setr_ps(box_matrix[0], box_matrix[3], box_matrix[6], 0.0f);
h[1] = _mm_setr_ps(box_matrix[1], box_matrix[4], box_matrix[7], 0.0f);
h[2] = _mm_setr_ps(box_matrix[2], box_matrix[5], box_matrix[8], 0.0f);
// Calculate the inverse of the box matrix, and also store it in the same
// format.
inverse33(box_matrix, hinv+0, hinv+1, hinv+2);
for (j = 0; j < n_pairs; j++) {
// Load the two vectors whos distance we want to compute
r1 = load_float3(xyz + 3*pairs[2*j + 0]);
r2 = load_float3(xyz + 3*pairs[2*j + 1]);
r12 = _mm_sub_ps(r2, r1);
// s12 = INVERSE(H) * r12
s12 = _mm_add_ps(_mm_add_ps(
_mm_mul_ps(hinv[0], _mm_shuffle_ps(r12, r12, _MM_SHUFFLE(0,0,0,0))),
_mm_mul_ps(hinv[1], _mm_shuffle_ps(r12, r12, _MM_SHUFFLE(1,1,1,1)))),
_mm_mul_ps(hinv[2], _mm_shuffle_ps(r12, r12, _MM_SHUFFLE(2,2,2,2))));
// s12 = s12 - NEAREST_INTEGER(s12)
#ifdef __SSE4_1__
s12 = _mm_sub_ps(s12, _mm_round_ps(s12, _MM_FROUND_TO_NEAREST_INT));
#else
s12 = _mm_sub_ps(s12, _mm_cvtepi32_ps(_mm_cvtps_epi32(s12)));
#endif
r12 = _mm_add_ps(_mm_add_ps(
_mm_mul_ps(h[0], _mm_shuffle_ps(s12, s12, _MM_SHUFFLE(0,0,0,0))),
_mm_mul_ps(h[1], _mm_shuffle_ps(s12, s12, _MM_SHUFFLE(1,1,1,1)))),
_mm_mul_ps(h[2], _mm_shuffle_ps(s12, s12, _MM_SHUFFLE(2,2,2,2))));
if (store_displacement) {
// store the two lower entries (x,y) in memory
_mm_storel_pi((__m64*)(displacement_out), r12);
displacement_out += 2;
// swap high-low and then store the z entry in the memory
_mm_store_ss(displacement_out++, _mm_movehl_ps(r12, r12));
}
if (store_distance) {
// out = sqrt(sum(r12**2))
r12_2 = _mm_mul_ps(r12, r12);
s = _mm_hadd_ps(r12_2, r12_2);
s = _mm_hadd_ps(s, s);
s = _mm_sqrt_ps(s);
_mm_store_ss(distance_out++, s);
}
}
// advance to the next frame
xyz += n_atoms*3;
box_matrix += 9;
}
#ifndef __SSE4_1__
_MM_SET_ROUNDING_MODE(rounding_mode);
#endif
return 1;
}
void CMatrixMultPoint2D::kernelSSE( float* imgIn, float* imgOut, __m128& m0, __m128& m1, __m128& m2, int i )
{
#if 0
__m128 *pSrcPos = (__m128*)imgIn;
__m64 *pDestPos = (__m64*)imgOut;
for (int j=0;j<SIZE_WIDTH/2 ;j++)
{
__m128 vIn0x, vIn0y, vIn1x, vIn1y;
int index = j+i*SIZE_WIDTH/2;
vIn0x = __MM_SELECT( pSrcPos[index], 0);
vIn0y = __MM_SELECT( pSrcPos[index], 1);
__m128 vOut0;
__m128 tmp00, tmp01;
tmp00 = _mm_mul_ps( m0, vIn0x );
tmp01 = _mm_mul_ps( m1, vIn0y );
vOut0 = _mm_add_ps( tmp00, tmp01 );
vOut0 = _mm_add_ps( vOut0, m2 );
_mm_storel_pi( (__m64*)(&pDestPos[index*2]), vOut0 );
vIn1x = __MM_SELECT( pSrcPos[index], 2);
vIn1y = __MM_SELECT( pSrcPos[index], 3);
__m128 vOut1;
__m128 tmp10, tmp11;
tmp10 = _mm_mul_ps( m0, vIn1x );
tmp11 = _mm_mul_ps( m1, vIn1y );
vOut1 = _mm_add_ps( tmp10, tmp11 );
vOut1 = _mm_add_ps( vOut1, m2 );
_mm_storel_pi( (__m64*)(&pDestPos[index*2+1]), vOut1 );
}
#else
__m128 *pSrcPos = (__m128*)imgIn;
__m128 *pDestPos = (__m128*)imgOut;
int nOffset = i*SIZE_WIDTH/2;
#if OPTIMIZE_OMP && OPTIMIZE_INNER_LOOP
#pragma omp parallel for //shared(nOffset)
#endif
for (int j=0;j<SIZE_WIDTH/2 ;j++)
{
// (x1,x1,x2,x2) (y1,y1,y2,y2)
__m128 vIn = pSrcPos[j+nOffset];
__m128 vInx = _mm_shuffle_ps( vIn, vIn, _MM_SHUFFLE(2,2,0,0) );
__m128 vIny = _mm_shuffle_ps( vIn, vIn, _MM_SHUFFLE(3,3,1,1) );
__m128 tmp00 = _mm_mul_ps( m0, vInx );
__m128 tmp01 = _mm_mul_ps( m1, vIny );
__m128 vOut0 = _mm_add_ps( tmp00, tmp01 );
vOut0 = _mm_add_ps( vOut0, m2 );
pDestPos[j+nOffset] = vOut0 ;
}
#endif
}
/* For data organized into a row for each bit (8 * elem_size rows), transpose
* the bytes. */
int64_t bshuf_trans_byte_bitrow_sse2(void* in, void* out, const size_t size,
const size_t elem_size) {
char* in_b = (char*) in;
char* out_b = (char*) out;
size_t nrows = 8 * elem_size;
size_t nbyte_row = size / 8;
size_t ii, jj;
__m128i a0, b0, c0, d0, e0, f0, g0, h0;
__m128i a1, b1, c1, d1, e1, f1, g1, h1;
__m128 *as, *bs, *cs, *ds, *es, *fs, *gs, *hs;
CHECK_MULT_EIGHT(size);
for (ii = 0; ii + 7 < nrows; ii += 8) {
for (jj = 0; jj + 15 < nbyte_row; jj += 16) {
a0 = _mm_loadu_si128((__m128i *) &in_b[(ii + 0)*nbyte_row + jj]);
b0 = _mm_loadu_si128((__m128i *) &in_b[(ii + 1)*nbyte_row + jj]);
c0 = _mm_loadu_si128((__m128i *) &in_b[(ii + 2)*nbyte_row + jj]);
d0 = _mm_loadu_si128((__m128i *) &in_b[(ii + 3)*nbyte_row + jj]);
e0 = _mm_loadu_si128((__m128i *) &in_b[(ii + 4)*nbyte_row + jj]);
f0 = _mm_loadu_si128((__m128i *) &in_b[(ii + 5)*nbyte_row + jj]);
g0 = _mm_loadu_si128((__m128i *) &in_b[(ii + 6)*nbyte_row + jj]);
h0 = _mm_loadu_si128((__m128i *) &in_b[(ii + 7)*nbyte_row + jj]);
a1 = _mm_unpacklo_epi8(a0, b0);
b1 = _mm_unpacklo_epi8(c0, d0);
c1 = _mm_unpacklo_epi8(e0, f0);
d1 = _mm_unpacklo_epi8(g0, h0);
e1 = _mm_unpackhi_epi8(a0, b0);
f1 = _mm_unpackhi_epi8(c0, d0);
g1 = _mm_unpackhi_epi8(e0, f0);
h1 = _mm_unpackhi_epi8(g0, h0);
a0 = _mm_unpacklo_epi16(a1, b1);
b0 = _mm_unpacklo_epi16(c1, d1);
c0 = _mm_unpackhi_epi16(a1, b1);
d0 = _mm_unpackhi_epi16(c1, d1);
e0 = _mm_unpacklo_epi16(e1, f1);
f0 = _mm_unpacklo_epi16(g1, h1);
g0 = _mm_unpackhi_epi16(e1, f1);
h0 = _mm_unpackhi_epi16(g1, h1);
a1 = _mm_unpacklo_epi32(a0, b0);
b1 = _mm_unpackhi_epi32(a0, b0);
c1 = _mm_unpacklo_epi32(c0, d0);
d1 = _mm_unpackhi_epi32(c0, d0);
e1 = _mm_unpacklo_epi32(e0, f0);
f1 = _mm_unpackhi_epi32(e0, f0);
g1 = _mm_unpacklo_epi32(g0, h0);
h1 = _mm_unpackhi_epi32(g0, h0);
/* We don't have a storeh instruction for integers, so interpret */
/* as a float. Have a storel (_mm_storel_epi64). */
as = (__m128 *) &a1;
bs = (__m128 *) &b1;
cs = (__m128 *) &c1;
ds = (__m128 *) &d1;
es = (__m128 *) &e1;
fs = (__m128 *) &f1;
gs = (__m128 *) &g1;
hs = (__m128 *) &h1;
_mm_storel_pi((__m64 *) &out_b[(jj + 0) * nrows + ii], *as);
_mm_storel_pi((__m64 *) &out_b[(jj + 2) * nrows + ii], *bs);
_mm_storel_pi((__m64 *) &out_b[(jj + 4) * nrows + ii], *cs);
_mm_storel_pi((__m64 *) &out_b[(jj + 6) * nrows + ii], *ds);
_mm_storel_pi((__m64 *) &out_b[(jj + 8) * nrows + ii], *es);
_mm_storel_pi((__m64 *) &out_b[(jj + 10) * nrows + ii], *fs);
_mm_storel_pi((__m64 *) &out_b[(jj + 12) * nrows + ii], *gs);
_mm_storel_pi((__m64 *) &out_b[(jj + 14) * nrows + ii], *hs);
_mm_storeh_pi((__m64 *) &out_b[(jj + 1) * nrows + ii], *as);
_mm_storeh_pi((__m64 *) &out_b[(jj + 3) * nrows + ii], *bs);
_mm_storeh_pi((__m64 *) &out_b[(jj + 5) * nrows + ii], *cs);
_mm_storeh_pi((__m64 *) &out_b[(jj + 7) * nrows + ii], *ds);
_mm_storeh_pi((__m64 *) &out_b[(jj + 9) * nrows + ii], *es);
_mm_storeh_pi((__m64 *) &out_b[(jj + 11) * nrows + ii], *fs);
_mm_storeh_pi((__m64 *) &out_b[(jj + 13) * nrows + ii], *gs);
_mm_storeh_pi((__m64 *) &out_b[(jj + 15) * nrows + ii], *hs);
}
for (jj = nbyte_row - nbyte_row % 16; jj < nbyte_row; jj ++) {
out_b[jj * nrows + ii + 0] = in_b[(ii + 0)*nbyte_row + jj];
out_b[jj * nrows + ii + 1] = in_b[(ii + 1)*nbyte_row + jj];
out_b[jj * nrows + ii + 2] = in_b[(ii + 2)*nbyte_row + jj];
out_b[jj * nrows + ii + 3] = in_b[(ii + 3)*nbyte_row + jj];
out_b[jj * nrows + ii + 4] = in_b[(ii + 4)*nbyte_row + jj];
out_b[jj * nrows + ii + 5] = in_b[(ii + 5)*nbyte_row + jj];
out_b[jj * nrows + ii + 6] = in_b[(ii + 6)*nbyte_row + jj];
out_b[jj * nrows + ii + 7] = in_b[(ii + 7)*nbyte_row + jj];
}
}
return size * elem_size;
}
M_Matrix44
M_MatrixInvert44_SSE(M_Matrix44 A)
{
M_Matrix44 Ainv;
float *src = &A.m[0][0];
float *dst = &Ainv.m[0][0];
__m128 minor0, minor1, minor2, minor3;
__m128 row0, row1, row2, row3;
__m128 det, tmp1;
tmp1 = _mm_loadh_pi(_mm_loadl_pi(tmp1, (__m64 *)(src)),
(__m64 *)(src+4));
row1 = _mm_loadh_pi(_mm_loadl_pi(row1, (__m64 *)(src+8)),
(__m64 *)(src+12));
row0 = _mm_shuffle_ps(tmp1, row1, 0x88);
row1 = _mm_shuffle_ps(row1, tmp1, 0xDD);
tmp1 = _mm_loadh_pi(_mm_loadl_pi(tmp1, (__m64 *)(src+2)),
(__m64 *)(src+6));
row3 = _mm_loadh_pi(_mm_loadl_pi(row3, (__m64 *)(src+10)),
(__m64 *)(src+14));
row2 = _mm_shuffle_ps(tmp1, row3, 0x88);
row3 = _mm_shuffle_ps(row3, tmp1, 0xDD);
tmp1 = _mm_mul_ps(row2, row3);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor0 = _mm_mul_ps(row1, tmp1);
minor1 = _mm_mul_ps(row0, tmp1);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor0 = _mm_sub_ps(_mm_mul_ps(row1, tmp1), minor0);
minor1 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor1);
minor1 = _mm_shuffle_ps(minor1, minor1, 0x4E);
tmp1 = _mm_mul_ps(row1, row2);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor0 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor0);
minor3 = _mm_mul_ps(row0, tmp1);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row3, tmp1));
minor3 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor3);
minor3 = _mm_shuffle_ps(minor3, minor3, 0x4E);
tmp1 = _mm_mul_ps(_mm_shuffle_ps(row1, row1, 0x4E), row3);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
row2 = _mm_shuffle_ps(row2, row2, 0x4E);
minor0 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor0);
minor2 = _mm_mul_ps(row0, tmp1);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor0 = _mm_sub_ps(minor0, _mm_mul_ps(row2, tmp1));
minor2 = _mm_sub_ps(_mm_mul_ps(row0, tmp1), minor2);
minor2 = _mm_shuffle_ps(minor2, minor2, 0x4E);
tmp1 = _mm_mul_ps(row0, row1);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor2 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor2);
minor3 = _mm_sub_ps(_mm_mul_ps(row2, tmp1), minor3);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor2 = _mm_sub_ps(_mm_mul_ps(row3, tmp1), minor2);
minor3 = _mm_sub_ps(minor3, _mm_mul_ps(row2, tmp1));
tmp1 = _mm_mul_ps(row0, row3);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row2, tmp1));
minor2 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor2);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor1 = _mm_add_ps(_mm_mul_ps(row2, tmp1), minor1);
minor2 = _mm_sub_ps(minor2, _mm_mul_ps(row1, tmp1));
tmp1 = _mm_mul_ps(row0, row2);
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0xB1);
minor1 = _mm_add_ps(_mm_mul_ps(row3, tmp1), minor1);
minor3 = _mm_sub_ps(minor3, _mm_mul_ps(row1, tmp1));
tmp1 = _mm_shuffle_ps(tmp1, tmp1, 0x4E);
minor1 = _mm_sub_ps(minor1, _mm_mul_ps(row3, tmp1));
minor3 = _mm_add_ps(_mm_mul_ps(row1, tmp1), minor3);
det = _mm_mul_ps(row0, minor0);
det = _mm_add_ps(_mm_shuffle_ps(det, det, 0x4E), det);
det = _mm_add_ss(_mm_shuffle_ps(det, det, 0xB1), det);
tmp1 = _mm_rcp_ss(det);
det = _mm_sub_ss(_mm_add_ss(tmp1, tmp1),
_mm_mul_ss(det, _mm_mul_ss(tmp1,tmp1)));
det = _mm_shuffle_ps(det, det, 0x00);
minor0 = _mm_mul_ps(det, minor0);
_mm_storel_pi((__m64 *)(dst), minor0);
_mm_storeh_pi((__m64 *)(dst+2), minor0);
minor1 = _mm_mul_ps(det, minor1);
_mm_storel_pi((__m64 *)(dst+4), minor1);
_mm_storeh_pi((__m64 *)(dst+6), minor1);
minor2 = _mm_mul_ps(det, minor2);
_mm_storel_pi((__m64 *)(dst+8), minor2);
_mm_storeh_pi((__m64 *)(dst+10), minor2);
minor3 = _mm_mul_ps(det, minor3);
_mm_storel_pi((__m64 *)(dst+12), minor3);
_mm_storeh_pi((__m64 *)(dst+14), minor3);
return (Ainv);
}
