408 lines
10 KiB
C++
408 lines
10 KiB
C++
#ifndef _VECTOR3_SSE_H
|
|
#define _VECTOR3_SSE_H
|
|
//------------------------------------------------------------------------------
|
|
/**
|
|
SSE based vector3 class.
|
|
|
|
@author
|
|
- RadonLabs GmbH
|
|
|
|
@since
|
|
- 2005.7.06
|
|
@remarks
|
|
- Áö¿Ï Ãß°¡
|
|
*/
|
|
|
|
#include <xmmintrin.h>
|
|
#include <math.h>
|
|
|
|
//------------------------------------------------------------------------------
|
|
class _vector3_sse
|
|
{
|
|
public:
|
|
/// constructor 1
|
|
_vector3_sse();
|
|
/// constructor 2
|
|
_vector3_sse(const float _x, const float _y, const float _z);
|
|
/// constructor 3
|
|
_vector3_sse(const _vector3_sse& vec);
|
|
/// constructor 4
|
|
_vector3_sse(const float* p);
|
|
/// private constructor
|
|
_vector3_sse(const __m128& m);
|
|
/// set elements 1
|
|
void set(const float _x, const float _y, const float _z);
|
|
/// set elements 2
|
|
void set(const _vector3_sse& vec);
|
|
/// set elements 3
|
|
void set(const float* p);
|
|
/// return length
|
|
float len() const;
|
|
/// return length squared
|
|
float lensquared() const;
|
|
/// normalize
|
|
void norm();
|
|
/// inplace add
|
|
void operator +=(const _vector3_sse& v0);
|
|
/// inplace sub
|
|
void operator -=(const _vector3_sse& v0);
|
|
/// inplace scalar multiplication
|
|
void operator *=(float s);
|
|
/// fuzzy compare
|
|
bool isequal(const _vector3_sse& v, float tol) const;
|
|
/// fuzzy compare, returns -1, 0, +1
|
|
int compare(const _vector3_sse& v, float tol) const;
|
|
/// rotate around axis
|
|
void rotate(const _vector3_sse& axis, float angle);
|
|
/// inplace linear interpolation
|
|
void lerp(const _vector3_sse& v0, float lerpVal);
|
|
/// returns a vector orthogonal to self, not normalized
|
|
_vector3_sse findortho() const;
|
|
|
|
union
|
|
{
|
|
__m128 m128;
|
|
struct
|
|
{
|
|
float x, y, z, pad;
|
|
};
|
|
};
|
|
};
|
|
|
|
//------------------------------------------------------------------------------
|
|
/**
|
|
*/
|
|
inline
|
|
_vector3_sse::_vector3_sse()
|
|
{
|
|
m128 = _mm_setzero_ps();
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
/**
|
|
*/
|
|
inline
|
|
_vector3_sse::_vector3_sse(const float _x, const float _y, const float _z)
|
|
{
|
|
m128 = _mm_set_ps(0.0f, _z, _y, _x);
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
/**
|
|
*/
|
|
inline
|
|
_vector3_sse::_vector3_sse(const _vector3_sse& vec)
|
|
{
|
|
m128 = vec.m128;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
/**
|
|
*/
|
|
inline
|
|
_vector3_sse::_vector3_sse(const __m128& m)
|
|
{
|
|
m128 = m;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
/**
|
|
*/
|
|
inline
|
|
void
|
|
_vector3_sse::set(const float _x, const float _y, const float _z)
|
|
{
|
|
m128 = _mm_set_ps(0.0f, _z, _y, _x);
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
/**
|
|
*/
|
|
inline
|
|
void
|
|
_vector3_sse::set(const _vector3_sse& vec)
|
|
{
|
|
m128 = vec.m128;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
/**
|
|
*/
|
|
inline
|
|
float
|
|
_vector3_sse::len() const
|
|
{
|
|
static const int X = 0;
|
|
static const int Y = 1;
|
|
static const int Z = 2;
|
|
static const int W = 3;
|
|
|
|
__m128 a = _mm_mul_ps(m128, m128);
|
|
|
|
// horizontal add
|
|
__m128 b = _mm_add_ss(_mm_shuffle_ps(a, a, _MM_SHUFFLE(X,X,X,X)), _mm_add_ss(_mm_shuffle_ps(a, a, _MM_SHUFFLE(Y,Y,Y,Y)), _mm_shuffle_ps(a, a, _MM_SHUFFLE(Z,Z,Z,Z))));
|
|
__m128 l = _mm_sqrt_ss(b);
|
|
return l.m128_f32[X];
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
/**
|
|
*/
|
|
inline
|
|
float
|
|
_vector3_sse::lensquared() const
|
|
{
|
|
static const int X = 0;
|
|
static const int Y = 1;
|
|
static const int Z = 2;
|
|
static const int W = 3;
|
|
|
|
__m128 a = _mm_mul_ps(m128, m128);
|
|
__m128 b = _mm_add_ss(_mm_shuffle_ps(a, a, _MM_SHUFFLE(X,X,X,X)), _mm_add_ss(_mm_shuffle_ps(a, a, _MM_SHUFFLE(Y,Y,Y,Y)), _mm_shuffle_ps(a, a, _MM_SHUFFLE(Z,Z,Z,Z))));
|
|
return b.m128_f32[X];
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
/**
|
|
*/
|
|
inline
|
|
void
|
|
_vector3_sse::norm()
|
|
{
|
|
static const int X = 0;
|
|
static const int Y = 1;
|
|
static const int Z = 2;
|
|
static const int W = 3;
|
|
|
|
__m128 a = _mm_mul_ps(m128, m128);
|
|
|
|
// horizontal add
|
|
__m128 b = _mm_add_ss(_mm_shuffle_ps(a, a, _MM_SHUFFLE(X,X,X,X)), _mm_add_ss(_mm_shuffle_ps(a, a, _MM_SHUFFLE(Y,Y,Y,Y)), _mm_shuffle_ps(a, a, _MM_SHUFFLE(Z,Z,Z,Z))));
|
|
|
|
// get reciprocal of square root of squared length
|
|
__m128 f = _mm_rsqrt_ss(b);
|
|
__m128 oneDivLen = _mm_shuffle_ps(f, f, _MM_SHUFFLE(X,X,X,X));
|
|
|
|
m128 = _mm_mul_ps(m128, oneDivLen);
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
/**
|
|
*/
|
|
inline
|
|
void
|
|
_vector3_sse::operator +=(const _vector3_sse& v)
|
|
{
|
|
m128 = _mm_add_ps(m128, v.m128);
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
/**
|
|
*/
|
|
inline
|
|
void
|
|
_vector3_sse::operator -=(const _vector3_sse& v)
|
|
{
|
|
m128 = _mm_sub_ps(m128, v.m128);
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
/**
|
|
*/
|
|
inline
|
|
void
|
|
_vector3_sse::operator *=(float s)
|
|
{
|
|
__m128 packed = _mm_set1_ps(s);
|
|
m128 = _mm_mul_ps(m128, packed);
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
/**
|
|
*/
|
|
inline
|
|
bool
|
|
_vector3_sse::isequal(const _vector3_sse& v, float tol) const
|
|
{
|
|
if (fabs(v.x - x) > tol) return false;
|
|
else if (fabs(v.y - y) > tol) return false;
|
|
else if (fabs(v.z - z) > tol) return false;
|
|
return true;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
/**
|
|
*/
|
|
inline
|
|
int
|
|
_vector3_sse::compare(const _vector3_sse& v, float tol) const
|
|
{
|
|
if (fabs(v.x - x) > tol) return (v.x > x) ? +1 : -1;
|
|
else if (fabs(v.y - y) > tol) return (v.y > y) ? +1 : -1;
|
|
else if (fabs(v.z - z) > tol) return (v.z > z) ? +1 : -1;
|
|
else return 0;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
/**
|
|
*/
|
|
inline
|
|
void
|
|
_vector3_sse::rotate(const _vector3_sse& axis, float angle)
|
|
{
|
|
// rotates this one around given vector. We do
|
|
// rotation with matrices, but these aren't defined yet!
|
|
float rotM[9];
|
|
float sa, ca;
|
|
|
|
sa = (float) sin(angle);
|
|
ca = (float) cos(angle);
|
|
|
|
// build a rotation matrix
|
|
rotM[0] = ca + (1 - ca) * axis.x * axis.x;
|
|
rotM[1] = (1 - ca) * axis.x * axis.y - sa * axis.z;
|
|
rotM[2] = (1 - ca) * axis.z * axis.x + sa * axis.y;
|
|
rotM[3] = (1 - ca) * axis.x * axis.y + sa * axis.z;
|
|
rotM[4] = ca + (1 - ca) * axis.y * axis.y;
|
|
rotM[5] = (1 - ca) * axis.y * axis.z - sa * axis.x;
|
|
rotM[6] = (1 - ca) * axis.z * axis.x - sa * axis.y;
|
|
rotM[7] = (1 - ca) * axis.y * axis.z + sa * axis.x;
|
|
rotM[8] = ca + (1 - ca) * axis.z * axis.z;
|
|
|
|
// "handmade" multiplication
|
|
_vector3_sse help(rotM[0] * this->x + rotM[1] * this->y + rotM[2] * this->z,
|
|
rotM[3] * this->x + rotM[4] * this->y + rotM[5] * this->z,
|
|
rotM[6] * this->x + rotM[7] * this->y + rotM[8] * this->z);
|
|
*this = help;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
/**
|
|
*/
|
|
static
|
|
inline
|
|
_vector3_sse operator +(const _vector3_sse& v0, const _vector3_sse& v1)
|
|
{
|
|
return _vector3_sse(_mm_add_ps(v0.m128, v1.m128));
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
/**
|
|
*/
|
|
static
|
|
inline
|
|
_vector3_sse operator -(const _vector3_sse& v0, const _vector3_sse& v1)
|
|
{
|
|
return _vector3_sse(_mm_sub_ps(v0.m128, v1.m128));
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
/**
|
|
*/
|
|
static
|
|
inline
|
|
_vector3_sse operator *(const _vector3_sse& v0, const float s)
|
|
{
|
|
__m128 packed = _mm_set1_ps(s);
|
|
return _vector3_sse(_mm_mul_ps(v0.m128, packed));
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
/**
|
|
*/
|
|
static
|
|
inline
|
|
_vector3_sse operator -(const _vector3_sse& v)
|
|
{
|
|
__m128 zero = _mm_setzero_ps();
|
|
return _vector3_sse(_mm_sub_ps(zero, v.m128));
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
/**
|
|
Dot product.
|
|
*/
|
|
static
|
|
inline
|
|
float operator %(const _vector3_sse& v0, const _vector3_sse& v1)
|
|
{
|
|
__m128 a = _mm_mul_ps(v0.m128, v1.m128);
|
|
__m128 b = _mm_add_ss(_mm_shuffle_ps(a, a, _MM_SHUFFLE(0,0,0,0)), _mm_add_ss(_mm_shuffle_ps(a, a, _MM_SHUFFLE(1,1,1,1)), _mm_shuffle_ps(a, a, _MM_SHUFFLE(2,2,2,2))));
|
|
return b.m128_f32[0];
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
/**
|
|
Cross product.
|
|
*/
|
|
static
|
|
inline
|
|
_vector3_sse operator *(const _vector3_sse& v0, const _vector3_sse& v1)
|
|
{
|
|
// x = v0.y * v1.z - v0.z * v1.y
|
|
// y = v0.z * v1.x - v0.x * v1.z
|
|
// z = v0.x * v1.y - v0.y * v1.x
|
|
//
|
|
// a = v0.y | v0.z | v0.x | xxx
|
|
// b = v1.z | v1.x | v1.y | xxx
|
|
// c = v0.z | v0.x | v0.y | xxx
|
|
// d = v1.y | v1.z | v1.x | xxx
|
|
//
|
|
|
|
static const int X = 0;
|
|
static const int Y = 1;
|
|
static const int Z = 2;
|
|
static const int W = 3;
|
|
|
|
__m128 a = _mm_shuffle_ps(v0.m128, v0.m128, _MM_SHUFFLE(W, X, Z, Y));
|
|
__m128 b = _mm_shuffle_ps(v1.m128, v1.m128, _MM_SHUFFLE(W, Y, X, Z));
|
|
__m128 c = _mm_shuffle_ps(v0.m128, v0.m128, _MM_SHUFFLE(W, Y, X, Z));
|
|
__m128 d = _mm_shuffle_ps(v1.m128, v1.m128, _MM_SHUFFLE(W, X, Z, Y));
|
|
|
|
__m128 e = _mm_mul_ps(a, b);
|
|
__m128 f = _mm_mul_ps(c, d);
|
|
|
|
return _vector3_sse(_mm_sub_ps(e, f));
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
/**
|
|
*/
|
|
inline
|
|
void
|
|
_vector3_sse::lerp(const _vector3_sse& v0, float lerpVal)
|
|
{
|
|
x = v0.x + ((x - v0.x) * lerpVal);
|
|
y = v0.y + ((y - v0.y) * lerpVal);
|
|
z = v0.z + ((z - v0.z) * lerpVal);
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
/**
|
|
Find a vector that is orthogonal to self. Self should not be (0,0,0).
|
|
Return value is not normalized.
|
|
*/
|
|
inline
|
|
_vector3_sse
|
|
_vector3_sse::findortho() const
|
|
{
|
|
if (0.0 != x)
|
|
{
|
|
return _vector3_sse((-y - z) / x, 1.0, 1.0);
|
|
} else
|
|
if (0.0 != y)
|
|
{
|
|
return _vector3_sse(1.0, (-x - z) / y, 1.0);
|
|
} else
|
|
if (0.0 != z)
|
|
{
|
|
return _vector3_sse(1.0, 1.0, (-x - y) / z);
|
|
} else
|
|
{
|
|
return _vector3_sse(0.0, 0.0, 0.0);
|
|
}
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
#endif
|