include/crbn/basic/vec3_sse.hpp

#ifndef __vec3d_sse_hpp__
#define __vec3d_sse_hpp__

#include <crbn/basic/scalar.hpp>

#include <xmmintrin.h>

struct vec3_sse {
  // Force data alignement
  union{
    __m128 _vector;
    struct { float w, x, y, z; };
  };

  vec3() : x( 0.f ), y( 0.f ), z( 0.f ) {}
  vec3( const float& a ) : x( a ), y( a ), z( a ) {}
  vec3( const float& a, const float& b, const float& c ) : x( a ), y( b ), z( c ) {}
  vec3( const vec3& v ) : x( v.x ), y( v.y ), z( v.z ) {}
};

typedef vec3_sse __attribute__ ((aligned (16))) vec3;

// a = b + c
static inline void
vadd( vec3& a, const vec3& b, const vec3& c ) {
  a.x = b.x + c.x;
  a.y = b.y + c.y;
  a.z = b.z + c.z;
  //a._vector = _mm_add_ps( b._vector, c._vector );
}
// a += b
static inline void
vadd( vec3& a, const vec3& b ) {
  a.x += b.x;
  a.y += b.y;
  a.z += b.z;
  //a._vector = _mm_add_ps( a._vector, b._vector );
}
// a = b - c
static inline void
vsub( vec3& a, const vec3& b, const vec3& c ){
  a.x = b.x - c.x;
  a.y = b.y - c.y;
  a.z = b.z - c.z;
  //a._vector = _mm_sub_ps( b._vector, c._vector );
} 
// a -= b
static inline void
vsub( vec3& a, const vec3& b ) {
  a.x -= b.x;
  a.y -= b.y;
  a.z -= b.z;
  //a._vector = _mm_sub_ps( a._vector, b._vector );
}
// a = -b
static inline void
vneg( vec3& a, const vec3& b ) {
  a.x = - b.x;
  a.y = - b.y;
  a.z = - b.z;
  //vec3 tmp( 0.f );
  //a._vector = _mm_sub_ps( tmp._vector, b._vector );
}
// a = -a
static inline void
vneg( vec3& a ) {
  a.x = - a.x;
  a.y = - a.y;
  a.z = - a.z;
  vec3 tmp( 0.f );
  a._vector = _mm_sub_ps( tmp._vector, a._vector );
}

//  a = f * b
static inline void
vmul( vec3& a, const float f, const vec3& b ) {
  a.x = f * b.x;
  a.y = f * b.y;
  a.z = f * b.z;  
//   vec3 tmp( f );
//   a._vector = _mm_mul_ps( tmp._vector, b._vector );
}
//  a *= f
static inline void
vmul( vec3& a, const float& f ) {
  a.x *= f;
  a.y *= f;
  a.z *= f;  
  //vec3 tmp( f );
  //a._vector = _mm_mul_ps( tmp._vector, a._vector );
}
//  a = b / f
static inline void
vdiv( vec3& a, const vec3& b, const float f ) {
  f = 1.f / f;
  a.x = f * b.x;
  a.y = f * b.y;
  a.z = f * b.z;  
//   vec3 tmp( 1.f / f );
//   a._vector = _mm_mul_ps( b._vector, tmp._vector );
}
//  a /= f
static inline void
vdiv( vec3& a, const float f ) {
  f = 1.f / f;
  a.x *= f;
  a.y *= f;
  a.z *= f;  
//   vec3 tmp( 1.f / f );
//   a._vector = _mm_mul_ps( a._vector, tmp._vector );
}

// a_i = b_i * c_i for each component
static inline void
vmul( vec3& a, const vec3& b, const vec3& c ) {
  a.x = b.x * c.x;
  a.y = b.y * c.y;
  a.z = b.z * c.z;
  //a._vector = _mm_mul_ps( b._vector, c._vector );
}
// a_i *= b_i for each component
static inline void
vmul( vec3& a, vec3& b ) {
  a.x *= b.x;
  a.y *= b.y;
  a.z *= b.z;
  //a._vector = _mm_mul_ps( a._vector, b._vector );
}

// a_i = b_i / c_i for each component
static inline void
vdiv( vec3& a, const vec3& b, const vec3& c ) {
  a.x = b.x / c.x;
  a.y = b.y / c.y;
  a.z = b.z / c.z;
  //a._vector = _mm_div_ps( b._vector, c._vector );
}
// a_i /= b_i for each component
static inline void
vdiv( vec3& a, const vec3& b ) {
  a.x /= b.x;
  a.y /= b.y;
  a.z /= b.z;
  //a._vector = _mm_div_ps( a._vector, b._vector );
}

// a = b + f * c
static inline void
vaddfmul( vec3& a, const vec3& b, const float& f, const vec3& c ) {
  vec3 tmp( f );
  a._vector = _mm_mul_ps( tmp._vector, c._vector );
  a._vector = _mm_add_ps( a._vector, b._vector );
}

// a += f * b
static inline void
vaddfmul( vec3& a, const float& f, const vec3& b ) {
  vec3 tmp( f );
  tmp._vector = _mm_mul_ps( tmp._vector, b._vector );
  a._vector = _mm_add_ps( a._vector, tmp._vector );
}

// a = b + c * d
static inline void
vaddmul( vec3& a, const vec3& b, const vec3& c, const vec3& d ) {
  a._vector = _mm_mul_ps( c._vector, d._vector );
  a._vector = _mm_add_ps( a._vector, b._vector );
}

