📄 _matrix33_sse.h
字号:
M21 = (float)(cj*sh); M22 = (float)(sj*ss+cc); M23 = (float)(sj*cs-sc);
M31 = (float)(-sj); M32 = (float)(cj*si); M33 = (float)(cj*ci);
}
// flip row/column
this->transpose();
}
//------------------------------------------------------------------------------
/**
*/
inline
void
_matrix33_sse::lookat(const _vector3_sse& from, const _vector3_sse& to, const _vector3_sse& up)
{
_vector3_sse z(from - to);
z.norm();
_vector3_sse y(up);
_vector3_sse x(y * z); // x = y cross z
y = z * x; // y = z cross x
x.norm();
y.norm();
m1 = x.m128;
m2 = y.m128;
m3 = z.m128;
}
//------------------------------------------------------------------------------
/**
*/
inline
void
_matrix33_sse::billboard(const _vector3_sse& from, const _vector3_sse& to, const _vector3_sse& up)
{
_vector3_sse z(from - to);
z.norm();
_vector3_sse y(up);
_vector3_sse x(y * z);
z = x * y;
x.norm();
y.norm();
z.norm();
m1 = x.m128;
m2 = y.m128;
m3 = z.m128;
}
//------------------------------------------------------------------------------
/**
*/
inline
void
_matrix33_sse::set(float m11, float m12, float m13,
float m21, float m22, float m23,
float m31, float m32, float m33)
{
m1 = _mm_set_ps(0.0f, m13, m12, m11);
m2 = _mm_set_ps(0.0f, m23, m22, m21);
m3 = _mm_set_ps(0.0f, m33, m32, m31);
}
//------------------------------------------------------------------------------
/**
*/
inline
void
_matrix33_sse::set(const _vector3_sse& v1, const _vector3_sse& v2, const _vector3_sse& v3)
{
m1 = v1.m128;
m2 = v2.m128;
m3 = v3.m128;
}
//------------------------------------------------------------------------------
/**
*/
inline
void
_matrix33_sse::set(const _matrix33_sse& mx)
{
m1 = mx.m1;
m2 = mx.m2;
m3 = mx.m3;
}
//------------------------------------------------------------------------------
/**
*/
inline
void
_matrix33_sse::ident()
{
memcpy(&(m[0][0]), _matrix33_sse_ident, sizeof(_matrix33_sse_ident));
}
//------------------------------------------------------------------------------
/**
FIXME: SSE OPTIMIZATION
*/
inline
void
_matrix33_sse::transpose()
{
#undef n_swap
#define n_swap(x,y) { float t=x; x=y; y=t; }
n_swap(m[0][1],m[1][0]);
n_swap(m[0][2],m[2][0]);
n_swap(m[1][2],m[2][1]);
}
//------------------------------------------------------------------------------
/**
FIXME: SSE OPTIMIZATION
*/
inline
bool
_matrix33_sse::orthonorm(float limit)
{
if (((M11*M21+M12*M22+M13*M23)<limit) &&
((M11*M31+M12*M32+M13*M33)<limit) &&
((M31*M21+M32*M22+M33*M23)<limit) &&
((M11*M11+M12*M12+M13*M13)>(1.0-limit)) &&
((M11*M11+M12*M12+M13*M13)<(1.0+limit)) &&
((M21*M21+M22*M22+M23*M23)>(1.0-limit)) &&
((M21*M21+M22*M22+M23*M23)<(1.0+limit)) &&
((M31*M31+M32*M32+M33*M33)>(1.0-limit)) &&
((M31*M31+M32*M32+M33*M33)<(1.0+limit)))
return true;
else
return false;
}
//------------------------------------------------------------------------------
/**
*/
inline
void
_matrix33_sse::scale(const _vector3_sse& s)
{
m1 = _mm_mul_ps(m1, s.m128);
m2 = _mm_mul_ps(m2, s.m128);
m3 = _mm_mul_ps(m3, s.m128);
}
//------------------------------------------------------------------------------
/**
FIXME: SSE OPTIMIZATION
*/
inline
void
_matrix33_sse::rotate_x(const float a)
{
float c = n_cos(a);
float s = n_sin(a);
int i;
for (i=0; i<3; i++)
{
float mi1 = m[i][1];
float mi2 = m[i][2];
m[i][1] = mi1*c + mi2*-s;
m[i][2] = mi1*s + mi2*c;
}
}
//------------------------------------------------------------------------------
/**
FIXME: SSE OPTIMIZATION
*/
inline
void
_matrix33_sse::rotate_y(const float a)
{
float c = n_cos(a);
float s = n_sin(a);
int i;
for (i=0; i<3; i++)
{
float mi0 = m[i][0];
float mi2 = m[i][2];
m[i][0] = mi0*c + mi2*s;
m[i][2] = mi0*-s + mi2*c;
}
}
//------------------------------------------------------------------------------
/**
FIXME: SSE OPTIMIZATION
*/
inline
void
_matrix33_sse::rotate_z(const float a)
{
float c = n_cos(a);
float s = n_sin(a);
int i;
for (i=0; i<3; i++)
{
float mi0 = m[i][0];
float mi1 = m[i][1];
m[i][0] = mi0*c + mi1*-s;
m[i][1] = mi0*s + mi1*c;
}
}
//------------------------------------------------------------------------------
/**
FIXME: SSE OPTIMIZATION
*/
inline
void
_matrix33_sse::rotate_local_x(const float a)
{
_matrix33_sse rotM; // initialized as identity matrix
rotM.M22 = (float) cos(a); rotM.M23 = -(float) sin(a);
rotM.M32 = (float) sin(a); rotM.M33 = (float) cos(a);
(*this) = rotM * (*this);
}
//------------------------------------------------------------------------------
/**
FIXME: SSE OPTIMIZATION
*/
inline
void
_matrix33_sse::rotate_local_y(const float a)
{
_matrix33_sse rotM; // initialized as identity matrix
rotM.M11 = (float) cos(a); rotM.M13 = (float) sin(a);
rotM.M31 = -(float) sin(a); rotM.M33 = (float) cos(a);
(*this) = rotM * (*this);
}
//------------------------------------------------------------------------------
/**
FIXME: SSE OPTIMIZATION
*/
inline
void
_matrix33_sse::rotate_local_z(const float a)
{
_matrix33_sse rotM; // initialized as identity matrix
rotM.M11 = (float) cos(a); rotM.M12 = -(float) sin(a);
rotM.M21 = (float) sin(a); rotM.M22 = (float) cos(a);
(*this) = rotM * (*this);
}
//------------------------------------------------------------------------------
/**
FIXME: SSE OPTIMIZATION
*/
inline
void
_matrix33_sse::rotate(const _vector3_sse& vec, float a)
{
_vector3_sse v(vec);
v.norm();
float sa = (float) n_sin(a);
float ca = (float) n_cos(a);
_matrix33_sse rotM;
rotM.M11 = ca + (1.0f - ca) * v.x * v.x;
rotM.M12 = (1.0f - ca) * v.x * v.y - sa * v.z;
rotM.M13 = (1.0f - ca) * v.z * v.x + sa * v.y;
rotM.M21 = (1.0f - ca) * v.x * v.y + sa * v.z;
rotM.M22 = ca + (1.0f - ca) * v.y * v.y;
rotM.M23 = (1.0f - ca) * v.y * v.z - sa * v.x;
rotM.M31 = (1.0f - ca) * v.z * v.x - sa * v.y;
rotM.M32 = (1.0f - ca) * v.y * v.z + sa * v.x;
rotM.M33 = ca + (1.0f - ca) * v.z * v.z;
(*this) = (*this) * rotM;
}
//------------------------------------------------------------------------------
/**
*/
inline
_vector3_sse
_matrix33_sse::x_component() const
{
return _vector3_sse(m1);
}
//------------------------------------------------------------------------------
/**
*/
inline
_vector3_sse
_matrix33_sse::y_component(void) const
{
return _vector3_sse(m2);
}
//------------------------------------------------------------------------------
/**
*/
inline
_vector3_sse
_matrix33_sse::z_component(void) const
{
return _vector3_sse(m3);
};
//------------------------------------------------------------------------------
/**
*/
inline
void
_matrix33_sse::operator *= (const _matrix33_sse& mx)
{
m1 = _mm_add_ps(
_mm_add_ps(
_mm_mul_ps(_mm_shuffle_ps(m1, m1, _MM_SHUFFLE(0,0,0,0)), mx.m1),
_mm_mul_ps(_mm_shuffle_ps(m1, m1, _MM_SHUFFLE(1,1,1,1)), mx.m2)),
_mm_mul_ps(_mm_shuffle_ps(m1, m1, _MM_SHUFFLE(2,2,2,2)), mx.m3));
m2 = _mm_add_ps(
_mm_add_ps(
_mm_mul_ps(_mm_shuffle_ps(m2, m2, _MM_SHUFFLE(0,0,0,0)), mx.m1),
_mm_mul_ps(_mm_shuffle_ps(m2, m2, _MM_SHUFFLE(1,1,1,1)), mx.m2)),
_mm_mul_ps(_mm_shuffle_ps(m2, m2, _MM_SHUFFLE(2,2,2,2)), mx.m3));
m3 = _mm_add_ps(
_mm_add_ps(
_mm_mul_ps(_mm_shuffle_ps(m3, m3, _MM_SHUFFLE(0,0,0,0)), mx.m1),
_mm_mul_ps(_mm_shuffle_ps(m3, m3, _MM_SHUFFLE(1,1,1,1)), mx.m2)),
_mm_mul_ps(_mm_shuffle_ps(m3, m3, _MM_SHUFFLE(2,2,2,2)), mx.m3));
}
//------------------------------------------------------------------------------
/**
multiply source vector with matrix and store in destination vector
this eliminates the construction of a temp _vector3_sse object
*/
inline
void
_matrix33_sse::mult(const _vector3_sse& src, _vector3_sse& dst) const
{
dst.m128 = _mm_add_ps(
_mm_add_ps(_mm_mul_ps(_mm_shuffle_ps(src.m128, src.m128, _MM_SHUFFLE(0,0,0,0)), m1),
_mm_mul_ps(_mm_shuffle_ps(src.m128, src.m128, _MM_SHUFFLE(1,1,1,1)), m2)),
_mm_mul_ps(_mm_shuffle_ps(src.m128, src.m128, _MM_SHUFFLE(2,2,2,2)), m3));
}
//------------------------------------------------------------------------------
#endif
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -