public static fp Cosh(fp x)
{
// 因定点数精度有限，abs(x)在接近fp(91986135426)时误差为最大5.98967337608337
if (x.m_value >= LN2RawMul31 ||
x.m_value <= -LN2RawMul31)
{
return(fp.MaxValue);
}
var n = x.m_value > 0 ? x.m_value : -x.m_value;
var q = 0;
if (n > HBMax)
{
n = NormalizeHyperbolic(n, out q);
}
var ux = CORDIC_KH.m_value + 2;
var uy = 0L;
CORDICHyperbolic(ref n, ref ux, ref uy);
var nx = (ulong)(ux > 0 ? ux : -ux);
var ny = (ulong)(uy > 0 ? uy : -uy);
nx = (nx + ny << q) + (nx - ny >> q) >> 1;
return(new fp((long)nx));
}
public static fp Sin(fp x)
{
// 粗略测试最大误差约为1e-7
var n = NormalizeRadian(x.m_value);
var p = true;
if (n > PIRaw)
{
n -= PIRaw;
p = false;
}
else if (n < -PIRaw)
{
n += PIRaw;
p = false;
}
if (n > PIRawDiv2)
{
n = PIRaw - n;
}
else if (n < -PIRawDiv2)
{
n = -(PIRaw + n);
}
// -4 -2 Magic number，使得Sin(0) == 0
var ux = CORDIC_K.m_value - 4;
var uy = -2L;
CORDICCircular(ref n, ref ux, ref uy);
return(new fp(p ? uy : -uy));
}
public static fp Log(fp x)
{
// 粗略测试最大误差约为1e-8
// 返回值为[-ln2 * 31, ln2 * 31]
var nx = x.m_value;
if (nx <= 0)
{
return(0);
}
var e = 0;
while (nx < LNMin)
{
nx <<= 1;
e -= 1;
}
while (nx > LNMax)
{
nx >>= 1;
e += 1;
}
var n = 2L;
var ux = nx + IntOne;
var uy = nx - IntOne - 1;
InvertCORDICHyperbolic(ref n, ref ux, ref uy);
return(new fp((n << 1) + LN2Raw * e));
}
public static fp Pow(fp x)
{
if (x.m_value >= LN2RawMul31)
{
return(fp.MaxValue);
}
if (x.m_value <= -LN2RawMul31)
{
return(0);
}
var n = x.m_value;
var q = 0;
if (n > HBMax)
{
n = NormalizeHyperbolic(n, out q);
}
else if (n < -HBMax)
{
n = -NormalizeHyperbolic(-n, out q);
}
var ux = CORDIC_KH.m_value + 1;
var uy = CORDIC_KH.m_value;
CORDICHyperbolic(ref n, ref ux, ref uy);
return(new fp(n > 0 ? ux << q : ux >> q));
}
public static fp Sign(fp x)
{
if (x > 0)
{
return(1);
}
if (x < 0)
{
return(-1);
}
return(0);
}
public static fp Asin(fp x)
{
// 粗略测试最大误差约为1e-8
// 返回值范围[-PI/2, PI/2]
if (x >= 1)
{
return(new fp(PIRawDiv2));
}
if (x <= -1)
{
return(new fp(-PIRawDiv2));
}
return(Atan(x / Sqrt(1 - x * x)));
}
public static fp Atan2(fp y, fp x)
{
// 粗略测试最大误差约为1e-9
// 返回值范围[-PI, PI]
var ux = x.m_value;
var uy = y.m_value;
if (uy == 0)
{
return(0);
}
if (ux == 0)
{
return(new fp(uy > 0 ? PIRawDiv2 : -PIRawDiv2));
}
var sx = ux > 0;
var sy = uy > 0;
// 特殊处理-long.MinValue
var nx = sx ? ux : ux == fp.MinValue.m_value ? fp.MaxValue.m_value : -ux;
var ny = sy ? uy : uy == fp.MinValue.m_value ? fp.MaxValue.m_value : -uy;
var zx = CountLeadingZeroes((ulong)nx);
var zy = CountLeadingZeroes((ulong)ny);
var df = zy - zx;
if (df >= FracBits)
{
return(sx ? (fp)0 : sy?PI : -PI);
}
if (df <= -FracBits)
{
return(new fp(sy ? PIRawDiv2 : -PIRawDiv2));
}
var shift = (zx + zy) / 2 - FracBits;
if (shift >= 0)
{
nx <<= shift;
uy <<= shift;
}
else
{
nx >>= -shift;
uy >>= -shift;
}
var n = 0L;
InvertCORDICCircular(ref n, ref nx, ref uy);
return(new fp(sx ? n : sy?PIRaw - n: -(PIRaw + n)));
}
public static fp Round(fp x)
{
var high = Floor(x);
var low = x.m_value & FracMask;
if (low < FracHalf)
{
return(high);
}
if (low > FracHalf)
{
return(high + 1);
}
return(high + ((high.m_value & IntOne) == 0 ? 0 : 1));
}
public static fp Atan(fp x)
{
// 粗略测试最大误差约为1e-9
// 返回值范围[-PI/2, PI/2]
if (x.m_value >= MaxAtan)
{
return(new fp(PIRawDiv2));
}
if (x.m_value <= -MaxAtan)
{
return(new fp(-PIRawDiv2));
}
var n = 0L;
var ux = IntOne;
var uy = x.m_value;
InvertCORDICCircular(ref n, ref ux, ref uy);
return(new fp(n));
}
public static fp Tan(fp x)
{
// 当x接近PI/2或-PI/2时，定点数的精度问题会导致误差被无限放大。
// 因为在PI/2和-PI/2时正切值不是连续的，在两侧分别趋近于正无穷和负无穷
var n = NormalizeRadian(x.m_value);
if (n > PIRaw)
{
n -= PIRaw;
}
else if (n < -PIRaw)
{
n += PIRaw;
}
var p = true;
if (n > PIRawDiv2)
{
n = PIRaw - n;
p = false;
}
else if (n < -PIRawDiv2)
{
n = -(PIRaw + n);
p = false;
}
var ux = CORDIC_K.m_value - 4;
var uy = -2L;
CORDICCircular(ref n, ref ux, ref uy);
if (uy == 0)
{
return(0);
}
uy = p ? uy : -uy;
if (ux == 0)
{
return(new fp(uy > 0 ? fp.MaxValue : fp.MinValue));
}
return(new fp(uy) / new fp(ux));
}
public static fp Tanh(fp x)
{
// 粗略测试最大误差约为1e-9
// 返回值为[-1, 1]
if (x.m_value >= LN2RawMul31)
{
return(1);
}
if (x.m_value <= -LN2RawMul31)
{
return(-1);
}
var sn = x.m_value > 0;
var n = sn ? x.m_value : -x.m_value;
var q = 0;
if (n > HBMax)
{
n = NormalizeHyperbolic(n, out q);
}
var ux = CORDIC_KH.m_value + 4;
var uy = 0L;
CORDICHyperbolic(ref n, ref ux, ref uy);
if (ux == 0)
{
return(0);
}
var nx = (ulong)(ux > 0 ? ux : -ux);
var ny = (ulong)(uy > 0 ? uy : -uy);
var tx = (nx + ny << q) + (nx - ny >> q) >> 1;
var ty = (nx + ny << q) - (nx - ny >> q) >> 1;
ux = (long)tx;
uy = (long)ty;
return(new fp(sn ? uy : -uy) / new fp(ux));
}
private static void BuildCORDIC(out fp[] atan, out fp k, out fp[] atanh, out fp kh)
{
atan = new fp[FracBits + 1];
atanh = new fp[FracBits + 1];
for (var i = 0; i < atan.Length; i++)
{
var t = System.Math.Pow(2, -i);
atan[i] = System.Math.Atan(t);
atanh[i] = 0.5 * System.Math.Log((1 + t * 0.5) / (1 - t * 0.5));
}
var x = IntOne;
var y = 0L;
var z = 0L;
CORDICCircular(ref z, ref x, ref y);
k = 1 / new fp(x);
x = IntOne;
y = 0L;
z = 0L;
CORDICHyperbolic(ref z, ref x, ref y);
kh = 1 / new fp(x);
}
public static fp Sqrt(fp x)
{
// http://jet.ro/files/The_neglected_art_of_Fixed_Point_arithmetic_20060913.pdf
// page 21
var root = 0UL;
var remHi = 0UL;
var remLo = (ulong)x.m_value;
var count = SqrtIterateCnt;
while (count-- != 0)
{
remHi = (remHi << SqrtShiftLeft) | (remLo >> SqrtShiftRight);
remLo <<= SqrtShiftLeft;
root <<= 1;
var testDiv = (root << 1) + 1;
if (remHi >= testDiv)
{
remHi -= testDiv;
root += 1;
}
}
return(new fp((long)root));
}
public static fp Clamp(fp x, fp min, fp max)
{
x = x < min ? min : x;
x = x > max ? max : x;
return(x);
}
public static fp Min(fp x, fp y)
{
return(x < y ? x : y);
}
public static fp Max(fp x, fp y)
{
return(x > y ? x : y);
}
public static fp Cos(fp x)
{
// 粗略测试最大误差约为1e-7
return(Sin(new fp(x.m_value + (x.m_value > 0 ? -PIRaw - PIRawDiv2 : PIRawDiv2))));
}
public static fp Ceiling(fp x)
{
return(Floor(x) + ((x.m_value & FracMask) == 0 ? 0 : 1));
}
public static fp Floor(fp x)
{
return(new fp((long)((ulong)x.m_value & IntMask)));
}
public static fp Abs(fp x)
{
var t = x.m_value >> TotalBits - 1;
return(new fp((x.m_value ^ t) - t));
}
public static fp Acos(fp x)
{
// 粗略测试最大误差约为1e-8
// 返回值范围[0, PI]
return(new fp(PIRawDiv2 - Asin(x).m_value));
}
