Inna/CypherCore
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
91f750e562bc525c23d9041ca979b165f83c7389
CypherCore/Source/Framework/Util/MathFunctions.cs
T

379 lines
12 KiB
C#
Raw Blame History

// Copyright (c) CypherCore <http://github.com/CypherCore> All rights reserved.
// Licensed under the GNU GENERAL PUBLIC LICENSE. See LICENSE file in the project root for full license information.
using Framework.Constants;
using Framework.GameMath;
using System;
using System.Collections.Generic;
using System.Numerics;
public static class MathFunctions
{
public const float E = 2.71828f;
public const float Log10E = 0.434294f;
public const float Log2E = 1.4427f;
public const float PI = 3.14159f;
public const float PiOver2 = 1.5708f;
public const float PiOver4 = 0.785398f;
public const float TwoPi = 6.28319f;
public const float Epsilon = 4.76837158203125E-7f;
public static float wrap(float t, float lo, float hi)
{
if ((t >= lo) && (t < hi))
{
return t;
}
float interval = hi - lo;
return (float)(t - interval * Math.Floor((t - lo) / interval));
}
public static void Swap<T>(ref T lhs, ref T rhs)
{
T temp = lhs;
lhs = rhs;
rhs = temp;
}
#region Clamp
/// <summary>
/// Clamp a <paramref name="value"/> to <paramref name="calmpedValue"/> if it is withon the <paramref name="tolerance"/> range.
/// </summary>
/// <param name="value">The value to clamp.</param>
/// <param name="calmpedValue">The clamped value.</param>
/// <param name="tolerance">The tolerance value.</param>
/// <returns>
/// Returns the clamped value.
/// result = (tolerance > Abs(value-calmpedValue)) ? calmpedValue : value;
/// </returns>
public static float Clamp(float value, float calmpedValue, float tolerance)
{
return (tolerance > Math.Abs(value - calmpedValue)) ? calmpedValue : value;
}
/// <summary>
/// Clamp a <paramref name="value"/> to <paramref name="calmpedValue"/> using the default tolerance value.
/// </summary>
/// <param name="value">The value to clamp.</param>
/// <param name="calmpedValue">The clamped value.</param>
/// <returns>
/// Returns the clamped value.
/// result = (EpsilonF > Abs(value-calmpedValue)) ? calmpedValue : value;
/// </returns>
/// <remarks><see cref="MathFunctions.Epsilon"/> is used for tolerance.</remarks>
public static float Clamp(float value, float calmpedValue)
{
return (Epsilon > Math.Abs(value - calmpedValue)) ? calmpedValue : value;
}
#endregion
static double eps(float a, float b)
{
float aa = Math.Abs(a) + 1.0f;
if (float.IsPositiveInfinity(aa))
return 0.0000005f;
return 0.0000005f * aa;
}
public static float Lerp(float a, float b, float f)
{
return a + (b - a) * f;
}
public static float DegToRad(float degrees)
{
return degrees * (2.0f * PI / 360.0f);
}
#region Fuzzy
public static bool fuzzyEq(float a, float b)
{
return (a == b) || (Math.Abs(a - b) <= eps(a, b));
}
public static bool fuzzyGt(float a, float b)
{
return a > b + eps(a, b);
}
public static bool fuzzyLt(float a, float b)
{
return a < b - eps(a, b);
}
public static bool fuzzyNe(float a, float b)
{
return !fuzzyEq(a, b);
}
public static bool fuzzyLe(float a, float b)
{
return a < b + eps(a, b);
}
public static bool fuzzyGe(float a, float b)
{
return a > b - eps(a, b);
}
#endregion
public static int ApplyPct(ref int Base, float pct)
{
return Base = CalculatePct(Base, pct);
}
public static uint ApplyPct(ref uint Base, float pct)
{
return Base = CalculatePct(Base, pct);
}
public static float ApplyPct(ref float Base, float pct)
{
return Base = CalculatePct(Base, pct);
}
public static long AddPct(ref long value, float pct)
{
return value += (long)CalculatePct(value, pct);
}
public static int AddPct(ref int value, float pct)
{
return value += CalculatePct(value, pct);
}
public static uint AddPct(ref uint value, float pct)
{
return value += CalculatePct(value, pct);
}
public static float AddPct(ref float value, float pct)
{
return value += CalculatePct(value, pct);
}
public static int CalculatePct(int value, float pct)
{
return (int)(value * Convert.ToSingle(pct) / 100.0f);
}
public static uint CalculatePct(uint value, float pct)
{
return (uint)(value * Convert.ToSingle(pct) / 100.0f);
}
public static float CalculatePct(float value, float pct)
{
return value * pct / 100.0f;
}
public static ulong CalculatePct(ulong value, float pct)
{
return (ulong)(value * pct / 100.0f);
}
public static float GetPctOf(float value, float max)
{
return value / max * 100.0f;
}
public static int RoundToInterval(ref int num, dynamic floor, dynamic ceil)
{
return num = (int)Math.Min(Math.Max(num, floor), ceil);
}
public static uint RoundToInterval(ref uint num, dynamic floor, dynamic ceil)
{
return num = Math.Min(Math.Max(num, floor), ceil);
}
public static float RoundToInterval(ref float num, dynamic floor, dynamic ceil)
{
return num = Math.Min(Math.Max(num, floor), ceil);
}
public static void ApplyPercentModFloatVar(ref float value, float val, bool apply)
{
if (val == -100.0f) // prevent set var to zero
val = -99.99f;
value *= (apply ? (100.0f + val) / 100.0f : 100.0f / (100.0f + val));
}
public static bool CompareValues(ComparisionType type, uint val1, uint val2)
{
switch (type)
{
case ComparisionType.EQ:
return val1 == val2;
case ComparisionType.High:
return val1 > val2;
case ComparisionType.Low:
return val1 < val2;
case ComparisionType.HighEQ:
return val1 >= val2;
case ComparisionType.LowEQ:
return val1 <= val2;
default:
// incorrect parameter
Cypher.Assert(false);
return false;
}
}
public static bool CompareValues(ComparisionType type, float val1, float val2)
{
switch (type)
{
case ComparisionType.EQ:
return val1 == val2;
case ComparisionType.High:
return val1 > val2;
case ComparisionType.Low:
return val1 < val2;
case ComparisionType.HighEQ:
return val1 >= val2;
case ComparisionType.LowEQ:
return val1 <= val2;
default:
// incorrect parameter
Cypher.Assert(false);
return false;
}
}
public static ulong MakePair64(uint l, uint h)
{
return (ulong)l | ((ulong)h << 32);
}
public static uint Pair64_HiPart(ulong x)
{
return (uint)((x >> 32) & 0x00000000FFFFFFFF);
}
public static uint Pair64_LoPart(ulong x)
{
return (uint)(x & 0x00000000FFFFFFFF);
}
public static ushort Pair32_HiPart(uint x)
{
return (ushort)((x >> 16) & 0x0000FFFF);
}
public static ushort Pair32_LoPart(uint x)
{
return (ushort)(x & 0x0000FFFF);
}
public static uint MakePair32(uint l, uint h)
{
return (ushort)l | (h << 16);
}
public static ushort MakePair16(uint l, uint h)
{
return (ushort)((byte)l | (ushort)h << 8);
}
public static double Variance(this IEnumerable<uint> source)
{
int n = 0;
double mean = 0;
double M2 = 0;
foreach (var x in source)
{
n = n + 1;
double delta = x - mean;
mean = mean + delta / n;
M2 += delta * (x - mean);
}
return M2 / (n - 1);
}
//3d math
public static Box toWorldSpace(Matrix4x4 rotation, Vector3 translation, Box box)
{
if (!box.isFinite())
return box;
Box outBox = box;
outBox._center = new(rotation.M11 * box._center.GetAt(0) + rotation.M12 * box._center.GetAt(1) + rotation.M13 * box._center.GetAt(2) + translation.GetAt(0),
rotation.M21 * box._center.GetAt(0) + rotation.M22 * box._center.GetAt(1) + rotation.M23 * box._center.GetAt(2) + translation.GetAt(1),
rotation.M31 * box._center.GetAt(0) + rotation.M32 * box._center.GetAt(1) + rotation.M33 * box._center.GetAt(2) + translation.GetAt(2));
for (int i = 0; i < 3; ++i)
outBox._edgeVector[i] = rotation.Multiply(box._edgeVector[i]);
outBox._area = box._area;
outBox._volume = box._volume;
return box;
}
public static Matrix4x4 Inverse(this Matrix4x4 elt)
{
Matrix4x4 kInverse;
elt.Inverse(out kInverse);
return kInverse;
}
public static bool Inverse(this Matrix4x4 elt, out Matrix4x4 rkInverse)
{
// Invert a 3x3 using cofactors. This is about 8 times faster than
// the Numerical Recipes code which uses Gaussian elimination.
rkInverse = new();
rkInverse.M11 = elt.M22 * elt.M33 -
elt.M23 * elt.M32;
rkInverse.M12 = elt.M13 * elt.M32 -
elt.M12 * elt.M33;
rkInverse.M13 = elt.M12 * elt.M23 -
elt.M13 * elt.M22;
rkInverse.M21 = elt.M23 * elt.M31 -
elt.M21 * elt.M33;
rkInverse.M22 = elt.M11 * elt.M33 -
elt.M13 * elt.M31;
rkInverse.M23 = elt.M13 * elt.M21 -
elt.M11 * elt.M23;
rkInverse.M31 = elt.M21 * elt.M32 -
elt.M22 * elt.M31;
rkInverse.M32 = elt.M12 * elt.M31 -
elt.M11 * elt.M32;
rkInverse.M33 = elt.M11 * elt.M22 -
elt.M12 * elt.M21;
float fDet =
elt.M11 * rkInverse.M11 +
elt.M12 * rkInverse.M21 +
elt.M13 * rkInverse.M31;
if (Math.Abs(fDet) <= float.Epsilon)
return false;
float fInvDet = 1.0f / fDet;
rkInverse.M11 *= fInvDet;
rkInverse.M12 *= fInvDet;
rkInverse.M13 *= fInvDet;
rkInverse.M21 *= fInvDet;
rkInverse.M22 *= fInvDet;
rkInverse.M23 *= fInvDet;
rkInverse.M31 *= fInvDet;
rkInverse.M32 *= fInvDet;
rkInverse.M33 *= fInvDet;
return true;
}
public static Matrix4x4 ToMatrix(this Quaternion _q)
{
// Implementation from Watt and Watt, pg 362
// See also http://www.flipcode.com/documents/matrfaq.html#Q54
Quaternion q = _q;
q *= 1.0f / MathF.Sqrt((q.X * q.X) + (q.Y * q.Y) + (q.Z * q.Z) + (q.W * q.W));
float xx = 2.0f * q.X * q.X;
float xy = 2.0f * q.X * q.Y;
float xz = 2.0f * q.X * q.Z;
float xw = 2.0f * q.X * q.W;
float yy = 2.0f * q.Y * q.Y;
float yz = 2.0f * q.Y * q.Z;
float yw = 2.0f * q.Y * q.W;
float zz = 2.0f * q.Z * q.Z;
float zw = 2.0f * q.Z * q.W;
return new Matrix4x4(1.0f - yy - zz, xy - zw, xz + yw, 0.0f,
xy + zw, 1.0f - xx - zz, yz - xw, 0.0f,
xz - yw, yz + xw, 1.0f - xx - yy, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f);
}
public static Vector3 Multiply(this Matrix4x4 elt, Vector3 v)
{
return new(elt.M11 * v.GetAt(0) + elt.M12 * v.GetAt(1) + elt.M13 * v.GetAt(2),
elt.M21 * v.GetAt(0) + elt.M22 * v.GetAt(1) + elt.M23 * v.GetAt(2),
elt.M31 * v.GetAt(0) + elt.M32 * v.GetAt(1) + elt.M33 * v.GetAt(2));
}
}
Reference in New Issue View Git Blame Copy Permalink