public Direction(float x, float y, float z)
{
__m128 dir = _mm_set_ps(z, y, x, 0f);
__m128 tmp = Detail.rsqrt_nr1(Detail.hi_dp_bc(dir, dir));
P3 = _mm_mul_ps(dir, tmp);
}
internal static (__m128 p1, __m128 p2) Inverse(__m128 p1, __m128 p2)
{
// s, t computed as in the normalization
__m128 b2 = Detail.hi_dp_bc(p1, p1);
__m128 s = Detail.rsqrt_nr1(b2);
__m128 bc = Detail.hi_dp_bc(p1, p2);
__m128 b2_inv = Detail.rcp_nr1(b2);
__m128 t = _mm_mul_ps(_mm_mul_ps(bc, b2_inv), s);
__m128 neg = _mm_set_ps(-0f, -0f, -0f, 0f);
// p1 * (s + t e0123)^2 = (s * p1 - t P1perp) * (s + t e0123)
// = s^2 p1 - s t P1perp - s t P1perp
// = s^2 p1 - 2 s t P1perp
// p2 * (s + t e0123)^2 = s^2 p2
// NOTE: s^2 = b2_inv
__m128 st = _mm_mul_ps(s, t);
st = _mm_mul_ps(p1, st);
p2 = _mm_sub_ps(_mm_mul_ps(p2, b2_inv), _mm_add_ps(st, st));
p2 = _mm_xor_ps(p2, neg);
p1 = _mm_xor_ps(_mm_mul_ps(p1, b2_inv), neg);
return(p1, p2);
}
public static __m128 Inverse(__m128 p1)
{
__m128 inv_norm = Detail.rsqrt_nr1(Detail.hi_dp_bc(p1, p1));
p1 = _mm_mul_ps(p1, inv_norm);
p1 = _mm_mul_ps(p1, inv_norm);
return(_mm_xor_ps(_mm_set_ps(-0f, -0f, -0f, 0f), p1));
}
public Plane Inverse()
{
__m128 p0 = P0;
__m128 invNorm = Detail.rsqrt_nr1(Detail.hi_dp_bc(p0, p0));
p0 = _mm_mul_ps(invNorm, p0);
p0 = _mm_mul_ps(invNorm, p0);
return(new Plane(p0));
}
public Plane Normalized()
{
__m128 invNorm = Detail.rsqrt_nr1(Detail.hi_dp_bc(P0, P0));
invNorm = Sse41.IsSupported
? _mm_blend_ps(invNorm, _mm_set_ss(1f), 1)
: _mm_add_ps(invNorm, _mm_set_ss(1f));
return(new Plane(_mm_mul_ps(invNorm, P0)));
}
internal static __m128 Inverse(__m128 p)
{
__m128 inv = Detail.rsqrt_nr1(Detail.hi_dp_bc(p, p));
p = _mm_mul_ps(p, inv);
p = _mm_mul_ps(p, inv);
p = _mm_xor_ps(_mm_set_ps(-0f, -0f, -0f, 0f), p);
return(p);
}
internal static (__m128, __m128) Normalized(__m128 p1, __m128 p2)
{
// l = b + c where b is p1 and c is p2
// l * ~l = |b|^2 - 2(b1 c1 + b2 c2 + b3 c3)e0123
//
// sqrt(l*~l) = |b| - (b1 c1 + b2 c2 + b3 c3)/|b| e0123
//
// 1/sqrt(l*~l) = 1/|b| + (b1 c1 + b2 c2 + b3 c3)/|b|^3 e0123
// = s + t e0123
__m128 b2 = Detail.hi_dp_bc(p1, p1);
__m128 s = Detail.rsqrt_nr1(b2);
__m128 bc = Detail.hi_dp_bc(p1, p2);
__m128 t = _mm_mul_ps(_mm_mul_ps(bc, Detail.rcp_nr1(b2)), s);
// p1 * (s + t e0123) = s * p1 - t P1perp
__m128 tmp = _mm_mul_ps(p2, s);
p2 = _mm_sub_ps(tmp, _mm_mul_ps(p1, t));
p1 = _mm_mul_ps(p1, s);
return(p1, p2);
}
public Direction Normalized()
{
__m128 tmp = Detail.rsqrt_nr1(Detail.hi_dp_bc(P3, P3));
return(new Direction(_mm_mul_ps(P3, tmp)));
}
internal static __m128 Normalized(__m128 p)
{
__m128 inv = Detail.rsqrt_nr1(Detail.hi_dp_bc(p, p));
return(_mm_mul_ps(p, inv));
}
