/// <summary>
/// Calculates reciprocal approximation.
/// </summary>
public static int RcpFastest(int x)
{
if (x == MinValue || x == 0)
{
FixedUtil.InvalidArgument("Fixed32.RcpFastest", "x", x);
return(0);
}
// Handle negative values.
int sign = (x < 0) ? -1 : 1;
x *= sign;
// Normalize input into [1.0, 2.0( range (convert to s2.30).
int offset = 29 - Nlz((uint)x);
int n = FixedUtil.ShiftRight(x, offset - 28);
const int ONE = (1 << 30);
Debug.Assert(n >= ONE);
// Polynomial approximation.
int res = FixedUtil.RcpPoly4(n - ONE);
//int res = Util.RcpPoly3Lut4(n - ONE);
// Apply exponent, convert back to s16.16.
return(FixedUtil.ShiftRight(sign * res, offset));
}
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 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));
}
/// <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));
}
/// <summary>
/// Calculates division approximation.
/// </summary>
public static int Div(int a, int b)
{
if (b == MinValue || b == 0)
{
FixedUtil.InvalidArgument("Fixed32.Div", "b", b);
return(0);
}
return((int)(((long)a << 16) / b));
}
/// <summary>
/// Calculates x to the power of the exponent.
/// </summary>
public static int PowFast(int x, int exponent)
{
// Return 0 for invalid values
if (x <= 0)
{
if (x < 0)
{
FixedUtil.InvalidArgument("Fixed32.PowFast", "x", x);
}
return(0);
}
return(ExpFast(Mul(exponent, LogFast(x))));
}
public static int AcosFastest(int x)
{
// Return 0 for invalid values
if (x < -One || x > One)
{
FixedUtil.InvalidArgument("Fixed32.AcosFastest", "x", x);
return(0);
}
// Compute Atan2(Sqrt((1+x) * (1-x)), x), using s32.32.
long xx = (long)(One + x) * (long)(One - x);
long y = Fixed64.SqrtFastest(xx);
return((int)(Fixed64.Atan2Fastest(y, (long)x << 16) >> 16));
}
/// <summary>
/// Calculates the square root of the given number.
/// </summary>
public static int SqrtPrecise(int a)
{
// Adapted from https://github.com/chmike/fpsqrt
if (a <= 0)
{
if (a < 0)
{
FixedUtil.InvalidArgument("Fixed32.SqrtPrecise", "a", a);
}
return(0);
}
#if JAVA
int r = a;
int b = 0x40000000;
int q = 0;
while (b > 0x40)
{
int t = q + b;
if (Integer.compareUnsigned(r, t) >= 0)
{
r -= t;
q = t + b;
}
r <<= 1;
b >>= 1;
}
q >> >= 8;
return(q);
#else
uint r = (uint)a;
uint b = 0x40000000;
uint q = 0;
while (b > 0x40)
{
uint t = q + b;
if (r >= t)
{
r -= t;
q = t + b;
}
r <<= 1;
b >>= 1;
}
q >>= 8;
return((int)q);
#endif
}
public static int Atan2Fastest(int y, int x)
{
// See: https://www.dsprelated.com/showarticle/1052.php
if (x == 0)
{
if (y > 0)
{
return(PiHalf);
}
if (y < 0)
{
return(-PiHalf);
}
FixedUtil.InvalidArgument("Fixed32.Atan2Fastest", "y, x", y, x);
return(0);
}
int nx = Abs(x);
int ny = Abs(y);
int negMask = ((x ^ y) >> 31);
if (nx >= ny)
{
int k = Atan2DivFastest(ny, nx);
int z = FixedUtil.AtanPoly4(k);
int angle = negMask ^ (z >> 14);
if (x > 0)
{
return(angle);
}
if (y >= 0)
{
return(angle + Pi);
}
return(angle - Pi);
}
else
{
int k = Atan2DivFastest(nx, ny);
int z = FixedUtil.AtanPoly4(k);
int angle = negMask ^ (z >> 14);
return(((y > 0) ? PiHalf : -PiHalf) - angle);
}
}
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));
}
/// <summary>
/// Calculates the base 2 exponent.
/// </summary>
public static int Exp2Fastest(int x)
{
// Handle values that would under or overflow.
if (x >= 15 * One)
{
return(MaxValue);
}
if (x <= -16 * One)
{
return(0);
}
// Compute exp2 for fractional part.
int k = (x & FractionMask) << 14;
int y = FixedUtil.Exp2Poly3(k);
// Combine integer and fractional result, and convert back to s16.16.
int intPart = x >> Shift;
return(FixedUtil.ShiftRight(y, 14 - intPart));
}
public static int Log2Fastest(int x)
{
// Return 0 for invalid values
if (x <= 0)
{
FixedUtil.InvalidArgument("Fixed32.Log2Fastest", "x", x);
return(0);
}
// Normalize value to range [1.0, 2.0( as s2.30 and extract exponent.
int offset = 15 - Nlz((uint)x);
int n = FixedUtil.ShiftRight(x, offset - 14);
// Polynomial approximation of mantissa.
const int ONE = (1 << 30);
Debug.Assert(n >= ONE);
int y = FixedUtil.Log2Poly5(n - ONE);
// Combine integer and fractional parts (into s16.16).
return((offset << Shift) + (y >> 14));
}
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);
}
