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);
}
/// <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());
}
/// <summary>
/// Returns 2 raised to the specified power.
/// </summary>
public static Fix32 Pow2(this Fix32 xx)
{
#if USE_DOUBLES
return(Math.Pow(2, x.ToDouble()).ToFix32());
#endif
Fix32 x = xx;
if ((int)x == 0)
{
return(Fix32.One);
}
// Avoid negative arguments by exploiting that exp(-x) = 1/exp(x).
bool neg = (int)x < 0;
if (neg)
{
x = x.Neg();
}
if ((int)x == (int)Fix32.One)
{
return(neg ? Fix32.One.Div(Fix32.Two) : Fix32.Two); // Can be cached
}
if ((int)x >= (int)Fix32.Log2Max)
{
return(neg ? Fix32.One.Div(Fix32.MaxValue) : Fix32.MaxValue); // Can be cached
}
if ((int)x <= (int)Fix32.Log2Min)
{
return(neg ? Fix32.MaxValue : Fix32.Zero);
}
/*
* The algorithm is based on the power series for exp(x):
* http://en.wikipedia.org/wiki/Exponential_function#Formal_definition
*
* From term n, we get term n+1 by multiplying with x/n.
* When the sum term drops to zero, we can stop summing.
*/
int integerPart = x.Floor().ToInt();
// Take fractional part of exponent
x = (Fix32)((uint)x & FRACTIONAL_MASK);
Fix32 result = Fix32.One;
Fix32 term = Fix32.One;
Fix32 i = Fix32.One;
while ((int)term != 0)
{
term = x.Mul(term).Mul(Fix32.Ln2.Div(i));
result = result.Add(term);
i = i.AddFast(Fix32.One);
}
result = (Fix32)((int)result << integerPart);
if (neg)
{
result = Fix32.One.Div(result);
}
return(result);
}
/// <summary>
/// Returns the arccos of of the specified number, calculated using Atan and Sqrt
/// </summary>
public static Fix32 Acos(this Fix32 x)
{
#if USE_DOUBLES
return((Fix64)Math.Acos((double)x));
#endif
if ((int)x < -(int)Fix32.One || (int)x > (int)Fix32.One)
{
throw new ArgumentOutOfRangeException(nameof(x));
}
if ((int)x == 0)
{
return(Fix32.PiOver2);
}
var result = (Fix32.One.Sub(x.Mul(x))).Sqrt().Div(x).Atan();
return((int)x < 0 ? result.Add(Fix32.Pi) : result);
}
/// <summary>
/// Returns the arctan of of the specified number, calculated using Euler series
/// </summary>
public static Fix32 Atan(this Fix32 zz)
{
#if USE_DOUBLES
return(Math.Atan(z.ToDouble()).ToFix32());
#endif
Fix32 z = zz;
if ((int)z == 0)
{
return(Fix32.Zero);
}
// Force positive values for argument
// Atan(-z) = -Atan(z).
bool neg = ((int)z < 0);
if (neg)
{
z = z.Neg();
}
Fix32 result;
if ((int)z == (int)Fix32.One)
{
result = Fix32.PiOver4;
}
else
{
bool invert = (int)z > (int)Fix32.One;
if (invert)
{
z = Fix32.One.Div(z);
}
result = Fix32.One;
Fix32 term = Fix32.One;
Fix32 zSq = z.Mul(z);
Fix32 zSq2 = zSq.Mul(Fix32.Two);
Fix32 zSqPlusOne = zSq.Add(Fix32.One);
Fix32 zSq12 = zSqPlusOne.Mul(Fix32.Two);
Fix32 dividend = zSq2;
Fix32 divisor = zSqPlusOne.Mul(Fix32.Three);
for (int i = 2; i < 30; i++)
{
term = term.Mul(dividend.Div(divisor));
result = result.Add(term);
dividend = dividend.Add(zSq2);
divisor = divisor.Add(zSq12);
if ((int)term == 0)
{
break;
}
}
result = result.Mul(z).Div(zSqPlusOne);
if (invert)
{
result = Fix32.PiOver2.Sub(result);
}
}
if (neg)
{
result = result.Neg();
}
return(result);
}
