FastMath 3D Geometry in pure inline assembly language supports SIMD SSE 4.1

//  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;
}