/// <summary>
/// Calculates the reciprocal square root.
/// </summary>
public static int RSqrtFastest(int x)
{
// Return 0 for invalid values
if (x <= 0)
{
FixedUtil.InvalidArgument("Fixed32.RSqrtFastest", "x", x);
return(0);
}
// Constants (s2.30).
const int ONE = (1 << 30);
const int HALF_SQRT2 = 759250125; // 0.5 * sqrt(2.0)
// Normalize input into [1.0, 2.0( range (as s2.30).
int offset = 1 - Nlz((uint)x);
int n = FixedUtil.ShiftRight(x, offset);
Debug.Assert(n >= ONE);
int y = FixedUtil.RSqrtPoly3(n - ONE);
// Divide offset by 2 (to get sqrt), compute adjust value for odd exponents.
int adjust = ((offset & 1) != 0) ? HALF_SQRT2 : ONE;
offset = offset >> 1;
// Apply exponent, convert back to s16.16.
int yr = FixedUtil.Qmul30(adjust, y);
return(FixedUtil.ShiftRight(yr, offset + 21));
}
public static int SqrtFastest(int x)
{
// Return 0 for all non-positive values.
if (x <= 0)
{
if (x < 0)
{
FixedUtil.InvalidArgument("Fixed32.SqrtFastest", "x", x);
}
return(0);
}
// Constants (s2.30).
const int ONE = (1 << 30);
const int SQRT2 = 1518500249; // sqrt(2.0)
// Normalize input into [1.0, 2.0( range (as s2.30).
int offset = 15 - Nlz((uint)x);
int n = FixedUtil.ShiftRight(x, offset - 14);
Debug.Assert(n >= ONE);
int y = FixedUtil.SqrtPoly3(n - ONE);
// Divide offset by 2 (to get sqrt), compute adjust value for odd exponents.
int adjust = ((offset & 1) != 0) ? SQRT2 : ONE;
offset = offset >> 1;
// Apply exponent, convert back to s16.16.
int yr = FixedUtil.Qmul30(adjust, y);
return(FixedUtil.ShiftRight(yr, 14 - offset));
}
/// <summary>
/// Calculates division approximation.
/// </summary>
public static int DivFastest(int a, int b)
{
if (b == MinValue || b == 0)
{
FixedUtil.InvalidArgument("Fixed32.DivFastest", "b", b);
return(0);
}
// Handle negative values.
int sign = (b < 0) ? -1 : 1;
b *= sign;
// Normalize input into [1.0, 2.0( range (convert to s2.30).
int offset = 29 - Nlz((uint)b);
int n = FixedUtil.ShiftRight(b, offset - 28);
const int ONE = (1 << 30);
Debug.Assert(n >= ONE);
// Polynomial approximation.
int res = FixedUtil.RcpPoly4(n - ONE);
// Multiply by reciprocal, apply exponent, convert back to s16.16.
int y = FixedUtil.Qmul30(res, a);
return(FixedUtil.ShiftRight(sign * y, offset - 14));
}
private static int Atan2DivFastest(int y, int x)
{
Debug.Assert(y >= 0 && x > 0 && x >= y);
// Normalize input into [1.0, 2.0( range (convert to s2.30).
const int ONE = (1 << 30);
const int HALF = (1 << 29);
int offset = 1 - Nlz((uint)x);
int n = FixedUtil.ShiftRight(x, offset);
// Polynomial approximation.
int oox = FixedUtil.RcpPoly4(n - ONE);
Debug.Assert(oox >= HALF && oox <= ONE);
// Apply exponent and multiply.
int yr = FixedUtil.ShiftRight(y, offset);
return(FixedUtil.Qmul30(yr, oox));
}
private static int UnitSinFastest(int z)
{
// See: http://www.coranac.com/2009/07/sines/
// Handle quadrants 1 and 2 by mirroring the [1, 3] range to [-1, 1] (by calculating 2 - z).
// The if condition uses the fact that for the quadrants of interest are 0b01 and 0b10 (top two bits are different).
if ((z ^ (z << 1)) < 0)
{
z = (1 << 31) - z;
}
// Now z is in range [-1, 1].
const int ONE = (1 << 30);
Debug.Assert((z >= -ONE) && (z <= ONE));
// Polynomial approximation.
int zz = FixedUtil.Qmul30(z, z);
int res = FixedUtil.Qmul30(FixedUtil.SinPoly2(zz), z);
// Return as s2.30.
return(res);
}
