1237 lines
36 KiB
C#
1237 lines
36 KiB
C#
#if DEBUG
|
|
#define RANGE_CHECK
|
|
#endif
|
|
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Diagnostics.Contracts;
|
|
using System.Numerics;
|
|
using System.Runtime.CompilerServices;
|
|
|
|
namespace MagmaEngine.Math;
|
|
|
|
public static class FixPoint16Ext
|
|
{
|
|
public static FixPoint16 Average(this IEnumerable<FixPoint16> source)
|
|
{
|
|
return Average(source, f => f);
|
|
}
|
|
|
|
public static FixPoint16 Average<TSource>(this IEnumerable<TSource> source, Func<TSource, FixPoint16> selector)
|
|
{
|
|
using IEnumerator<TSource> e = source.GetEnumerator();
|
|
if (!e.MoveNext())
|
|
return FixPoint16.Zero;
|
|
|
|
long sum = selector(e.Current).m_Value;
|
|
int count = 1;
|
|
while (e.MoveNext())
|
|
{
|
|
checked { sum += selector(e.Current).m_Value; }
|
|
count++;
|
|
}
|
|
|
|
return new() { m_Value = (int)(sum / count) };
|
|
}
|
|
|
|
public static FixPoint16 Sum(this IEnumerable<FixPoint16> source)
|
|
{
|
|
using IEnumerator<FixPoint16> e = source.GetEnumerator();
|
|
if (!e.MoveNext())
|
|
return FixPoint16.Zero;
|
|
|
|
var sum = e.Current;
|
|
while (e.MoveNext())
|
|
{
|
|
sum += e.Current;
|
|
}
|
|
|
|
return sum;
|
|
}
|
|
}
|
|
|
|
public partial struct FixPoint16 : IEquatable<FixPoint16>, IComparable<FixPoint16>, IAdditionOperators<FixPoint16, FixPoint16, FixPoint16>, IMultiplyOperators<FixPoint16, FixPoint16, FixPoint16>
|
|
{
|
|
private enum EParseState
|
|
{
|
|
TrimStartWhitespace,
|
|
ParseMantissa,
|
|
ParseExponent,
|
|
TrimEndWhitespace
|
|
}
|
|
|
|
public const int c_Shift = 16;
|
|
public const int c_IntegerMin = -32768;
|
|
public const int c_IntegerMax = 32767;
|
|
public const long c_LongMin = -0x80000000L;
|
|
public const long c_LongMax = 0x7fffffffL;
|
|
public const int c_Half = 32768;
|
|
private const int c_FractionMask = 0x0000ffff;
|
|
public const double c_Multiplier = 65536.0;
|
|
public const double c_Divisor = 1.0 / 65536.0;
|
|
public const float c_MultiplierFloat = 65536.0f;
|
|
public const float c_DivisorFloat = (float)c_Divisor;
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public FixPoint16(FixPoint16 other)
|
|
{
|
|
m_Value = other.m_Value;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public FixPoint16(int value)
|
|
{
|
|
#if RANGE_CHECK
|
|
if (value < c_IntegerMin || value > c_IntegerMax)
|
|
{
|
|
throw new ArithmeticException($"Integer to FixPoint argument out of range: {value}");
|
|
}
|
|
#endif
|
|
m_Value = value << c_Shift;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public FixPoint16(int dividend, int divisor)
|
|
{
|
|
#if RANGE_CHECK
|
|
if (divisor == 0)
|
|
{
|
|
throw new ArithmeticException("Divison by zero");
|
|
}
|
|
#endif
|
|
long iResult;
|
|
|
|
if (((uint)dividend & 0x80000000U) == ((uint)divisor & 0x80000000U))
|
|
{
|
|
iResult = (((long)dividend << c_Shift) + (divisor / 2)) / divisor;
|
|
}
|
|
else
|
|
{
|
|
iResult = (((long)dividend << c_Shift) - (divisor / 2)) / divisor;
|
|
}
|
|
#if RANGE_CHECK
|
|
if (iResult < c_LongMin || iResult > c_LongMax)
|
|
{
|
|
throw new ArithmeticException($"Division result out of range: {iResult}");
|
|
}
|
|
#endif
|
|
m_Value = (int)iResult;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public FixPoint16(long dividend, long divisor)
|
|
{
|
|
#if RANGE_CHECK
|
|
if (divisor == 0L)
|
|
throw new ArithmeticException("Divison by zero");
|
|
#endif
|
|
while (dividend > (long.MaxValue >> c_Shift) || dividend < (long.MinValue >> c_Shift))
|
|
{
|
|
dividend >>= 1;
|
|
divisor >>= 1;
|
|
}
|
|
|
|
long iResult;
|
|
|
|
if (((ulong)dividend & 0x8000000000000000UL) == ((ulong)divisor & 0x8000000000000000UL))
|
|
iResult = ((dividend << c_Shift) + (divisor / 2)) / divisor;
|
|
else
|
|
iResult = ((dividend << c_Shift) - (divisor / 2)) / divisor;
|
|
|
|
#if RANGE_CHECK
|
|
if (iResult < c_LongMin || iResult > c_LongMax)
|
|
throw new ArithmeticException($"Division result out of range: {iResult}");
|
|
#endif
|
|
m_Value = (int)iResult;
|
|
}
|
|
|
|
public FixPoint16(double value)
|
|
{
|
|
#if RANGE_CHECK
|
|
if (value < c_IntegerMin || value > c_IntegerMax)
|
|
{
|
|
throw new ArithmeticException($"Double to FixPoint argument out of range: {value}");
|
|
}
|
|
#endif
|
|
if (value < 0.0)
|
|
{
|
|
m_Value = (int)((value * c_Multiplier) - 0.5);
|
|
}
|
|
else
|
|
{
|
|
m_Value = (int)((value * c_Multiplier) + 0.5);
|
|
}
|
|
}
|
|
|
|
public FixPoint16(float value)
|
|
{
|
|
#if RANGE_CHECK
|
|
if (value < c_IntegerMin || value > c_IntegerMax)
|
|
{
|
|
throw new ArithmeticException($"Single to FixPoint argument out of range: {value}");
|
|
}
|
|
#endif
|
|
m_Value = 0;
|
|
Update(value);
|
|
}
|
|
|
|
public static FixPoint16 FromString(ReadOnlySpan<char> number)
|
|
{
|
|
Parse(number, out var numerator, out var denominator);
|
|
return FromRational(numerator, denominator);
|
|
}
|
|
|
|
public static bool TryParse(ReadOnlySpan<char> number, out FixPoint16 result)
|
|
{
|
|
if (!TryParse(number, out var numerator, out var denominator))
|
|
{
|
|
result = Zero;
|
|
return false;
|
|
}
|
|
|
|
result = FromRational(numerator, denominator);
|
|
return true;
|
|
}
|
|
|
|
public static bool TryParse(ReadOnlySpan<char> number, out long numerator, out long denominator)
|
|
{
|
|
try
|
|
{
|
|
Parse(number, out numerator, out denominator);
|
|
}
|
|
catch (Exception)
|
|
{
|
|
numerator = 0;
|
|
denominator = 1;
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
public static void Parse(ReadOnlySpan<char> number, out long numerator, out long denominator)
|
|
{
|
|
numerator = 0L;
|
|
var afterDecimal = false;
|
|
var mantissaHasDigit = false;
|
|
var mantissaNegative = false;
|
|
var hasExponent = false;
|
|
var exponentHasDigit = false;
|
|
var exponentNegative = false;
|
|
var exponentValue = 0;
|
|
var denominatorPower = 0;
|
|
var state = EParseState.TrimStartWhitespace;
|
|
|
|
checked
|
|
{
|
|
var i = 0;
|
|
while (i < number.Length)
|
|
{
|
|
var character = number[i];
|
|
switch (state)
|
|
{
|
|
case EParseState.TrimStartWhitespace:
|
|
{
|
|
if (char.IsWhiteSpace(character))
|
|
{
|
|
++i;
|
|
}
|
|
else
|
|
{
|
|
state = EParseState.ParseMantissa;
|
|
}
|
|
break;
|
|
}
|
|
case EParseState.ParseMantissa:
|
|
{
|
|
if (char.IsWhiteSpace(character))
|
|
{
|
|
state = EParseState.TrimEndWhitespace;
|
|
break;
|
|
}
|
|
|
|
switch (character)
|
|
{
|
|
case '+' or '-' when !mantissaHasDigit:
|
|
{
|
|
mantissaNegative = character == '-';
|
|
break;
|
|
}
|
|
case >= '0' and <= '9':
|
|
{
|
|
var digit = character - '0';
|
|
numerator = numerator * 10 + digit;
|
|
|
|
if (afterDecimal)
|
|
denominatorPower += 1;
|
|
|
|
mantissaHasDigit = true;
|
|
break;
|
|
}
|
|
case '.' when !afterDecimal:
|
|
{
|
|
afterDecimal = true;
|
|
break;
|
|
}
|
|
case 'e' or 'E' when mantissaHasDigit:
|
|
{
|
|
hasExponent = true;
|
|
state = EParseState.ParseExponent;
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
throw new FormatException($"Invalid character '{character}' in mantissa.");
|
|
}
|
|
}
|
|
|
|
++i;
|
|
break;
|
|
}
|
|
case EParseState.ParseExponent:
|
|
{
|
|
if (char.IsWhiteSpace(character))
|
|
{
|
|
state = EParseState.TrimEndWhitespace;
|
|
break;
|
|
}
|
|
|
|
switch (character)
|
|
{
|
|
case '+' or '-' when !exponentHasDigit:
|
|
{
|
|
exponentNegative = character == '-';
|
|
break;
|
|
}
|
|
case >= '0' and <= '9':
|
|
{
|
|
var digit = character - '0';
|
|
exponentValue = exponentValue * 10 + digit;
|
|
exponentHasDigit = true;
|
|
break;
|
|
}
|
|
default:
|
|
{
|
|
throw new FormatException($"Invalid character '{character}' in exponent.");
|
|
}
|
|
}
|
|
|
|
++i;
|
|
break;
|
|
}
|
|
case EParseState.TrimEndWhitespace:
|
|
{
|
|
if (char.IsWhiteSpace(character))
|
|
{
|
|
++i;
|
|
}
|
|
else
|
|
{
|
|
throw new FormatException($"Unexpected character '{character}' after the number.");
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (!mantissaHasDigit)
|
|
throw new FormatException("Missing mantissa digits.");
|
|
|
|
if (hasExponent)
|
|
{
|
|
if (!exponentHasDigit)
|
|
throw new FormatException("Missing exponent digits.");
|
|
|
|
exponentValue = exponentNegative ? -exponentValue : exponentValue;
|
|
if (exponentValue > 0)
|
|
{
|
|
while (exponentValue-- > 0)
|
|
{
|
|
if (denominatorPower > 0)
|
|
{
|
|
denominatorPower -= 1;
|
|
}
|
|
else
|
|
{
|
|
numerator *= 10;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
denominatorPower -= exponentValue;
|
|
}
|
|
}
|
|
}
|
|
|
|
numerator = mantissaNegative ? -numerator : numerator;
|
|
denominator = 1;
|
|
for (int i = 0; i < denominatorPower; ++i)
|
|
denominator *= 10;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 FromValue(long value)
|
|
{
|
|
#if RANGE_CHECK
|
|
if (value < c_LongMin || value > c_LongMax)
|
|
throw new ArithmeticException($"Value out of range: {value}");
|
|
#endif
|
|
|
|
return new() { m_Value = (int)value };
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 FromInt(int value)
|
|
{
|
|
return new(value);
|
|
}
|
|
|
|
public static FixPoint16 FromDouble(double value)
|
|
{
|
|
return new(value);
|
|
}
|
|
|
|
public static FixPoint16 FromFloat(float value)
|
|
{
|
|
return new(value);
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 FromRational(int dividend, int divisor)
|
|
{
|
|
return new(dividend, divisor);
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 FromRational(long dividend, long divisor)
|
|
{
|
|
return new(dividend, divisor);
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public readonly int ToIntFloor()
|
|
{
|
|
return m_Value >> c_Shift;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public readonly int ToIntCeil()
|
|
{
|
|
unchecked
|
|
{
|
|
return (int)(((long)m_Value + c_FractionMask) >> c_Shift);
|
|
}
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public readonly int ToIntRound()
|
|
{
|
|
unchecked
|
|
{
|
|
return (int)(((long)m_Value + c_Half) >> c_Shift);
|
|
}
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public readonly int ToInt()
|
|
{
|
|
unchecked
|
|
{
|
|
if (m_Value < 0)
|
|
{
|
|
return -(-m_Value >> c_Shift);
|
|
}
|
|
|
|
return m_Value >> c_Shift;
|
|
}
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public readonly short ToShort()
|
|
{
|
|
var value = ToInt();
|
|
return (short)value;
|
|
}
|
|
|
|
public static FixPoint16 AngleFromRadian(double angle)
|
|
{
|
|
return new(angle * (0.5 / System.Math.PI));
|
|
}
|
|
|
|
public static FixPoint16 AngleFromDegrees(double angle)
|
|
{
|
|
return new(angle * (1.0 / 360.0));
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 AngleFromDegrees(int iAngle)
|
|
{
|
|
return (FixPoint16)iAngle / 360;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public readonly double ToDouble()
|
|
{
|
|
return c_Divisor * m_Value;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public readonly float ToFloat()
|
|
{
|
|
return c_DivisorFloat * m_Value;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public readonly double AngleToRadians()
|
|
{
|
|
return ToDouble() * (System.Math.PI * 2.0);
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public readonly float AngleToRadiansFloat()
|
|
{
|
|
return ToFloat() * (MathF.PI * 2.0f);
|
|
}
|
|
|
|
public readonly double AngleToDegrees()
|
|
{
|
|
return ToDouble() * 360.0;
|
|
}
|
|
|
|
public override readonly string ToString()
|
|
{
|
|
return $"{ToDouble()}[0x{m_Value:x}]";
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public override readonly int GetHashCode()
|
|
{
|
|
return m_Value.GetHashCode();
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public readonly int CompareTo(FixPoint16 other)
|
|
{
|
|
return m_Value.CompareTo(other.m_Value);
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public override readonly bool Equals(object? obj)
|
|
{
|
|
if (obj == null || obj.GetType() != typeof(FixPoint16))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
return ((FixPoint16)obj).m_Value == m_Value;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public readonly bool Equals(FixPoint16 other)
|
|
{
|
|
return m_Value == other.m_Value;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public readonly bool IsZero()
|
|
{
|
|
return m_Value == 0;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static bool operator ==(FixPoint16 a, FixPoint16 b)
|
|
{
|
|
return a.m_Value == b.m_Value;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static bool operator !=(FixPoint16 a, FixPoint16 b)
|
|
{
|
|
return a.m_Value != b.m_Value;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static bool operator <(FixPoint16 a, FixPoint16 b)
|
|
{
|
|
return a.m_Value < b.m_Value;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static bool operator >(FixPoint16 a, FixPoint16 b)
|
|
{
|
|
return a.m_Value > b.m_Value;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static bool operator <=(FixPoint16 a, FixPoint16 b)
|
|
{
|
|
return a.m_Value <= b.m_Value;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static bool operator >=(FixPoint16 a, FixPoint16 b)
|
|
{
|
|
return a.m_Value >= b.m_Value;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 operator <<(FixPoint16 a, int shift)
|
|
{
|
|
return new() { m_Value = a.m_Value << shift };
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 operator >> (FixPoint16 a, int shift)
|
|
{
|
|
return new() { m_Value = a.m_Value >> shift };
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 operator +(FixPoint16 a, FixPoint16 b)
|
|
{
|
|
unchecked
|
|
{
|
|
#if RANGE_CHECK
|
|
var iResult = (long)a.m_Value + b.m_Value;
|
|
|
|
if (iResult < c_LongMin || iResult > c_LongMax)
|
|
{
|
|
throw new ArithmeticException($"Addition result out of range: {iResult}");
|
|
}
|
|
|
|
return new() { m_Value = (int)iResult };
|
|
#else
|
|
return new FixPoint16 { m_Value = a.m_Value + b.m_Value };
|
|
#endif
|
|
}
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 operator -(FixPoint16 a, FixPoint16 b)
|
|
{
|
|
unchecked
|
|
{
|
|
#if RANGE_CHECK
|
|
var iResult = (long)a.m_Value - b.m_Value;
|
|
|
|
if (iResult < c_LongMin || iResult > c_LongMax)
|
|
{
|
|
throw new ArithmeticException($"Substraction result out of range: {iResult}");
|
|
}
|
|
|
|
return new() { m_Value = (int)iResult };
|
|
#else
|
|
return new FixPoint16 { m_Value = a.m_Value - b.m_Value };
|
|
#endif
|
|
}
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 operator -(FixPoint16 a)
|
|
{
|
|
unchecked
|
|
{
|
|
#if RANGE_CHECK
|
|
if (a.m_Value < 0 && -a.m_Value < 0)
|
|
{
|
|
throw new ArithmeticException($"Negation result out of range: {a.m_Value}");
|
|
}
|
|
#endif
|
|
return new() { m_Value = -a.m_Value };
|
|
}
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 operator *(FixPoint16 a, FixPoint16 b)
|
|
{
|
|
unchecked
|
|
{
|
|
var iResult = (((long)a.m_Value * b.m_Value) + c_Half) >> c_Shift;
|
|
#if RANGE_CHECK
|
|
if (iResult < c_LongMin || iResult > c_LongMax)
|
|
{
|
|
throw new ArithmeticException($"Multiplication result out of range: {iResult}");
|
|
}
|
|
#endif
|
|
return new() { m_Value = (int)iResult };
|
|
}
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 operator *(FixPoint16 a, int value)
|
|
{
|
|
unchecked
|
|
{
|
|
#if RANGE_CHECK
|
|
var iResult = (long)a.m_Value * value;
|
|
|
|
if (iResult < c_LongMin || iResult > c_LongMax)
|
|
{
|
|
throw new ArithmeticException($"Multiplication result out of range: {iResult}");
|
|
}
|
|
|
|
return new() { m_Value = (int)iResult };
|
|
#else
|
|
return new FixPoint16 { m_Value = a.m_Value * value };
|
|
#endif
|
|
}
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 operator /(FixPoint16 a, int value)
|
|
{
|
|
unchecked
|
|
{
|
|
#if RANGE_CHECK
|
|
if (value == 0)
|
|
{
|
|
throw new ArithmeticException("Divison by zero");
|
|
}
|
|
#endif
|
|
if ((a.m_Value & 0x80000000) == (value & 0x80000000))
|
|
{
|
|
return new() { m_Value = (int)(((long)a.m_Value + (value / 2)) / value) };
|
|
}
|
|
|
|
return new() { m_Value = (int)(((long)a.m_Value - (value / 2)) / value) };
|
|
}
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 operator /(FixPoint16 a, FixPoint16 b)
|
|
{
|
|
unchecked
|
|
{
|
|
#if RANGE_CHECK
|
|
if (b.m_Value == 0)
|
|
{
|
|
throw new ArithmeticException("Divison by zero");
|
|
}
|
|
#endif
|
|
long iResult;
|
|
|
|
if ((a.m_Value & 0x80000000) == (b.m_Value & 0x80000000))
|
|
{
|
|
iResult = (((long)a.m_Value << c_Shift) + (b.m_Value / 2)) / b.m_Value;
|
|
}
|
|
else
|
|
{
|
|
iResult = (((long)a.m_Value << c_Shift) - (b.m_Value / 2)) / b.m_Value;
|
|
}
|
|
#if RANGE_CHECK
|
|
if (iResult < c_LongMin || iResult > c_LongMax)
|
|
{
|
|
throw new ArithmeticException($"Division result out of range: {iResult}");
|
|
}
|
|
#endif
|
|
return new() { m_Value = (int)iResult };
|
|
}
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static implicit operator FixPoint16(int value)
|
|
{
|
|
return new(value);
|
|
}
|
|
|
|
public static explicit operator FixPoint16(double value)
|
|
{
|
|
return new(value);
|
|
}
|
|
|
|
public static explicit operator FixPoint16(float value)
|
|
{
|
|
return new(value);
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static explicit operator int(FixPoint16 value)
|
|
{
|
|
return value.ToInt();
|
|
}
|
|
|
|
public static explicit operator double(FixPoint16 value)
|
|
{
|
|
return value.ToDouble();
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static explicit operator float(FixPoint16 value)
|
|
{
|
|
return value.ToFloat();
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 Floor(FixPoint16 value)
|
|
{
|
|
return value.ToIntFloor();
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 Ceil(FixPoint16 value)
|
|
{
|
|
return value.ToIntCeil();
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 Round(FixPoint16 value)
|
|
{
|
|
return value.ToIntRound();
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static int Sign(FixPoint16 value)
|
|
{
|
|
return IntMath.Sign(value.m_Value);
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static int SignZeroAsPositive(FixPoint16 value)
|
|
{
|
|
return IntMath.Sign(value.m_Value | 1);
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 Abs(FixPoint16 value)
|
|
{
|
|
return new() { m_Value = IntMath.Abs(value.m_Value) };
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 Min(FixPoint16 a, FixPoint16 b)
|
|
{
|
|
return new() { m_Value = IntMath.Min(a.m_Value, b.m_Value) };
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 Max(FixPoint16 a, FixPoint16 b)
|
|
{
|
|
return new() { m_Value = IntMath.Max(a.m_Value, b.m_Value) };
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 Clamp(FixPoint16 value, FixPoint16 min, FixPoint16 max)
|
|
{
|
|
return new() { m_Value = IntMath.Clamp(value.m_Value, min.m_Value, max.m_Value) };
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 Saturate(FixPoint16 value)
|
|
{
|
|
return Clamp(value, Zero, One);
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 Step(FixPoint16 value, FixPoint16 amount)
|
|
{
|
|
return amount >= value ? One : Zero;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Interpolates between two values using a cubic equation.
|
|
/// </summary>
|
|
/// <param name="value1">The source value.</param>
|
|
/// <param name="value2">The source value.</param>
|
|
/// <param name="amount">The weighting value.</param>
|
|
/// <returns>The interpolated value.</returns>
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 SmoothStep(FixPoint16 value1, FixPoint16 value2, FixPoint16 amount)
|
|
{
|
|
if (value2 <= value1)
|
|
return Step(value1, amount);
|
|
|
|
var x = Saturate((amount - value1) / (value2 - value1));
|
|
return x * x * (new FixPoint16(3) - x * 2);
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static bool Equal(FixPoint16 a, FixPoint16 b, FixPoint16 threshold)
|
|
{
|
|
var fpDiff = Abs(a - b);
|
|
return fpDiff <= threshold;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 Sqrt(FixPoint16 value)
|
|
{
|
|
var longValue = (long)value.m_Value << c_Shift;
|
|
return new() { m_Value = (int)IntMath.Sqrt(longValue) };
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 SqrtLong(long value)
|
|
{
|
|
value = IntMath.Sqrt(value);
|
|
#if RANGE_CHECK
|
|
if (value > 0x7fffffff)
|
|
{
|
|
throw new ArithmeticException($"Length out of range: {value}");
|
|
}
|
|
#endif
|
|
return new() { m_Value = (int)value };
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static long LengthSquared(FixPoint16 a, FixPoint16 b)
|
|
{
|
|
var aSquared = (long)a.m_Value * a.m_Value;
|
|
var bSquared = (long)b.m_Value * b.m_Value;
|
|
var value = aSquared + bSquared;
|
|
#if RANGE_CHECK
|
|
if (value < 0)
|
|
{
|
|
throw new ArithmeticException($"Length squared out of range: {value}");
|
|
}
|
|
#endif
|
|
|
|
return value;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 Length(FixPoint16 a, FixPoint16 b)
|
|
{
|
|
var aSquared = (long)a.m_Value * a.m_Value;
|
|
var bSquared = (long)b.m_Value * b.m_Value;
|
|
var value = aSquared + bSquared;
|
|
#if RANGE_CHECK
|
|
if (value < 0)
|
|
{
|
|
throw new ArithmeticException($"Length squared out of range: {value}");
|
|
}
|
|
#endif
|
|
|
|
value = IntMath.Sqrt(value);
|
|
#if RANGE_CHECK
|
|
if (value > 0x7fffffff)
|
|
{
|
|
throw new ArithmeticException($"Length out of range: {value}");
|
|
}
|
|
#endif
|
|
return new() { m_Value = (int)value };
|
|
}
|
|
|
|
public static FixPoint16 Length(FixPoint16 a, FixPoint16 b, FixPoint16 c)
|
|
{
|
|
var aSquared = (long)a.m_Value * a.m_Value;
|
|
var bSquared = (long)b.m_Value * b.m_Value;
|
|
var cSquared = (long)c.m_Value * c.m_Value;
|
|
var value = aSquared + bSquared;
|
|
#if RANGE_CHECK
|
|
if (value < 0)
|
|
{
|
|
throw new ArithmeticException($"Length squared out of range: {value}");
|
|
}
|
|
#endif
|
|
value += cSquared;
|
|
#if RANGE_CHECK
|
|
if (value < 0)
|
|
{
|
|
throw new ArithmeticException($"Length squared out of range: {value}");
|
|
}
|
|
#endif
|
|
|
|
value = IntMath.Sqrt(value);
|
|
#if RANGE_CHECK
|
|
if (value > 0x7fffffff)
|
|
{
|
|
throw new ArithmeticException($"Length out of range: {value}");
|
|
}
|
|
#endif
|
|
return new() { m_Value = (int)value };
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 Cos(FixPoint16 value)
|
|
{
|
|
return new() { m_Value = s_CosTable[value.m_Value & 0xffff] };
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 Sin(FixPoint16 value)
|
|
{
|
|
return Cos(value - HalfPi);
|
|
}
|
|
|
|
// No table for Tan, uses division. Add table if performance of Tan is critical.
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 Tan(FixPoint16 value)
|
|
{
|
|
return Sin(value) / Cos(value);
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 Asin(FixPoint16 value)
|
|
{
|
|
if (value.m_Value < 0)
|
|
{
|
|
#if RANGE_CHECK
|
|
if (value.m_Value < MinusOne.m_Value)
|
|
{
|
|
throw new ArithmeticException($"Asin number out of range: {value.m_Value}");
|
|
}
|
|
#endif
|
|
return new() { m_Value = -s_AsinTable[-value.m_Value] };
|
|
}
|
|
#if RANGE_CHECK
|
|
if (value.m_Value > One.m_Value)
|
|
{
|
|
throw new ArithmeticException($"Asin number out of range: {value.m_Value}");
|
|
}
|
|
#endif
|
|
return new() { m_Value = s_AsinTable[value.m_Value] };
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 Acos(FixPoint16 value)
|
|
{
|
|
return HalfPi - Asin(value);
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 Atan(FixPoint16 value)
|
|
{
|
|
if (value.m_Value < 0)
|
|
{
|
|
if (-value.m_Value < s_AtanTable.Length)
|
|
{
|
|
return new() { m_Value = s_AtanTable[-value.m_Value] };
|
|
}
|
|
|
|
return new() { m_Value = MinusHalfPi.m_Value + s_AtanTable[-(One / value).m_Value] };
|
|
}
|
|
|
|
if (value.m_Value < s_AtanTable.Length)
|
|
{
|
|
return new() { m_Value = -s_AtanTable[value.m_Value] };
|
|
}
|
|
|
|
return new() { m_Value = s_AtanTable[(One / value).m_Value] };
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 Atan2(FixPoint16 y, FixPoint16 x)
|
|
{
|
|
unchecked
|
|
{
|
|
if (y.m_Value == 0)
|
|
{
|
|
if (x.m_Value >= 0)
|
|
{
|
|
return Zero;
|
|
}
|
|
|
|
return Pi;
|
|
}
|
|
|
|
if (y.m_Value > 0)
|
|
{
|
|
if (x.m_Value == 0)
|
|
{
|
|
return HalfPi;
|
|
}
|
|
|
|
if (x.m_Value > 0)
|
|
{
|
|
// x > 0, y > 0
|
|
if (y.m_Value <= x.m_Value)
|
|
{
|
|
return new() { m_Value = s_AtanTable[(y / x).m_Value] };
|
|
}
|
|
|
|
return new() { m_Value = HalfPi.m_Value - s_AtanTable[(x / y).m_Value] };
|
|
}
|
|
|
|
// x < 0, y > 0
|
|
if (y.m_Value <= -x.m_Value)
|
|
{
|
|
return new() { m_Value = Pi.m_Value - s_AtanTable[-(y / x).m_Value] };
|
|
}
|
|
|
|
return new() { m_Value = HalfPi.m_Value + s_AtanTable[-(x / y).m_Value] };
|
|
}
|
|
|
|
if (x.m_Value == 0)
|
|
{
|
|
return MinusHalfPi;
|
|
}
|
|
|
|
if (x.m_Value > 0)
|
|
{
|
|
// x > 0, y < 0
|
|
if (-y.m_Value <= x.m_Value)
|
|
{
|
|
return new() { m_Value = -s_AtanTable[-(y / x).m_Value] };
|
|
}
|
|
|
|
return new() { m_Value = MinusHalfPi.m_Value + s_AtanTable[-(x / y).m_Value] };
|
|
}
|
|
|
|
// x < 0, y < 0
|
|
if (y.m_Value >= x.m_Value)
|
|
{
|
|
return new() { m_Value = MinusPi.m_Value + s_AtanTable[(y / x).m_Value] };
|
|
}
|
|
|
|
return new() { m_Value = MinusHalfPi.m_Value - s_AtanTable[(x / y).m_Value] };
|
|
}
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 AngleDifference(FixPoint16 a, FixPoint16 b)
|
|
{
|
|
var iResult = (b.m_Value - a.m_Value) & c_FractionMask;
|
|
|
|
if (iResult > Pi.m_Value)
|
|
{
|
|
iResult -= TwoPi.m_Value;
|
|
}
|
|
|
|
return new() { m_Value = iResult };
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 AngleDifferencePositive(FixPoint16 a, FixPoint16 b)
|
|
{
|
|
var iResult = (b.m_Value - a.m_Value) & c_FractionMask;
|
|
return new() { m_Value = iResult };
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 AngleNormalize(FixPoint16 angle)
|
|
{
|
|
return Fract(angle + Pi) - Pi;
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 Lerp(FixPoint16 value1, FixPoint16 value2, FixPoint16 amount)
|
|
{
|
|
return value1 + (amount * (value2 - value1));
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 InvLerp(FixPoint16 amount, FixPoint16 min, FixPoint16 max)
|
|
{
|
|
if (min == max)
|
|
return amount > max ? One : Zero;
|
|
|
|
return (amount - min) / (max - min);
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 Remap(FixPoint16 oldValue, FixPoint16 oldMin, FixPoint16 oldMax, FixPoint16 newMin, FixPoint16 newMax)
|
|
{
|
|
if (oldValue >= oldMax)
|
|
return newMax;
|
|
|
|
if (oldValue <= oldMin)
|
|
return newMin;
|
|
|
|
return Lerp(newMin, newMax, InvLerp(oldValue, oldMin, oldMax));
|
|
}
|
|
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
public static FixPoint16 Fract(FixPoint16 p)
|
|
{
|
|
return new() { m_Value = p.m_Value & c_FractionMask };
|
|
}
|
|
|
|
public static FixPoint16 Sum(IEnumerable<FixPoint16> range)
|
|
{
|
|
var result = Zero;
|
|
|
|
foreach (var fp in range)
|
|
result += fp;
|
|
return result;
|
|
}
|
|
|
|
public static FixPoint16 Product(IEnumerable<FixPoint16> range)
|
|
{
|
|
var result = One;
|
|
|
|
foreach (var fp in range)
|
|
result *= fp;
|
|
return result;
|
|
}
|
|
|
|
public static FixPoint16 Min(IEnumerable<FixPoint16> range)
|
|
{
|
|
var result = MaxValue;
|
|
|
|
foreach (var fp in range)
|
|
result = Min(fp, result);
|
|
return result;
|
|
}
|
|
|
|
public static FixPoint16 Max(IEnumerable<FixPoint16> range)
|
|
{
|
|
var result = MinValue;
|
|
|
|
foreach (var fp in range)
|
|
result = Max(fp, result);
|
|
return result;
|
|
}
|
|
|
|
public void Update(float value)
|
|
{
|
|
if (value < 0.0f)
|
|
{
|
|
m_Value = (int)((value * c_MultiplierFloat) - 0.5f);
|
|
}
|
|
else
|
|
{
|
|
m_Value = (int)((value * c_MultiplierFloat) + 0.5f);
|
|
}
|
|
}
|
|
|
|
public int m_Value;
|
|
|
|
public FixPoint16 Doubled
|
|
{
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
[Pure]
|
|
get => new FixPoint16 { m_Value = m_Value * 2 };
|
|
}
|
|
|
|
public FixPoint16 Quadrupled
|
|
{
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
[Pure]
|
|
get => new FixPoint16 { m_Value = m_Value * 4 };
|
|
}
|
|
|
|
public FixPoint16 Halved
|
|
{
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
[Pure]
|
|
get => new FixPoint16 { m_Value = m_Value / 2 };
|
|
}
|
|
|
|
public FixPoint16 Quartered
|
|
{
|
|
[MethodImpl(MethodImplOptions.AggressiveInlining)]
|
|
[Pure]
|
|
get => new FixPoint16 { m_Value = m_Value / 4 };
|
|
}
|
|
|
|
public static readonly FixPoint16 Zero = default;
|
|
public static readonly FixPoint16 MinValue = new() { m_Value = int.MinValue };
|
|
public static readonly FixPoint16 MaxValue = new() { m_Value = int.MaxValue };
|
|
public static readonly FixPoint16 Epsilon = new() { m_Value = 1 };
|
|
public static readonly FixPoint16 EpsilonOrientation = new() { m_Value = 10 };
|
|
public static readonly FixPoint16 MinusEpsilon = new() { m_Value = -1 };
|
|
public static readonly FixPoint16 One = new(1);
|
|
public static readonly FixPoint16 MinusOne = new(-1);
|
|
public static readonly FixPoint16 Half = new() { m_Value = One.m_Value / 2 };
|
|
public static readonly FixPoint16 Quarter = new() { m_Value = One.m_Value / 4 };
|
|
public static readonly FixPoint16 MinusHalf = -Half;
|
|
public static readonly FixPoint16 TwoPi = One;
|
|
public static readonly FixPoint16 Pi = Half;
|
|
public static readonly FixPoint16 HalfPi = new() { m_Value = Pi.m_Value / 2 };
|
|
public static readonly FixPoint16 QuaterPi = new() { m_Value = HalfPi.m_Value / 2 };
|
|
public static readonly FixPoint16 MinusTwoPi = -TwoPi;
|
|
public static readonly FixPoint16 MinusPi = -Pi;
|
|
public static readonly FixPoint16 MinusHalfPi = -HalfPi;
|
|
public static readonly FixPoint16 MinusQuaterPi = -QuaterPi;
|
|
public static readonly FixPoint16 InvSqrt2 = One / Sqrt(2);
|
|
} |