DESKTOP-5RP3AKU\Jisol 0d600a2786 提交
2024-10-17 03:20:22 +08:00

827 lines
23 KiB
C#

/*
* @FileName: LMath.cs
* @Date: 2024-04-20 20:06:15
* @LastEditTime: 2024-04-23 00:27:23
* @Description: 定点数数学工具函数
*/
namespace JNGame.Math
{
/// <summary>
/// 定点数数学工具库
/// </summary>
public static partial class LMath
{
public static LFloat Floor(LFloat value)
{
return FloorToInt(value);
}
public static LFloat Ceiling(LFloat value)
{
return CeilToInt(value);
}
#region
/// <summary>
/// 定点数反余弦函数Acos
/// </summary>
/// <param name="val">余弦值</param>
/// <returns>弧度</returns>
public static LFloat Acos(LFloat val)
{
int idx = (int)(val.rawValue * LUTAcos.HALF_COUNT / LFloat.Precision) +
LUTAcos.HALF_COUNT;
idx = Clamp(idx, 0, LUTAcos.COUNT);
return new LFloat(true, LUTAcos.table[idx]);
}
/// <summary>
/// 定点数反正弦函数Asin
/// </summary>
/// <param name="val">正弦值</param>
/// <returns>弧度</returns>
public static LFloat Asin(LFloat val)
{
int idx = (int)(val.rawValue * LUTAsin.HALF_COUNT / LFloat.Precision) + LUTAsin.HALF_COUNT;
idx = Clamp(idx, 0, LUTAsin.COUNT);
return new LFloat(true, LUTAsin.table[idx]);
}
/// <summary>
/// 定点数正弦函数Sin
/// </summary>
/// <param name="radians">弧度</param>
/// <returns>正弦值</returns>
public static LFloat Sin(LFloat radians)
{
return new LFloat(true, LUTSin.table[InternalGetIdx(radians)]);
}
/// <summary>
/// 定点数版本余弦函数
/// </summary>
/// <param name="radians">弧度</param>
/// <returns>余弦值</returns>
public static LFloat Cos(LFloat radians)
{
return new LFloat(true, LUTCos.table[InternalGetIdx(radians)]);
}
/// <summary>
/// 定点数版本正弦余弦函数,同时获取正弦值和余弦值
/// </summary>
/// <param name="sinValue">输出:正弦值</param>
/// <param name="cosValue">输出:余弦值</param>
/// <param name="radians">弧度</param>
public static void SinCos(out LFloat sinValue, out LFloat cosValue, LFloat radians)
{
int idx = InternalGetIdx(radians);
sinValue = new LFloat(true, LUTSin.table[idx]);
cosValue = new LFloat(true, LUTCos.table[idx]);
}
/// <summary>
/// 通过弧度获取查表索引
/// </summary>
/// <param name="radians">弧度</param>
/// <returns></returns>
private static int InternalGetIdx(LFloat radians)
{
var rawVal = radians.rawValue % LMath.kRawL2PI;
if (rawVal < 0) rawVal += LMath.kRawL2PI;
var val = new LFloat(true, rawVal) / LMath.PI2;
var idx = (int)(val * LUTCos.COUNT);
idx = Clamp(idx, 0, LUTCos.COUNT);
return idx;
}
#region Atan2 & Atan
/// <summary>
/// 定点数版本反正切函数Atan2
/// <para>值域[-PI, PI],返回点(x,y)与X轴正向的夹角</para>
/// <para>一二象限为正,三四象限为负</para>
/// </summary>
/// <param name="y"></param>
/// <param name="x"></param>
/// <returns>定点数,单位弧度</returns>
public static LFloat Atan2(LFloat y, LFloat x)
{
return Atan2(y.rawValue, x.rawValue);
}
/// <summary>
/// long版本反正切函数Atan2
/// <para>值域[-PI, PI],返回点(x,y)与X轴正向的夹角</para>
/// <para>一二象限为正,三四象限为负</para>
/// </summary>
/// <param name="y"></param>
/// <param name="x"></param>
/// <returns>定点数,单位弧度</returns>
public static LFloat Atan2(long y, long x)
{
return new LFloat(true, InternalAtan2(y, x));
}
/// <summary>
/// int版本反正切函数Atan2
/// <para>值域[-PI, PI],返回点(x,y)与X轴正向的夹角</para>
/// <para>一二象限为正,三四象限为负</para>
/// </summary>
/// <param name="y"></param>
/// <param name="x"></param>
/// <returns>定点数,单位弧度</returns>
public static LFloat Atan2(int y, int x)
{
return new LFloat(true, InternalAtan2(y, x));
}
/// <summary>
/// 定点数版本反正切函数Atan
/// <para>值域[-PI/2, PI/2],返回点(x,y)与X轴正向的夹角</para>
/// </summary>
/// <param name="val"></param>
/// <returns></returns>
public static LFloat Atan(LFloat val)
{
return new LFloat(true, InternalLutATan(val));
}
private struct LutAtan2Helper
{
public long sign;
public long offset;
public LutAtan2Helper(long sign, long offset)
{
this.sign = sign;
this.offset = offset;
}
}
private readonly static LutAtan2Helper[] s_Idx2LutInfo = new LutAtan2Helper[]
{
new LutAtan2Helper(-1, LMath.kRawLQuadPI),
new LutAtan2Helper(1, LMath.kRawLQuadPI),
new LutAtan2Helper(1, -LMath.kRawLQuadPI),
new LutAtan2Helper(-1, -LMath.kRawLQuadPI),
new LutAtan2Helper(1, LMath.kRawLQuadPI * 3),
new LutAtan2Helper(-1, LMath.kRawLQuadPI * 3),
new LutAtan2Helper(-1, -LMath.kRawLQuadPI * 3),
new LutAtan2Helper(1, -LMath.kRawLQuadPI * 3),
};
/// <summary>
/// Atan2的内部实现
/// <para>值域[-PI, PI],返回点(x,y)与X轴正向的夹角</para>
/// <para>一二象限为正,三四象限为负</para>
/// </summary>
/// <param name="y"></param>
/// <param name="x"></param>
/// <returns></returns>
private static long InternalAtan2(long y, long x)
{
//特殊情况处理
if (y == 0)
{
if (x == 0)
{
return 0;
}
return x < 0 ? LMath.kRawLPI : 0;
}
if (x == 0)
{
return y > 0 ? LMath.kRawLHalfPI : -LMath.kRawLHalfPI;
}
//决定象限
int idxV = 0;
if (x < 0)
{
x = -x;
idxV += 4;
}
if (y < 0)
{
y = -y;
idxV += 2;
}
LFloat factor = 0;
if (y > x)
{
idxV += 1;
factor = new LFloat(y) / x;
}
else
{
factor = new LFloat(x) / y;
}
//逆时针 idx 为 0 1 5 4 6 7 3 2
var info = s_Idx2LutInfo[idxV];
if (x == y)
{
return info.offset;
}
var deg = InternalLutATan(factor) - LMath.kRawLQuadPI;
return info.sign * deg + info.offset;
}
/// <summary>
/// ATan内部实现(查表)
/// </summary>
/// <param name="ydx">y/x的值</param>
/// <returns></returns>
private static long InternalLutATan(LFloat ydx)
{
//Debug.Assert(ydx >= 1);
if (ydx >= LUTAtan2.MaxQueryIdx) { return LMath.kRawLHalfPI; }
var iydx = (int)ydx;
var startIdx = LUTAtan2._startIdx[iydx - 1];
var size = LUTAtan2._arySize[iydx - 1];
var remaind = ydx - iydx;
var idx = startIdx + (int)(remaind * size);
return LUTAtan2._tblTbl[idx];
}
#endregion
#endregion
#region
/// <summary>
/// 开方-int版本
/// </summary>
/// <param name="value"></param>
/// <returns></returns>
public static int Sqrt(int value)
{
if (value <= 0)
{
return 0;
}
return (int)LMath.InternalSqrt32((uint)value);
}
/// <summary>
/// 开方 - long版本
/// </summary>
/// <param name="value"></param>
/// <returns></returns>
public static long Sqrt(long value)
{
if (value <= 0L)
{
return 0;
}
if (value <= (long)(0xffffffffu))
{
return (long)LMath.InternalSqrt32((uint)value);
}
return (long)LMath.InternalSqrt64((ulong)value);
}
/// <summary>
/// 开方 - ulong版本
/// </summary>
/// <param name="value"></param>
/// <returns></returns>
public static long Sqrt(ulong value)
{
return (long)LMath.InternalSqrt64(value);
}
/// <summary>
/// 开方 - LFloat版本
/// </summary>
/// <param name="value"></param>
/// <returns></returns>
public static LFloat Sqrt(LFloat value)
{
if (value.rawValue <= 0)
{
return LFloat.Zero;
}
return new LFloat(true, Sqrt(value.rawValue) * LFloat.RateOfOldPrecision);
}
/// <summary>
/// 32位开平方计算内部实现
/// </summary>
/// <param name="val"></param>
/// <returns></returns>
private static uint InternalSqrt32(uint val)
{
ulong rem = 0;
ulong root = 0;
ulong divisor = 0;
for (int i = 0; i < 16; i++)
{
root <<= 1;
rem = ((rem << 2) + (val >> 30));
val <<= 2;
divisor = (root << 1) + 1;
if (divisor <= rem)
{
rem -= divisor;
root++;
}
}
return (uint)root;
}
/// <summary>
/// 64位开平方计算内部实现
/// x = 2*p + q
/// x^2 = 4*p^2 + 4pq + q^2
/// q = (x^2 - 4*p^2)/(4*p+q)
/// https://www.cnblogs.com/10cm/p/3922398.html
/// </summary>
/// <param name="value"></param>
/// <returns></returns>
private static uint InternalSqrt64(ulong value)
{
ulong rem = 0;
ulong root = 0;
ulong divisor = 0;
for (int i = 0; i < 32; i++)
{
root <<= 1;
rem = ((rem << 2) + (value >> 62)); //(x^2 - 4*p^2)
value <<= 2;
divisor = (root << 1) + 1; //(4*p+q)
if (divisor <= rem)
{
rem -= divisor;
root++;
}
}
return (uint)root;
}
/// <summary>
/// 定点数版本 - 平方计算
/// </summary>
/// <param name="value"></param>
/// <returns></returns>
public static LFloat Sqr(LFloat value)
{
return value * value;
}
/// <summary>
/// 获取参数value向上取整到2的整数倍
/// </summary>
/// <param name="value"></param>
/// <returns></returns>
public static int CeilPowerOfTwo(int value)
{
value--;
value |= value >> 1;
value |= value >> 2;
value |= value >> 4;
value |= value >> 8;
value |= value >> 16;
value++;
return value;
}
/// <summary>
/// 是否是2的指数倍
/// </summary>
/// <param name="value"></param>
/// <returns></returns>
public static bool IsPowerOfTwo(int value)
{
return (value & (value - 1)) == 0;
}
#endregion
#region
/// <summary>
/// 判断两个定点数是否同符号
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
public static bool SameSign(LFloat a, LFloat b)
{
return a.rawValue * b.rawValue > 0L;
}
public static int Abs(int val)
{
if (val < 0)
{
return -val;
}
return val;
}
public static long Abs(long val)
{
if (val < 0L)
{
return -val;
}
return val;
}
public static LFloat Abs(LFloat val)
{
if (val.rawValue < 0)
{
return new LFloat(true, -val.rawValue);
}
return val;
}
/// <summary>
/// 定点数向下取整
/// </summary>
/// <param name="value"></param>
/// <returns></returns>
public static int FloorToInt(LFloat value)
{
var x = value.rawValue;
if (x > 0)
{
x /= LFloat.Precision;
}
else
{
if (x % LFloat.Precision == 0)
{
x /= LFloat.Precision;
}
else
{
x = x / LFloat.Precision - 1;
}
}
return (int)x;
}
/// <summary>
/// 定点数向上取整
/// </summary>
/// <returns></returns>
public static int CeilToInt(LFloat value)
{
var x = value.rawValue;
if (x < 0)
{
x /= LFloat.Precision;
}
else
{
if (x % LFloat.Precision == 0)
{
x /= LFloat.Precision;
}
else
{
x = x / LFloat.Precision + 1;
}
}
return (int)x;
}
/// <summary>
/// 定点数四舍五入取整
/// </summary>
/// <param name="val"></param>
/// <returns></returns>
public static int RoundToInt(LFloat value)
{
var val = value.rawValue;
if (val >= 0L)
{
var rd = val % LFloat.Precision;
var retVal = (int)(val / LFloat.Precision);
if (rd > LFloat.HalfPrecision)
{
return retVal + 1;
}
return retVal;
}
else
{
val = -val;
var rd = val % LFloat.Precision;
var retVal = -(int)(val / LFloat.Precision);
if (rd > LFloat.HalfPrecision)
{
return (retVal - 1);
}
return retVal;
}
}
/// <summary>
/// 定点数四舍五入取整
/// </summary>
/// <param name="val"></param>
/// <returns></returns>
public static LFloat Round(LFloat val)
{
if (val <= 0)
{
var remainder = (-val.rawValue) % LFloat.Precision;
if (remainder > LFloat.HalfPrecision)
{
return new LFloat(true, val.rawValue + remainder - LFloat.Precision);
}
else
{
return new LFloat(true, val.rawValue + remainder);
}
}
else
{
var remainder = (val.rawValue) % LFloat.Precision;
if (remainder > LFloat.HalfPrecision)
{
return new LFloat(true, val.rawValue - remainder + LFloat.Precision);
}
else
{
return new LFloat(true, val.rawValue - remainder);
}
}
}
#endregion
#region MinMaxClamp相关
public static int Clamp(int value, int min, int max)
{
if (value < min)
value = min;
else if (value > max)
value = max;
return value;
}
public static long Clamp(long value, long min, long max)
{
if (value < min)
{
return min;
}
if (value > max)
{
return max;
}
return value;
}
public static LFloat Clamp(LFloat value, LFloat min, LFloat max)
{
if (value < min)
{
return min;
}
if (value > max)
{
return max;
}
return value;
}
public static LFloat Clamp01(LFloat value)
{
if (value < LFloat.Zero)
{
return LFloat.Zero;
}
if (value > LFloat.One)
{
return LFloat.One;
}
return value;
}
public static long Max(long a, long b)
{
return (a <= b) ? b : a;
}
public static int Max(int a, int b)
{
return (a <= b) ? b : a;
}
public static long Min(long a, long b)
{
return (a > b) ? b : a;
}
public static int Min(int a, int b)
{
return (a > b) ? b : a;
}
public static LFloat Min(LFloat a, LFloat b)
{
return new LFloat(true, Min(a.rawValue, b.rawValue));
}
public static LFloat Min(LFloat a, int b)
{
return new LFloat(true, Min(a.rawValue, b * LFloat.Precision));
}
public static LFloat Min(int a, LFloat b)
{
return new LFloat(true, Min(a * LFloat.Precision, b.rawValue));
}
public static LFloat Max(LFloat a, LFloat b)
{
return new LFloat(true, Max(a.rawValue, b.rawValue));
}
public static LFloat Max(int a, LFloat b)
{
return new LFloat(true, Max(a * LFloat.Precision, b.rawValue));
}
public static LFloat Max(LFloat a, int b)
{
return new LFloat(true, Max(a.rawValue, b * LFloat.Precision));
}
public static int Min(params int[] values)
{
int length = values.Length;
if (length == 0) { return 0; }
int num = values[0];
for (int index = 1; index < length; ++index)
{
if (values[index] < num)
num = values[index];
}
return num;
}
public static LFloat Min(params LFloat[] values)
{
int length = values.Length;
if (length == 0) { return LFloat.Zero; }
LFloat num = values[0];
for (int index = 1; index < length; ++index)
{
if (values[index] < num)
num = values[index];
}
return num;
}
public static int Max(params int[] values)
{
int length = values.Length;
if (length == 0)
return 0;
int num = values[0];
for (int index = 1; index < length; ++index)
{
if (values[index] > num)
num = values[index];
}
return num;
}
public static LFloat Max(params LFloat[] values)
{
int length = values.Length;
if (length == 0)
return LFloat.Zero;
var num = values[0];
for (int index = 1; index < length; ++index)
{
if (values[index] > num)
num = values[index];
}
return num;
}
#endregion
#region Lerp相关
/// <summary>
/// 定点数线性插值
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <param name="f"></param>
/// <returns></returns>
public static LFloat Lerp(LFloat from, LFloat to, LFloat f)
{
return new LFloat(true, ((to.rawValue - from.rawValue) * f.rawValue / LFloat.Precision) + from.rawValue);
}
/// <summary>
/// 获取线性插值比例
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <param name="value"></param>
/// <returns></returns>
public static LFloat InverseLerp(LFloat from, LFloat to, LFloat value)
{
if (from != to) { return Clamp01(((value - from) / (to - from))); }
return LFloat.Zero;
}
#endregion
#region constant value
/// <summary>
/// 数学常数 - 定点数四分PI
/// </summary>
public static readonly LFloat QuadPI = new LFloat(true, kRawLQuadPI);
/// <summary>
/// 数学常数 - 定点数二分PI
/// </summary>
public static readonly LFloat HalfPI = new LFloat(true, kRawLHalfPI);
/// <summary>
/// 数学常数 - 定点数PI
/// </summary>
public static readonly LFloat PI = new LFloat(true, kRawLPI);
/// <summary>
/// 数学常数 - 定点数2PI
/// </summary>
public static readonly LFloat PI2 = new LFloat(true, kRawL2PI);
/// <summary>
/// 数学常数 - 定点数弧度转角度的算术因子
/// </summary>
public static readonly LFloat Rad2Deg = new LFloat(true, kRawLRad2Deg);
/// <summary>
/// 数学常数 - 定点数角度转弧度的算术因子
/// </summary>
public static readonly LFloat Deg2Rad = new LFloat(true, kRawLDeg2Rad);
/// <summary>
/// 四分PI定点数真实值
/// </summary>
private const long kRawLQuadPI = 785398L; //0.7853981
/// <summary>
/// 二分PI定点数真实值
/// </summary>
private const long kRawLHalfPI = 1570796L; //1.5707963
/// <summary>
/// PI定点数真实值
/// </summary>
private const long kRawLPI = 3141593L; //3.1415926
/// <summary>
/// 2PI定点数真实值
/// </summary>
private const long kRawL2PI = 6283185L; //6.2831853
/// <summary>
/// 弧度转角度的算术因子定点数真实值
/// </summary>
private const long kRawLRad2Deg = 57295780L; //57.2957795
/// <summary>
/// 角度转弧度的算术因子定点数真实值
/// </summary>
private const long kRawLDeg2Rad = 17453L; //0.0174532
#endregion
}
}