void float3x4::Set(float _00, float _01, float _02, float _03,
float _10, float _11, float _12, float _13,
float _20, float _21, float _22, float _23)
{
#ifdef MATH_SIMD
row[0] = set_ps(_03, _02, _01, _00);
row[1] = set_ps(_13, _12, _11, _10);
row[2] = set_ps(_23, _22, _21, _20);
#else
v[0][0] = _00; v[0][1] = _01; v[0][2] = _02; v[0][3] = _03;
v[1][0] = _10; v[1][1] = _11; v[1][2] = _12; v[1][3] = _13;
v[2][0] = _20; v[2][1] = _21; v[2][2] = _22; v[2][3] = _23;
#endif
}
float3 MUST_USE_RESULT Quat::Transform(float x, float y, float z) const
{
#if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
return float4(quat_transform_vec4(q, set_ps(0.f, z, y, x))).xyz();
#else
return Transform(float3(x, y, z));
#endif
}
float3 float3x4::TransformDir(const float3 &directionVector) const
{
#ifdef MATH_SSE
return mat3x4_mul_vec(row, set_ps(0.f, directionVector.z, directionVector.y, directionVector.x));
#else
return TransformDir(directionVector.x, directionVector.y, directionVector.z);
#endif
}
float3 float3x4::TransformPos(const float3 &pointVector) const
{
#ifdef MATH_SSE
return mat3x4_mul_vec(row, set_ps(1.f, pointVector.z, pointVector.y, pointVector.x));
#else
return TransformPos(pointVector.x, pointVector.y, pointVector.z);
#endif
}
float3 MUST_USE_RESULT Quat::Transform(float X, float Y, float Z) const
{
#if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SIMD)
return float4(quat_transform_vec4(q, set_ps(0.f, Z, Y, X))).xyz();
#else
return Transform(float3(X, Y, Z));
#endif
}
Quat::Quat(float x_, float y_, float z_, float w_)
#if !defined(MATH_AUTOMATIC_SSE)
:x(x_), y(y_), z(z_), w(w_)
#endif
{
#if defined(MATH_AUTOMATIC_SSE)
q = set_ps(w_, z_, y_, x_);
#endif
}
float3 float3x4::TransformDir(float x, float y, float z) const
{
#ifdef MATH_SSE
return mat3x4_mul_vec(row, set_ps(0, z, y, x));
#else
return float3(DOT3_xyz(v[0], x,y,z),
DOT3_xyz(v[1], x,y,z),
DOT3_xyz(v[2], x,y,z));
#endif
}
void Quat::Set(float x_, float y_, float z_, float w_)
{
#ifdef MATH_AUTOMATIC_SSE
q = set_ps(w_, z_, y_, x_);
#else
x = x_;
y = y_;
z = z_;
w = w_;
#endif
}
/** Implementation based on the math in the book Watt, Policarpo. 3D Games: Real-time rendering and Software Technology, pp. 383-386. */
Quat MUST_USE_RESULT Quat::Slerp(const Quat &q2, float t) const
{
///\todo SSE.
assume(0.f <= t && t <= 1.f);
assume(IsNormalized());
assume(q2.IsNormalized());
float angle = this->Dot(q2);
float sign = 1.f; // Multiply by a sign of +/-1 to guarantee we rotate the shorter arc.
if (angle < 0.f)
{
angle = -angle;
sign = -1.f;
}
float a;
float b;
if (angle <= 0.97f) // perform spherical linear interpolation.
{
angle = Acos(angle); // After this, angle is in the range pi/2 -> 0 as the original angle variable ranged from 0 -> 1.
float angleT = t*angle;
#if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE)
// Compute three sines in one go with SSE.
simd4f s = set_ps(0.f, angleT, angle - angleT, angle);
s = sin_ps(s);
simd4f denom = shuffle1_ps(s, _MM_SHUFFLE(0, 0, 0, 0));
s = div_ps(s, denom);
a = s4f_y(s);
b = s4f_z(s);
#else
float s[3] = { Sin(angle), Sin(angle - angleT), Sin(angleT) };
float c = 1.f / s[0];
a = s[1] * c;
b = s[2] * c;
#endif
}
else // If angle is close to taking the denominator to zero, resort to linear interpolation (and normalization).
{
a = 1.f - t;
b = t;
}
return (*this * (a * sign) + q2 * b).Normalized();
}
void float3x4::SetRow(int rowIndex, float m_r0, float m_r1, float m_r2, float m_r3)
{
assume(rowIndex >= 0);
assume(rowIndex < Rows);
assume(MATH_NS::IsFinite(m_r0));
assume(MATH_NS::IsFinite(m_r1));
assume(MATH_NS::IsFinite(m_r2));
assume(MATH_NS::IsFinite(m_r3));
#ifndef MATH_ENABLE_INSECURE_OPTIMIZATIONS
if (rowIndex < 0 || rowIndex >= Rows)
return; // Benign failure
#endif
#ifdef MATH_SIMD
this->row[rowIndex] = set_ps(m_r3, m_r2, m_r1, m_r0);
#else
v[rowIndex][0] = m_r0;
v[rowIndex][1] = m_r1;
v[rowIndex][2] = m_r2;
v[rowIndex][3] = m_r3;
#endif
}