public static FixedPoint Atan2(FixedPoint y, FixedPoint x)
{
if (y.Equals(FixedPoint.Zero) && x.Equals(FixedPoint.Zero))
{
return(FixedPoint.Zero);
}
var coefficient1 = FixedPoint.Pi / FixedPoint.Four;
var coefficient2 = FixedPoint.Three * coefficient1;
var absY = Abs(y) + FixedPoint.Kludge; // + 1e-10 // kludge to prevent 0/0 condition
FixedPoint angle;
if (x >= FixedPoint.Zero)
{
var r = (x - absY) / (x + absY);
angle = coefficient1 - coefficient1 * r;
}
else
{
var r = (x + absY) / (absY - x);
angle = coefficient2 - coefficient1 * r;
}
if (y < FixedPoint.Zero)
{
return(angle * FixedPoint.NegativeOne); // negate if in quad III or IV
}
return(angle);
}
public static FixedPoint Sign(FixedPoint fp)
{
if (fp.Equals(FixedPoint.Zero))
{
return(FixedPoint.Zero);
}
return(fp > FixedPoint.Zero ? FixedPoint.One : FixedPoint.NegativeOne);
}