void AABBTransformAsAABB_SIMD(AABB &aabb, const float4x4 &m) { simd4f minPt = aabb.minPoint; simd4f maxPt = aabb.maxPoint; simd4f centerPoint = muls_ps(add_ps(minPt, maxPt), 0.5f); simd4f newCenter = mat4x4_mul_vec4(m.row, centerPoint); simd4f halfSize = sub_ps(centerPoint, minPt); simd4f x = abs_ps(mul_ps(m.row[0], halfSize)); simd4f y = abs_ps(mul_ps(m.row[1], halfSize)); simd4f z = abs_ps(mul_ps(m.row[2], halfSize)); simd4f w = zero_ps(); simd4f newDir = hadd4_ps(x, y, z, w); aabb.minPoint = sub_ps(newCenter, newDir); aabb.maxPoint = add_ps(newCenter, newDir); }
float3x4 float3x4::operator -() const { float3x4 r; #ifdef MATH_SIMD simd4f z = zero_ps(); r.row[0] = sub_ps(z, row[0]); r.row[1] = sub_ps(z, row[1]); r.row[2] = sub_ps(z, row[2]); #else for(int y = 0; y < Rows; ++y) for(int x = 0; x < Cols; ++x) r[y][x] = -v[y][x]; #endif return r; }
void Quat::ToAxisAngle(float4 &axis, float &angle) const { #if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE) // Best: 35.332 nsecs / 94.328 ticks, Avg: 35.870 nsecs, Worst: 57.607 nsecs assume2(this->IsNormalized(), *this, this->Length()); simd4f cosAngle = _mm_shuffle_ps(q, q, _MM_SHUFFLE(3, 3, 3, 3)); simd4f rcpSinAngle = rsqrt_ps(sub_ps(set1_ps(1.f), mul_ps(cosAngle, cosAngle))); angle = Acos(s4f_x(cosAngle)) * 2.f; simd4f a = mul_ps(q, rcpSinAngle); // Set the w component to zero. simd4f highPart = _mm_unpackhi_ps(a, zero_ps()); // [_ _ 0 z] axis.v = _mm_movelh_ps(a, highPart); // [0 z y x] #else // Best: 85.258 nsecs / 227.656 ticks, Avg: 85.492 nsecs, Worst: 86.410 nsecs ToAxisAngle(reinterpret_cast<float3&>(axis), angle); axis.w = 0.f; #endif }
float Quat::Normalize() { #ifdef MATH_AUTOMATIC_SSE simd4f lenSq = vec4_length_sq_ps(q); simd4f len = rsqrt_ps(lenSq); simd4f isZero = cmplt_ps(lenSq, simd4fEpsilon); // Was the length zero? simd4f normalized = mul_ps(q, len); // Normalize. q = cmov_ps(normalized, float4::unitX.v, isZero); // If length == 0, output the vector (1,0,0,0). len = cmov_ps(len, zero_ps(), isZero); // If length == 0, output zero as length. return s4f_x(len); #else float length = Length(); if (length < 1e-4f) return 0.f; float rcpLength = 1.f / length; x *= rcpLength; y *= rcpLength; z *= rcpLength; w *= rcpLength; return length; #endif }
vec Quat::Axis() const { assume2(this->IsNormalized(), *this, this->Length()); #if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE) // Best: 6.145 nsecs / 16.88 ticks, Avg: 6.367 nsecs, Worst: 6.529 nsecs assume2(this->IsNormalized(), *this, this->Length()); simd4f cosAngle = _mm_shuffle_ps(q, q, _MM_SHUFFLE(3, 3, 3, 3)); simd4f rcpSinAngle = rsqrt_ps(sub_ps(set1_ps(1.f), mul_ps(cosAngle, cosAngle))); simd4f a = mul_ps(q, rcpSinAngle); // Set the w component to zero. simd4f highPart = _mm_unpackhi_ps(a, zero_ps()); // [_ _ 0 z] a = _mm_movelh_ps(a, highPart); // [0 z y x] return FLOAT4_TO_DIR(a); #else // Best: 6.529 nsecs / 18.152 ticks, Avg: 6.851 nsecs, Worst: 8.065 nsecs // Convert cos to sin via the identity sin^2 + cos^2 = 1, and fuse reciprocal and square root to the same instruction, // since we are about to divide by it. float rcpSinAngle = RSqrt(1.f - w*w); return DIR_VEC(x, y, z) * rcpSinAngle; #endif }
Quat MUST_USE_RESULT Quat::Slerp(const Quat &q2, float t) const { assume(0.f <= t && t <= 1.f); assume(IsNormalized()); assume(q2.IsNormalized()); #if defined(MATH_AUTOMATIC_SSE) && defined(MATH_SSE) simd4f angle = dot4_ps(q, q2.q); // <q, q2.q> simd4f neg = cmplt_ps(angle, zero_ps()); // angle < 0? neg = and_ps(neg, set1_ps_hex(0x80000000)); // Convert 0/0xFFFFFFFF mask to a 0x/0x80000000 mask. // neg = s4i_to_s4f(_mm_slli_epi32(s4f_to_s4i(neg), 31)); // A SSE2-esque way to achieve the above would be this, but this seems to clock slower (12.04 clocks vs 11.97 clocks) angle = xor_ps(angle, neg); // if angle was negative, make it positive. simd4f one = set1_ps(1.f); angle = min_ps(angle, one); // If user passed t > 1 or t < -1, clamp the range. // Compute a fast polynomial approximation to arccos(angle). // arccos(x): (-0.69813170079773212f * x * x - 0.87266462599716477f) * x + 1.5707963267948966f; angle = madd_ps(msub_ps(mul_ps(set1_ps(-0.69813170079773212f), angle), angle, set1_ps(0.87266462599716477f)), angle, set1_ps(1.5707963267948966f)); // Shuffle an appropriate vector from 't' and 'angle' for computing two sines in one go. simd4f T = _mm_set_ss(t); // (.., t) simd4f oneSubT = sub_ps(one, T); // (.., 1-t) T = _mm_movelh_ps(T, oneSubT); // (.., 1-t, .., t) angle = mul_ps(angle, T); // (.., (1-t)*angle, .., t*angle) // Compute a fast polynomial approximation to sin(t*angle) and sin((1-t)*angle). // Here could use "angle = sin_ps(angle);" for precision, but favor speed instead with the following polynomial expansion: // sin(x): ((5.64311797634681035370e-03 * x * x - 1.55271410633428644799e-01) * x * x + 9.87862135574673806965e-01) * x simd4f angle2 = mul_ps(angle, angle); angle = mul_ps(angle, madd_ps(madd_ps(angle2, set1_ps(5.64311797634681035370e-03f), set1_ps(-1.55271410633428644799e-01f)), angle2, set1_ps(9.87862135574673806965e-01f))); // Compute the final lerp factors a and b to scale q and q2. simd4f a = zzzz_ps(angle); simd4f b = xxxx_ps(angle); a = xor_ps(a, neg); a = mul_ps(q, a); a = madd_ps(q2, b, a); // The lerp above generates an unnormalized quaternion which needs to be renormalized. return mul_ps(a, rsqrt_ps(dot4_ps(a, a))); #else 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.999) // 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. angle = (-0.69813170079773212f * angle * angle - 0.87266462599716477f) * angle + 1.5707963267948966f; float ta = t*angle; #ifdef MATH_USE_SINCOS_LOOKUPTABLE // If Sin() is based on a lookup table, prefer that over polynomial approximation. a = Sin(angle - ta); b = Sin(ta); #else // Not using a lookup table, manually compute the two sines by using a very rough approximation. float ta2 = ta*ta; b = ((5.64311797634681035370e-03f * ta2 - 1.55271410633428644799e-01f) * ta2 + 9.87862135574673806965e-01f) * ta; a = angle - ta; float a2 = a*a; a = ((5.64311797634681035370e-03f * a2 - 1.55271410633428644799e-01f) * a2 + 9.87862135574673806965e-01f) * a; #endif } else // If angle is close to taking the denominator to zero, resort to linear interpolation (and normalization). { a = 1.f - t; b = t; } // Lerp and renormalize. return (*this * (a * sign) + q2 * b).Normalized(); #endif }