public static Fix32 Ln(this Fix32 x)
{
#if USE_DOUBLES
return(Math.Log(x.ToDouble()).ToFix32());
#endif
return(Log2(x).Mul(Fix32.Ln2));
}
public static Fix32 Exp(this Fix32 x)
{
#if USE_DOUBLES
return(Math.Exp(x.ToDouble()).ToFix32());
#endif
return((Fix32)Fixed32.Exp((int)x));
}
/// <summary>
/// Returns a specified number raised to the specified power. Saturates
/// </summary>
/// <exception cref="DivideByZeroException">
/// The base was zero, with a negative exponent
/// </exception>
/// <exception cref="ArgumentOutOfRangeException">
/// The base was negative, with a non-zero exponent
/// </exception>
public static Fix32 Pow(this Fix32 b, Fix32 exp)
{
#if USE_DOUBLES
return(Math.Pow(b.ToDouble(), exp.ToDouble()).ToFix32());
#endif
if ((int)b == (int)Fix32.One)
{
return(Fix32.One);
}
if ((int)exp == 0)
{
return(Fix32.One);
}
if ((int)b == 0)
{
if ((int)exp < 0)
{
throw new DivideByZeroException();
}
return(Fix32.Zero);
}
if (b < 0 && exp != 0)
{
throw new ArgumentOutOfRangeException("Non-positive value passed to Ln", "x");
}
return(exp.Mul(b.Log2()).Pow2());
}
public static Fix32 TanFastest(this Fix32 x)
{
#if USE_DOUBLES
return(Math.Tan(x.ToDouble()).ToFix32());
#endif
return((Fix32)Fixed32.TanFastest((int)x));
}
public static Fix32 RcpFast(this Fix32 x)
{
#if USE_DOUBLES
return((1 / x.ToDouble()).ToFix32());
#endif
return((Fix32)Fixed32.RcpFast((int)x));
}
public static Fix32 RSqrtFastest(this Fix32 x)
{
#if USE_DOUBLES
return((1 / Math.Sqrt(x.ToDouble())).ToFix32());
#endif
return((Fix32)Fixed32.RSqrtFastest((int)x));
}
public static Fix32 Ceiling(this Fix32 x)
{
#if USE_DOUBLES
return(Math.Floor(x.ToDouble()).ToFix32());
#endif
var hasFractionalPart = ((int)x & FRACTIONAL_MASK) != 0;
return(hasFractionalPart ? ((Fix32)((int)x & INTEGER_MASK)).Add(Fix32.One) : x);
}
public static Fix32 Floor(this Fix32 x)
{
#if USE_DOUBLES
return(Math.Floor(x.ToDouble()).ToFix32());
#endif
// Just zero out the fractional part
return((Fix32)((int)x & INTEGER_MASK));
}
public static Fix32 Neg(this Fix32 x)
{
#if USE_DOUBLES
return((-x.ToDouble()).ToFix32());
#endif
//return new Fix64(-x.RawValue);
return((Fix32)((int)x == MIN_VALUE ? MAX_VALUE : -(int)x));
}
public static Fix32 Sub(this Fix32 x, Fix32 y)
{
#if USE_DOUBLES
return((x.ToDouble() - y.ToDouble()).ToFix32());
#endif
long sub = (long)x - (long)y; // TO TEST: Shift and operate to check overflow
return((Fix32)(((int)sub) != sub ? (int)((((uint)x >> NUM_BITS_MINUS_ONE) - 1U) ^ (1U << NUM_BITS_MINUS_ONE)) : (int)sub));
}
public static Fix32 Cos(this Fix32 x)
{
#if USE_DOUBLES
return(Math.Cos(x.ToDouble()).ToFix32());
#endif
// Don't use Fixed32.Cos, it gives an error
var rawAngle = (int)x + (x > 0 ? -PI - PI_OVER_2 : PI_OVER_2);
return(((Fix32)rawAngle).Sin());
}
public static Fix32 Mod(this Fix32 x, Fix32 y)
{
#if USE_DOUBLES
return((x.ToDouble() % y.ToDouble()).ToFix32());
#endif
return((Fix32)(
(int)x == MIN_VALUE & (int)y == -1 ?
0 :
(int)x % (int)y));
}
/// <summary>
/// Returns the base-2 logarithm of a specified number.
/// </summary>
/// <exception cref="ArgumentOutOfRangeException">
/// The argument was non-positive
/// </exception>
public static Fix32 Log2(this Fix32 x)
{
#if USE_DOUBLES
return(Math.Log(x.ToDouble(), 2).ToFix32());
#endif
if ((int)x <= 0)
{
throw new ArgumentOutOfRangeException("Non-positive value passed to Ln", "x");
}
//return (Fix32) Fixed32.Log2((int) x);
// This implementation is based on Clay. S. Turner's fast binary logarithm
// algorithm (C. S. Turner, "A Fast Binary Logarithm Algorithm", IEEE Signal
// Processing Mag., pp. 124,140, Sep. 2010.)
//https://github.com/dmoulding/log2fix/blob/master/log2fix.c
const int EXTRA_SHIFT = 8;
long xx = (long)x << EXTRA_SHIFT;
const int PRECISION = FRACTIONAL_BITS + EXTRA_SHIFT;
long b = 1 << (PRECISION - 1);
long y = 0;
long rawX = (long)xx;
while (rawX < 1 << PRECISION)
{
rawX <<= 1;
y -= 1 << PRECISION;
}
while (rawX >= 2 << PRECISION)
{
rawX >>= 1;
y += 1 << PRECISION;
}
ulong z = (ulong)rawX;
for (int i = 0; i < PRECISION; i++)
{
z = z * z >> PRECISION;
if (z >= 2 << PRECISION)
{
z >>= 1;
y += b;
}
b >>= 1;
}
return((Fix32)(int)(y >> EXTRA_SHIFT));
}
public static Fix32 Mul(this Fix32 x, Fix32 y)
{
#if USE_DOUBLES
return(Math.Round(x.ToDouble() * y.ToDouble()).ToFix32());
#endif
long multLong = ((long)x * (long)y) >> FRACTIONAL_BITS;
int finalSign = (int)x ^ (int)y;
return((Fix32)(((finalSign ^ multLong) & SIGN_MASK) != 0 && multLong != 0 ?
(int)((((uint)finalSign >> NUM_BITS_MINUS_ONE) - 1U) ^ (1U << NUM_BITS_MINUS_ONE)) :
(int)multLong));
}
public static Fix32 Atan2(this Fix32 y, Fix32 x)
{
#if USE_DOUBLES
return(Math.Atan2(y.ToDouble(), x.ToDouble()).ToFix32());
#endif
var yl = (int)y;
var xl = (int)x;
if (xl == 0)
{
if (yl > 0)
{
return(Fix32.PiOver2);
}
if (yl == 0)
{
return(Fix32.Zero);
}
return(Fix32.PiOver2.Neg());
}
Fix32 atan;
var z = y.Div(x);
// Deal with overflow
if ((int)Fix32.One.Add(C0p28.Mul(z).Mul(z)) == (int)Fix32.MaxValue)
{
return((int)y < 0 ? Fix32.PiOver2.Neg() : Fix32.PiOver2);
}
if ((int)Abs(z) < (int)Fix32.One)
{
atan = z.Div(Fix32.One.Add(C0p28.Mul(z).Mul(z)));
if (xl < 0)
{
if (yl < 0)
{
return(atan.Sub(Fix32.Pi));
}
return(atan.Add(Fix32.Pi));
}
}
else
{
atan = Fix32.PiOver2.Sub(z.Div(z.Mul(z).Add(C0p28)));
if (yl < 0)
{
return(atan.Sub(Fix32.Pi));
}
}
return(atan);
}
public static Fix32 Abs(this Fix32 x)
{
#if USE_DOUBLES
return(Math.Abs(x.ToDouble()).ToFix32());
#endif
if ((int)x == MIN_VALUE)
{
return(Fix32.MaxValue);
}
// branchless implementation, see http://www.strchr.com/optimized_abs_function
var mask = (int)x >> NUM_BITS_MINUS_ONE;
return((Fix32)(((int)x + mask) ^ mask));
}
public static Fix32 Add(this Fix32 x, Fix32 y)
{
#if USE_DOUBLES
return((x.ToDouble() + y.ToDouble()).ToFix32());
#endif
// https://stackoverflow.com/questions/17580118/signed-saturated-add-of-64-bit-ints/17587197#17587197
// determine the lower or upper bound of the result
//int ret = (x.RawValue < 0) ? MIN_VALUE : MAX_VALUE;
int ret = (int)((((uint)x >> NUM_BITS_MINUS_ONE) - 1U) ^ (1U << NUM_BITS_MINUS_ONE));
// this is always well defined:
// if x < 0 this adds a positive value to INT64_MIN
// if x > 0 this subtracts a positive value from INT64_MAX
//int comp = ret - xRaw;
// the condition is equivalent to
// ((x < 0) && (y > comp)) || ((x >=0) && (y <= comp))
return((Fix32)((x < 0) != ((int)y > (ret - (int)x)) ? ret : (int)x + (int)y));
}
/// <summary>
/// Returns the square root of a specified number.
/// </summary>
/// <exception cref="ArgumentOutOfRangeException">
/// The argument was negative.
/// </exception>
public static Fix32 SqrtSlow(this Fix32 x)
{
#if USE_DOUBLES
return(Math.Sqrt(x.ToDouble()).ToFix32());
#endif
if (x <= 0)
{
return(0);
}
// https://stackoverflow.com/questions/1100090/looking-for-an-efficient-integer-square-root-algorithm-for-arm-thumb2
// Manually calculated constants (somehow) for precision Q18.14
const int ITERATIONS = 25; // (((NUM_BITS - 16) / 4) * 2) + 16; // 24
int shift = NUM_BITS - 2;
// [1] First iteration
int bottomHalf = 0; // 0x3 & ((int) x >> SHIFT); equals 0
int a = 0; // accumulator
int r = 0; // remainder
for (int i = 0; i < ITERATIONS; i++)
{
r = (r << 2) | bottomHalf;
a <<= 1;
int e = (a << 1) | 1; // trial product
if (r >= e)
{
r -= e;
a |= 1;
}
// [1] Subsequent iterations
bottomHalf = 0x3 & ((int)x >> shift);
shift -= 2;
}
return((Fix32)a);
}
/// <summary>
/// Divide. Saturates on overflow. May change to Fast.
/// </summary>
public static Fix32 Div(this Fix32 x, Fix32 y)
{
#if USE_DOUBLES
return((x.ToDouble() / y.ToDouble()).ToFix32());
#endif
if ((int)y == 0)
{
return((Fix32)(unchecked ((int)(((((uint)x) >> NUM_BITS_MINUS_ONE) - 1U) ^ (1U << NUM_BITS_MINUS_ONE)))));
return(x >= 0 ? Fix32.MaxValue : Fix32.MinValue); // Branched version of the previous code, for clarity. Slower
}
long r = ((long)x << FRACTIONAL_BITS) / (int)y;
if (r > MAX_VALUE)
{
return(Fix32.MaxValue);
}
if (r < MIN_VALUE)
{
return(Fix32.MinValue);
}
return((Fix32)(int)r);
}
public static string ToStringExt(this Fix32 x)
{
return(x.ToDouble().ToString("0.##########"));
}
