delta fares: mai 2017

// 3D geometry   support SIMD SSE4.1
// programmed by delta fares

#include <emmintrin.h>
#include <stdio.h>

typedef __m128 vec;
typedef struct {
__m128 x;
__m128 y;
__m128 z;} matrix;

// 3D Math

vec vec4(float x,float y, float z,float w);
vec vec3(float x,float y, float z);
vec vec2(float x,float y);
inline vec operator +(vec a,vec b);
inline vec operator -(vec a,vec b);
inline vec operator *(float k,vec v);
inline vec operator *(vec v,float k);
inline vec operator /(vec v,float k);
inline vec cross(vec a, vec b);
inline float norm(vec v);
inline float norm2(vec v); // SSE4.1
inline float dot(vec a,vec b);
inline float dot2(vec a,vec b); //SSE4.1
inline float dist2(vec a,vec b); //SSE41
inline void normalize(vec& v);
inline void normalize2(vec& v); //SSE4.1
inline vec reflect(vec v,vec n); //SSE4.1

matrix mat3(float a11,float a21,float a31,
             float a12,float a22,float a32,
                   float a13,float a23,float a33);

matrix mat4(float a11,float a21,float a31,float a41,
             float a12,float a22,float a32,float a42,
           float a13,float a23,float a33,float a43);

inline vec operator *(matrix& mat,vec v);
.
.
// body program

vec vec3(float x,float y, float z) {
return _mm_set_ps(0,z,y,x);}
.
.
inline vec operator +(vec a,vec b) {
__asm {
         movaps xmm0,a
       movaps xmm1,b
             addps xmm0,xmm1
      }
};

inline vec operator -(vec a,vec b) {
__asm {
       movaps xmm0,a
       movaps xmm1,b
           subps xmm0,xmm1
      }
};

.
.
inline vec operator *(vec v,float k) {
_asm {
            movss xmm0,k
            shufps xmm0,xmm0,00h
          movaps xmm1,v
          mulps xmm0,xmm1
     }
};
.
.
inline void normalize(vec& v) {
_asm {
              mov    eax,v
            movaps xmm1,[eax]
              movaps xmm0,xmm1   // xmm0 = v
            mulps xmm1,xmm1 // x² y² z²
          movaps xmm2,xmm1
              movaps xmm3,xmm1
              shufps xmm1,xmm1,_MM_SHUFFLE(0, 0, 0, 0)   // xmm1 = x²
              shufps xmm2,xmm2,_MM_SHUFFLE(1, 1, 1, 1)   // xmm2 = y²
              shufps xmm3,xmm3,_MM_SHUFFLE(2, 2, 2, 2)   // xmm3 = z²
              addps xmm1,xmm2
          addps xmm1,xmm3            // xmm1 = x²+y²+z²
          sqrtps xmm1,xmm1             // xmm1= sqrt(v*v)
            divps xmm0,xmm1              // xmm0 = v/sqrt(v*v)
              movaps [eax],xmm0
       }

}
.
.
.
inline vec reflect(vec v,vec n) {

   return 2*dot2(v,n)*n-v;
}

inline vec operator *(matrix& mat,vec v) {
   vec res;
res.m128_f32[0] = dot2(mat.x,v);
   res.m128_f32[1] = dot2(mat.y,v);
   res.m128_f32[2] = dot2(mat.z,v);
   res.m128_f32[3] = 0;
   return res;
}

int main(int argc, char* argv[]) {

matrix mat = mat3(1 , 8,3,
                 3,-1,1,
               4, 2,1);

vec a,b,c,V;

a = vec3(51,13,10);
b = vec3(1,1,0);

c =dot(a,b)*cross(a,b)+3*b+a;
show(c);
normalize2(c);
show(c);

vec r = reflect(b,a);
show(r);

printf("%f %f %f\n", dot(a,b),dot2(a,b));   // compare SSE2 with SSE4.1

V = mat*a;
show(V);
return 0;
}