// a += b * c
static inline void
vaddmul( vec3& a, const vec3& b, const vec3& c ) {
  vec3 tmp;
  tmp._vector = _mm_mul_ps( b._vector, c._vector );
  a._vector = _mm_add_ps( a._vector, tmp._vector );
}

// a = normalize( b )
static inline void
vnormalize( vec3& a, const vec3& b ) {
  vec3 tmp;
  a._vector = _mm_mul_ps( b._vector, b._vector );
  tmp._vector = _mm_shuffle_ps( a._vector, a._vector, 0x31 );
  a._vector = _mm_add_ps( a._vector, tmp._vector );
  tmp._vector = _mm_shuffle_ps( a._vector, a._vector, 0x2 );
  a._vector = _mm_add_ps( a._vector, tmp._vector );
  a._vector = _mm_rsqrt_ss( a._vector );
  a._vector = _mm_shuffle_ps( a._vector, a._vector, 0x0 );
  a._vector = _mm_mul_ps( a._vector, b._vector );
}
// a = normalize( a )
static inline void
vnormalize( vec3& a ) {
  vec3 tmp, v;
  b._vector = _mm_mul_ps( a._vector, a._vector );
  tmp._vector = _mm_shuffle_ps( b._vector, b._vector, 0x31 );
  b._vector = _mm_add_ps( b._vector, tmp._vector );
  tmp._vector = _mm_shuffle_ps( b._vector, b._vector, 0x2 );
  b._vector = _mm_add_ps( b._vector, tmp._vector );
  b._vector = _mm_rsqrt_ss( b._vector );
  b._vector = _mm_shuffle_ps( b._vector, b._vector, 0x0 );
  a._vector = _mm_mul_ps( a._vector, b._vector );
}
// ret = norm( a )
static inline float
vlength( const vec3& a ) {
  vec3 tmp, b;
  float f;
  b._vector = _mm_mul_ps( a._vector, a._vector );
  tmp._vector = _mm_shuffle_ps( b._vector, b._vector, 0x31 );
  b._vector = _mm_add_ps( b._vector, tmp._vector );
  tmp._vector = _mm_shuffle_ps( b._vector, b._vector, 0x2 );
  b._vector = _mm_add_ps( b._vector, tmp._vector );
  b._vector = _mm_rsqrt_ss( b._vector );
  _mm_store_ss( &f, b._vector );
  return f;
}

// ret = norm( a ) ^ 2
static inline float
vlength2( const vec3& a ) {
  vec3 tmp, b;
  float f;
  b._vector = _mm_mul_ps( a._vector, a._vector );
  tmp._vector = _mm_shuffle_ps( b._vector, b._vector, 0x31 );
  b._vector = _mm_add_ps( b._vector, tmp._vector );
  tmp._vector = _mm_shuffle_ps( b._vector, b._vector, 0x2 );
  b._vector = _mm_add_ps( b._vector, tmp._vector );
  _mm_store_ss( &f, b._vector );
  return f;
}

// ret = distance( a, b )
static inline float
vdistance( const vec3& a, const vec3& b ) {
  vec3 tmp;
  tmp._vector = _mm_sub_ps( a._vector, b._vector );
  return vlength( tmp );
}
// ret = distance( a, b ) ^ 2
static inline float
vdistance2( const vec3& a, const vec3& b ) {
  vec3 tmp;
  tmp._vector = _mm_sub_ps( a._vector, b._vector );
  return vlength2( tmp );
}

// ret = a . b
static inline float
vdot( const vec3& a, const vec3& b ) {
  /*
    vec3 tmp, tmp2;
    float f;
    tmp._vector = _mm_mul_ps( a._vector, b._vector );
    tmp2._vector = _mm_shuffle_ps( tmp._vector, tmp._vector, 0x31 );
    tmp._vector = _mm_add_ps( tmp._vector, tmp._vector );
    tmp2._vector = _mm_shuffle_ps( tmp._vector, tmp._vector, 0x2 );
    _mm_store_ss( &f, b._vector );
    return f;
  */
  return a.x * b.x + a.y * b.y + a.z * b.z;
}
// a = b ^ c
static inline void
vcross( vec3& a, const vec3& b, const vec3& c ) {
  a.x = b.y * c.z - b.z * c.y;
  a.y = b.z * c.x - b.x * c.z;
  a.z = b.x * c.y - b.y * c.x;
}

// v_i = min( a_i, b_i ) for each component
static inline void
vmin( vec3& v, const vec3& a, const vec3& b ) {
  v.x = min( a.x, b.x );
  v.y = min( a.y, b.y );
  v.z = min( a.z, b.z );
}
// v_i = max( a_i, b_i ) for each component
static inline void
vmax( vec3& v, const vec3& a, const vec3& b ) {
  v.x = max( a.x, b.x );
  v.y = max( a.y, b.y );
  v.z = max( a.z, b.z );
}

// compute coordinate system from a vector
static inline void
coordinate_system(vec3& v1, vec3& v2, vec3& v3) {
  float inv_length;
  if(fabs(v1.x) > fabs(v1.y)) {
    inv_length = 1.0f / sqrtf((v1.x * v1.x) + (v1.z * v1.z));
    v2 = vec3(-v1.z * inv_length, 0.0f, v1.x * inv_length);
  }
  else {
    inv_length = 1.0f / sqrtf((v1.y * v1.y) + (v1.z * v1.z));
    v2 = vec3(0.0f, v1.z * inv_length, -v1.y * inv_length);
  }
  vcross(v3, v1, v2);
}

#endif // __vec3d_sse_hpp__

---