├── Editor.meta ├── Editor ├── FAnglePropertyDrawer.cs ├── FAnglePropertyDrawer.cs.meta ├── FPropertyDrawer.cs ├── FPropertyDrawer.cs.meta ├── FVector2PropertyDrawer.cs ├── FVector2PropertyDrawer.cs.meta ├── FVector3PropertyDrawer.cs ├── FVector3PropertyDrawer.cs.meta ├── FixedPoint.Editor.asmdef ├── FixedPoint.Editor.asmdef.meta ├── FixedPointStatsWindow.cs └── FixedPointStatsWindow.cs.meta ├── LICENSE ├── LICENSE.meta ├── README.md ├── README.md.meta ├── Runtime.meta ├── Runtime ├── Collisions.meta ├── Collisions │ ├── EPA.cs │ ├── EPA.cs.meta │ ├── GJK.cs │ ├── GJK.cs.meta │ ├── ISupportMappable.cs │ ├── ISupportMappable.cs.meta │ ├── MinkowskiDifference.cs │ └── MinkowskiDifference.cs.meta ├── Cordic.meta ├── Cordic │ ├── FCordic.Lut.cs │ ├── FCordic.Lut.cs.meta │ ├── FCordic.Unrolled.cs │ ├── FCordic.Unrolled.cs.meta │ ├── FCordic.cs │ └── FCordic.cs.meta ├── FConversions.cs ├── FConversions.cs.meta ├── FMath.Lut.cs ├── FMath.Lut.cs.meta ├── FMath.Raw.cs ├── FMath.Raw.cs.meta ├── FMath.Trig.cs ├── FMath.Trig.cs.meta ├── FMath.cs ├── FMath.cs.meta ├── FP.Constants.cs ├── FP.Constants.cs.meta ├── FP.Raw.cs ├── FP.Raw.cs.meta ├── FP.cs ├── FP.cs.meta ├── FixedPoint.asmdef ├── FixedPoint.asmdef.meta ├── IL2CPP.cs ├── IL2CPP.cs.meta ├── Structs.meta └── Structs │ ├── FAngle.cs │ ├── FAngle.cs.meta │ ├── FQuaternion.cs │ ├── FQuaternion.cs.meta │ ├── FRotation2D.cs │ ├── FRotation2D.cs.meta │ ├── FVector2.cs │ ├── FVector2.cs.meta │ ├── FVector3.cs │ └── FVector3.cs.meta ├── Tests.meta ├── Tests ├── FPTests.cs ├── FPTests.cs.meta ├── FQuaternionTests.cs ├── FQuaternionTests.cs.meta ├── FixedPoint.Tests.asmdef └── FixedPoint.Tests.asmdef.meta ├── package.json └── package.json.meta /Editor.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: d6c91c634926457cb8c030343b5fdbac 3 | timeCreated: 1737205585 -------------------------------------------------------------------------------- /Editor/FAnglePropertyDrawer.cs: -------------------------------------------------------------------------------- 1 | using UnityEditor; 2 | using UnityEngine; 3 | 4 | namespace Mathematics.Fixed.Editor 5 | { 6 | [CustomPropertyDrawer(typeof(FAngle))] 7 | public class FAnglePropertyDrawer : PropertyDrawer 8 | { 9 | public override float GetPropertyHeight(SerializedProperty property, GUIContent label) 10 | { 11 | return EditorGUIUtility.singleLineHeight; 12 | } 13 | 14 | public override void OnGUI(Rect position, SerializedProperty property, GUIContent label) 15 | { 16 | var valueProperty = property.FindPropertyRelative(nameof(FP.RawValue)); 17 | 18 | EditorGUI.BeginProperty(position, label, valueProperty); 19 | { 20 | var propertyValue = FP.FromRaw(valueProperty.longValue).ToFloat(); 21 | 22 | EditorGUI.BeginChangeCheck(); 23 | var newValue = EditorGUI.FloatField(position, new GUIContent(property.displayName), propertyValue); 24 | if (EditorGUI.EndChangeCheck()) 25 | { 26 | valueProperty.longValue = newValue.ToFP().RawValue; 27 | } 28 | } 29 | EditorGUI.EndProperty(); 30 | } 31 | } 32 | } 33 | -------------------------------------------------------------------------------- /Editor/FAnglePropertyDrawer.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: b94342b420cb4608a7d9848321446e43 3 | timeCreated: 1737916339 -------------------------------------------------------------------------------- /Editor/FPropertyDrawer.cs: -------------------------------------------------------------------------------- 1 | using UnityEditor; 2 | using UnityEngine; 3 | 4 | namespace Mathematics.Fixed.Editor 5 | { 6 | [CustomPropertyDrawer(typeof(FP))] 7 | public class FPropertyDrawer : PropertyDrawer 8 | { 9 | public override float GetPropertyHeight(SerializedProperty property, GUIContent label) 10 | { 11 | return EditorGUIUtility.singleLineHeight; 12 | } 13 | 14 | public override void OnGUI(Rect position, SerializedProperty property, GUIContent label) 15 | { 16 | var valueProperty = property.FindPropertyRelative(nameof(FP.RawValue)); 17 | 18 | EditorGUI.BeginProperty(position, label, valueProperty); 19 | { 20 | var propertyValue = FP.FromRaw(valueProperty.longValue).ToFloat(); 21 | 22 | EditorGUI.BeginChangeCheck(); 23 | var newValue = EditorGUI.FloatField(position, new GUIContent(property.displayName), propertyValue); 24 | if (EditorGUI.EndChangeCheck()) 25 | { 26 | valueProperty.longValue = newValue.ToFP().RawValue; 27 | } 28 | } 29 | EditorGUI.EndProperty(); 30 | } 31 | } 32 | } 33 | -------------------------------------------------------------------------------- /Editor/FPropertyDrawer.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: b520afaeed4a4228b810764cea6cef47 3 | timeCreated: 1737205729 -------------------------------------------------------------------------------- /Editor/FVector2PropertyDrawer.cs: -------------------------------------------------------------------------------- 1 | using UnityEditor; 2 | using UnityEngine; 3 | 4 | namespace Mathematics.Fixed.Editor 5 | { 6 | [CustomPropertyDrawer(typeof(FVector2))] 7 | public class FVector2PropertyDrawer : PropertyDrawer 8 | { 9 | public override float GetPropertyHeight(SerializedProperty property, GUIContent label) 10 | { 11 | return EditorGUIUtility.singleLineHeight; 12 | } 13 | 14 | public override void OnGUI(Rect position, SerializedProperty property, GUIContent label) 15 | { 16 | var xProperty = property.FindPropertyRelative(nameof(FVector2.X)).FindPropertyRelative(nameof(FP.RawValue)); 17 | var yProperty = property.FindPropertyRelative(nameof(FVector2.Y)).FindPropertyRelative(nameof(FP.RawValue)); 18 | 19 | EditorGUI.BeginProperty(position, label, property); 20 | { 21 | var xValue = FP.FromRaw(xProperty.longValue).ToFloat(); 22 | var yValue = FP.FromRaw(yProperty.longValue).ToFloat(); 23 | 24 | bool wideMode = EditorGUIUtility.wideMode; 25 | EditorGUIUtility.wideMode = true; 26 | 27 | EditorGUI.BeginChangeCheck(); 28 | var newValue = EditorGUI.Vector2Field(position, new GUIContent(property.displayName), new Vector2(xValue, yValue)); 29 | if (EditorGUI.EndChangeCheck()) 30 | { 31 | xProperty.longValue = newValue.x.ToFP().RawValue; 32 | yProperty.longValue = newValue.y.ToFP().RawValue; 33 | } 34 | 35 | EditorGUIUtility.wideMode = wideMode; 36 | } 37 | EditorGUI.EndProperty(); 38 | } 39 | } 40 | } 41 | -------------------------------------------------------------------------------- /Editor/FVector2PropertyDrawer.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 39332f3d51a948dd9984a9b7d948a268 3 | timeCreated: 1737215545 -------------------------------------------------------------------------------- /Editor/FVector3PropertyDrawer.cs: -------------------------------------------------------------------------------- 1 | using UnityEditor; 2 | using UnityEngine; 3 | 4 | namespace Mathematics.Fixed.Editor 5 | { 6 | [CustomPropertyDrawer(typeof(FVector3))] 7 | public class FVector3PropertyDrawer : PropertyDrawer 8 | { 9 | public override float GetPropertyHeight(SerializedProperty property, GUIContent label) 10 | { 11 | return EditorGUIUtility.singleLineHeight; 12 | } 13 | 14 | public override void OnGUI(Rect position, SerializedProperty property, GUIContent label) 15 | { 16 | var xProperty = property.FindPropertyRelative(nameof(FVector3.X)).FindPropertyRelative(nameof(FP.RawValue)); 17 | var yProperty = property.FindPropertyRelative(nameof(FVector3.Y)).FindPropertyRelative(nameof(FP.RawValue)); 18 | var zProperty = property.FindPropertyRelative(nameof(FVector3.Z)).FindPropertyRelative(nameof(FP.RawValue)); 19 | 20 | EditorGUI.BeginProperty(position, label, property); 21 | { 22 | var xValue = FP.FromRaw(xProperty.longValue).ToFloat(); 23 | var yValue = FP.FromRaw(yProperty.longValue).ToFloat(); 24 | var zValue = FP.FromRaw(zProperty.longValue).ToFloat(); 25 | 26 | bool wideMode = EditorGUIUtility.wideMode; 27 | EditorGUIUtility.wideMode = true; 28 | 29 | EditorGUI.BeginChangeCheck(); 30 | var newValue = EditorGUI.Vector3Field(position, new GUIContent(property.displayName), new Vector3(xValue, yValue, zValue)); 31 | if (EditorGUI.EndChangeCheck()) 32 | { 33 | xProperty.longValue = newValue.x.ToFP().RawValue; 34 | yProperty.longValue = newValue.y.ToFP().RawValue; 35 | zProperty.longValue = newValue.z.ToFP().RawValue; 36 | } 37 | 38 | EditorGUIUtility.wideMode = wideMode; 39 | } 40 | EditorGUI.EndProperty(); 41 | } 42 | } 43 | } 44 | -------------------------------------------------------------------------------- /Editor/FVector3PropertyDrawer.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: e0b0ce55611243dba9dac48c5b4eee2d 3 | timeCreated: 1737209525 -------------------------------------------------------------------------------- /Editor/FixedPoint.Editor.asmdef: -------------------------------------------------------------------------------- 1 | { 2 | "name": "FixedPoint.Editor", 3 | "rootNamespace": "", 4 | "references": [ 5 | "GUID:401db8312f319734b8ce2e50f0180af2", 6 | "GUID:343deaaf83e0cee4ca978e7df0b80d21" 7 | ], 8 | "includePlatforms": [ 9 | "Editor" 10 | ], 11 | "excludePlatforms": [], 12 | "allowUnsafeCode": false, 13 | "overrideReferences": false, 14 | "precompiledReferences": [], 15 | "autoReferenced": true, 16 | "defineConstraints": [], 17 | "versionDefines": [], 18 | "noEngineReferences": false 19 | } -------------------------------------------------------------------------------- /Editor/FixedPoint.Editor.asmdef.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 601f2c247f5e8244bacdb024bd13eec5 3 | AssemblyDefinitionImporter: 4 | externalObjects: {} 5 | userData: 6 | assetBundleName: 7 | assetBundleVariant: 8 | -------------------------------------------------------------------------------- /Editor/FixedPointStatsWindow.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using UnityEditor; 3 | using UnityEngine; 4 | 5 | namespace Mathematics.Fixed.Editor 6 | { 7 | public class FixedPointStatsWindow : EditorWindow 8 | { 9 | [SerializeField] private float _testAngle = 30; 10 | [SerializeField, Range(-1, 1)] private float _testValue = 0; 11 | 12 | private SerializedObject _serializedObject; 13 | private Vector2 _scroll; 14 | 15 | [MenuItem("Window/Fixed Point/Stats")] 16 | public static void ShowWindow() 17 | { 18 | FixedPointStatsWindow wnd = GetWindow(); 19 | wnd.titleContent = new GUIContent("FP Stats"); 20 | } 21 | 22 | private void OnEnable() 23 | { 24 | _serializedObject = new SerializedObject(this); 25 | } 26 | 27 | private void OnGUI() 28 | { 29 | using var scrollscope = new EditorGUILayout.ScrollViewScope(_scroll, false, true); 30 | _scroll = scrollscope.scrollPosition; 31 | 32 | if (GUILayout.Button("Log CORDIC constants")) 33 | { 34 | string sum = string.Empty; 35 | for (int i = 0; i < 64; ++i) 36 | { 37 | var result = Math.Atan(1.0 / Math.Pow(2, i)); 38 | sum += (long)(result * (1UL << 63)) + ",\n"; 39 | } 40 | Debug.Log(sum); 41 | 42 | double cos = 1f; 43 | for (int i = 0; i < 64; ++i) 44 | { 45 | var result = Math.Atan(1.0 / Math.Pow(2, i)); 46 | cos *= Math.Cos(result); 47 | } 48 | Debug.Log((long)(cos * (1UL << 63))); 49 | } 50 | 51 | const float log10Of2 = 0.30103f; 52 | int decimalDigitsOfAccuracy = Mathf.CeilToInt(log10Of2 * FP.FractionalBits); 53 | 54 | _serializedObject.Update(); 55 | 56 | EditorGUILayout.Space(5f); 57 | 58 | EditorGUILayout.TextField("Fractional Places", FP.FractionalBits.ToString()); 59 | EditorGUILayout.TextField("Integer Places", FP.IntegerBits.ToString()); 60 | EditorGUILayout.Space(10f); 61 | 62 | EditorGUILayout.TextField("Max Integer Value", (FMath.Floor(FP.MaxValue).ToLong()).ToString()); 63 | EditorGUILayout.TextField("Min Integer Value", (FMath.Floor(FP.MinValue).ToLong()).ToString()); 64 | EditorGUILayout.TextField("Max Integer To Sqr", (FMath.Sqrt(FP.MaxValue).ToLong()).ToString()); 65 | EditorGUILayout.Space(5f); 66 | EditorGUILayout.TextField("Absolute Epsilon ", FP.Epsilon.ToString("G5")); 67 | EditorGUILayout.TextField("Calculations Epsilon ", FP.CalculationsEpsilon.ToString("G5")); 68 | EditorGUILayout.TextField("Calculations Epsilon Sqr ", FP.CalculationsEpsilonSqr.ToString("G5")); 69 | 70 | EditorGUILayout.Space(10f); 71 | EditorGUILayout.TextField("Pi", FP.Pi.ToString("F" + decimalDigitsOfAccuracy)); 72 | EditorGUILayout.TextField("One Degrees In Rad", FP.Deg2Rad.ToString("F" + decimalDigitsOfAccuracy)); 73 | 74 | EditorGUILayout.Space(10f); 75 | 76 | EditorGUILayout.PropertyField(_serializedObject.FindProperty(nameof(_testAngle)), new GUIContent("Test Angle")); 77 | _serializedObject.ApplyModifiedProperties(); 78 | var testFp = FP.Deg2Rad * _testAngle.ToFP(); 79 | var testRadians = 0.017453292519943295 * _testAngle; 80 | 81 | FCordic.SinCos(testFp.RawValue, out var sinRaw, out var cosRaw); 82 | var sinFP = FP.FromRaw(sinRaw); 83 | var cosFP = FP.FromRaw(cosRaw); 84 | 85 | EditorGUILayout.TextField("Sin", sinFP.ToString("F" + decimalDigitsOfAccuracy)); 86 | EditorGUILayout.TextField("Actual Sin", Math.Sin(testRadians).ToString("F" + decimalDigitsOfAccuracy)); 87 | EditorGUILayout.TextField("Delta", Math.Abs(sinFP.ToDouble() - Math.Sin(testRadians)).ToString("G5")); 88 | EditorGUILayout.Space(2f); 89 | EditorGUILayout.TextField("Cos", (cosFP.ToDouble()).ToString("G5")); 90 | EditorGUILayout.TextField("Actual Cos", Math.Cos(testRadians).ToString("G5")); 91 | EditorGUILayout.TextField("Delta", Math.Abs(cosFP.ToDouble() - Math.Cos(testRadians)).ToString("G5")); 92 | EditorGUILayout.Space(2f); 93 | 94 | var tanFP = FP.FromRaw(FCordic.Tan(testFp.RawValue)); 95 | EditorGUILayout.TextField("Tan", tanFP.ToDouble().ToString("G5")); 96 | EditorGUILayout.TextField("Actual Tan", Math.Tan(testRadians).ToString("G5")); 97 | EditorGUILayout.TextField("Delta", Math.Abs(tanFP.ToDouble() - Math.Tan(testRadians)).ToString("G5")); 98 | 99 | EditorGUILayout.Space(5f); 100 | EditorGUILayout.TextField("Sin (MaxValue)", FMath.Sin(FP.MaxValue).ToDouble().ToString("G5")); 101 | EditorGUILayout.TextField("Actual Sin (MaxValue)", Math.Sin(FP.MaxValue.ToDouble()).ToString("G5")); 102 | EditorGUILayout.TextField("Delta", Math.Abs(FMath.Sin(FP.MaxValue).ToDouble() - Math.Sin(FP.MaxValue.ToDouble())).ToString("G5")); 103 | EditorGUILayout.Space(5f); 104 | EditorGUILayout.TextField("Cos (MaxValue)", FMath.Cos(FP.MaxValue).ToDouble().ToString("G5")); 105 | EditorGUILayout.TextField("Actual Cos (MaxValue)", Math.Cos(FP.MaxValue.ToDouble()).ToString("G5")); 106 | EditorGUILayout.TextField("Delta", Math.Abs(FMath.Cos(FP.MaxValue).ToDouble() - Math.Cos(FP.MaxValue.ToDouble())).ToString("G5")); 107 | 108 | EditorGUILayout.PropertyField(_serializedObject.FindProperty(nameof(_testValue)), new GUIContent("Test Value")); 109 | _serializedObject.ApplyModifiedProperties(); 110 | var testValueFp = _testValue.ToFP(); 111 | 112 | var asinFP = FMath.Asin(testValueFp); 113 | EditorGUILayout.TextField("Arcsin", asinFP.ToString("F" + decimalDigitsOfAccuracy)); 114 | EditorGUILayout.TextField("Actual Arcsin", Math.Asin(_testValue).ToString("F" + decimalDigitsOfAccuracy)); 115 | EditorGUILayout.TextField("Delta", Math.Abs(asinFP.ToDouble() - Math.Asin(_testValue)).ToString("G5")); 116 | var atanFP = FMath.Atan(testValueFp); 117 | EditorGUILayout.TextField("Arctan", atanFP.ToString("F" + decimalDigitsOfAccuracy)); 118 | EditorGUILayout.TextField("Actual Arctan", Math.Atan(_testValue).ToString("F" + decimalDigitsOfAccuracy)); 119 | EditorGUILayout.TextField("Delta", Math.Abs(atanFP.ToDouble() - Math.Atan(_testValue)).ToString("G5")); 120 | } 121 | } 122 | } 123 | -------------------------------------------------------------------------------- /Editor/FixedPointStatsWindow.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 59931142bbaa4e1cb3915b2bd98807d5 3 | timeCreated: 1737306858 -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 Daniil Pankevich 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /LICENSE.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 9af3fe2d73dc98f46ae6628c49f9d487 3 | DefaultImporter: 4 | externalObjects: {} 5 | userData: 6 | assetBundleName: 7 | assetBundleVariant: 8 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Fixed Point 2 | 3 | Deterministic fixed-point math library designed for online multiplayer games and other use cases. 4 | Can be used as a pure C# library or as a Unity package. 5 | 6 | > [!WARNING] 7 | > This project is in the early stages of development and lacks some core functionality. It is not yet suitable for production use. 8 | 9 | ## Overview 10 | 11 | This library is based on [FixedMath.Net](https://github.com/asik/FixedMath.Net) but has been rewritten without hardcoded magic numbers that were tied to a specific scaling factor. 12 | 13 | ### Key Features 14 | 15 | - The number of fractional bits can be controlled using a single constant. 16 | - LUTs are generated deterministically at runtime with adjustable precision and size. 17 | - Avoids static initializers, preventing IL2CPP from introducing unnecessary static guards. 18 | -------------------------------------------------------------------------------- /README.md.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 8ceb8aa11851a81439c90dabfe1c9a38 3 | TextScriptImporter: 4 | externalObjects: {} 5 | userData: 6 | assetBundleName: 7 | assetBundleVariant: 8 | -------------------------------------------------------------------------------- /Runtime.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: ac60a2c0f7c39e849aa630d6e2bdf3bf 3 | folderAsset: yes 4 | DefaultImporter: 5 | externalObjects: {} 6 | userData: 7 | assetBundleName: 8 | assetBundleVariant: 9 | -------------------------------------------------------------------------------- /Runtime/Collisions.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 7377c2d7df4b45f3894e8002fa2e5f98 3 | timeCreated: 1738330966 -------------------------------------------------------------------------------- /Runtime/Collisions/EPA.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Collections.Generic; 3 | 4 | namespace Mathematics.Fixed 5 | { 6 | public readonly struct Collision 7 | { 8 | public ContactPoint ContactFirst { get; } 9 | 10 | public ContactPoint ContactSecond { get; } 11 | 12 | public FVector3 PenetrationNormal { get; } 13 | 14 | public FP PenetrationDepth { get; } 15 | 16 | public Collision(ContactPoint contactFirst, ContactPoint contactSecond, FVector3 penetrationNormal, FP penetrationDepth) 17 | { 18 | ContactFirst = contactFirst; 19 | PenetrationNormal = penetrationNormal; 20 | PenetrationDepth = penetrationDepth; 21 | ContactSecond = contactSecond; 22 | } 23 | } 24 | 25 | public struct ContactPoint 26 | { 27 | public FVector3 Position { get; } 28 | 29 | public ContactPoint(FVector3 position) 30 | { 31 | Position = position; 32 | } 33 | } 34 | 35 | public static class EPA 36 | { 37 | private static FP Tolerance => FP.CalculationsEpsilon; 38 | private static FP NormalBias => FP.CalculationsEpsilonSqr; 39 | 40 | public static List Vertices { get; } = new List(); 41 | public static List Faces { get; } = new List(); 42 | private static List LooseEdges { get; } = new List(); 43 | 44 | public static FVector3 Barycentric(FVector3 a, FVector3 b, FVector3 c, FVector3 point, bool clamp = false) 45 | { 46 | var v0 = b - a; 47 | var v1 = c - a; 48 | var v2 = point - a; 49 | var d00 = FVector3.Dot(v0, v0); 50 | var d01 = FVector3.Dot(v0, v1); 51 | var d11 = FVector3.Dot(v1, v1); 52 | var d20 = FVector3.Dot(v2, v0); 53 | var d21 = FVector3.Dot(v2, v1); 54 | var denominator = d00 * d11 - d01 * d01; 55 | var v = (d11 * d20 - d01 * d21) / denominator; 56 | var w = (d00 * d21 - d01 * d20) / denominator; 57 | var u = FP.One - v - w; 58 | 59 | return new FVector3(u, v, w); 60 | } 61 | 62 | public struct PolytopeFace 63 | { 64 | public int A; 65 | public int B; 66 | public int C; 67 | public FVector3 Normal; 68 | 69 | public PolytopeFace(int a, int b, int c, FVector3 normal) 70 | { 71 | A = a; 72 | B = b; 73 | C = c; 74 | Normal = normal; 75 | } 76 | } 77 | 78 | public struct PolytopeEdge 79 | { 80 | public int A; 81 | public int B; 82 | 83 | public PolytopeEdge(int a, int b) 84 | { 85 | A = a; 86 | B = b; 87 | } 88 | 89 | public void Deconstruct(out int a, out int b) 90 | { 91 | a = A; 92 | b = B; 93 | } 94 | } 95 | 96 | public struct ClosestFace 97 | { 98 | public FP Distance; 99 | public PolytopeFace Face; 100 | 101 | public ClosestFace(FP distance, PolytopeFace face) 102 | { 103 | Distance = distance; 104 | Face = face; 105 | } 106 | } 107 | 108 | public static Collision Calculate(Simplex simplex, T shapeA, 109 | T shapeB, int maxIterations = 100) where T : ISupportMappable 110 | { 111 | Faces.Clear(); 112 | Vertices.Clear(); 113 | 114 | Vertices.Add(simplex.A); 115 | Vertices.Add(simplex.B); 116 | Vertices.Add(simplex.C); 117 | Vertices.Add(simplex.D); 118 | 119 | Faces.Add(new PolytopeFace(0, 1, 2, CalculateFaceNormal(simplex.A.Difference, simplex.B.Difference, simplex.C.Difference))); 120 | Faces.Add(new PolytopeFace(0, 2, 3, CalculateFaceNormal(simplex.A.Difference, simplex.C.Difference, simplex.D.Difference))); 121 | Faces.Add(new PolytopeFace(0, 3, 1, CalculateFaceNormal(simplex.A.Difference, simplex.D.Difference, simplex.B.Difference))); 122 | Faces.Add(new PolytopeFace(1, 3, 2, CalculateFaceNormal(simplex.B.Difference, simplex.D.Difference, simplex.C.Difference))); 123 | 124 | ClosestFace closestFace = default; 125 | 126 | var iteration = 1; 127 | for (var i = 0; i < maxIterations; i++) 128 | { 129 | closestFace = FindClosestFace(Faces); 130 | 131 | var searchDirection = closestFace.Face.Normal; 132 | var supportPoint = MinkowskiDifference.Calculate(shapeA, shapeB, searchDirection); 133 | 134 | var minkowskiDistance = FVector3.Dot(supportPoint.Difference, searchDirection); 135 | if (minkowskiDistance - closestFace.Distance < Tolerance) 136 | { 137 | break; 138 | } 139 | 140 | Vertices.Add(supportPoint); 141 | 142 | ExpandPolytope(supportPoint); 143 | } 144 | 145 | if (iteration >= maxIterations) 146 | { 147 | throw new Exception(); 148 | } 149 | 150 | var barycentric = Barycentric( 151 | Vertices[closestFace.Face.A].Difference, 152 | Vertices[closestFace.Face.B].Difference, 153 | Vertices[closestFace.Face.C].Difference, 154 | closestFace.Face.Normal * closestFace.Distance); 155 | 156 | var supportAA = Vertices[closestFace.Face.A].SupportA; 157 | var supportAB = Vertices[closestFace.Face.B].SupportA; 158 | var supportAC = Vertices[closestFace.Face.C].SupportA; 159 | var supportBA = Vertices[closestFace.Face.A].SupportB; 160 | var supportBB = Vertices[closestFace.Face.B].SupportB; 161 | var supportBC = Vertices[closestFace.Face.C].SupportB; 162 | 163 | var point1 = barycentric.X * supportAA + barycentric.Y * supportAB + barycentric.Z * supportAC; 164 | var point2 = barycentric.X * supportBA + barycentric.Y * supportBB + barycentric.Z * supportBC; 165 | 166 | return new Collision(new ContactPoint(point1), new ContactPoint(point2), closestFace.Face.Normal, closestFace.Distance + Tolerance); 167 | } 168 | 169 | public static void ExpandPolytope(MinkowskiDifference supportPoint) 170 | { 171 | LooseEdges.Clear(); 172 | 173 | for (var i = 0; i < Faces.Count; i++) 174 | { 175 | var face = Faces[i]; 176 | 177 | if (FVector3.Dot(face.Normal, supportPoint.Difference - Vertices[face.A].Difference) > NormalBias) 178 | { 179 | var edgeAB = new PolytopeEdge(face.A, face.B); 180 | var edgeBC = new PolytopeEdge(face.B, face.C); 181 | var edgeCA = new PolytopeEdge(face.C, face.A); 182 | 183 | RemoveIfExistsOrAdd(LooseEdges, edgeAB); 184 | RemoveIfExistsOrAdd(LooseEdges, edgeBC); 185 | RemoveIfExistsOrAdd(LooseEdges, edgeCA); 186 | 187 | Faces.RemoveAt(i); 188 | i -= 1; 189 | } 190 | } 191 | 192 | var c = Vertices.Count - 1; 193 | foreach (var (a, b) in LooseEdges) 194 | { 195 | var vA = Vertices[a].Difference; 196 | var vB = Vertices[b].Difference; 197 | var vC = Vertices[c].Difference; 198 | 199 | var face = new PolytopeFace(a, b, c, FVector3.Normalize(FVector3.Cross(vA - vB, vA - vC))); 200 | 201 | if (FVector3.Dot(vA, face.Normal) < -NormalBias) 202 | { 203 | (face.A, face.B) = (face.B, face.A); 204 | face.Normal = -face.Normal; 205 | } 206 | 207 | Faces.Add(face); 208 | } 209 | } 210 | 211 | private static ClosestFace FindClosestFace(List faces) 212 | { 213 | var closest = new ClosestFace(FP.MaxValue, default); 214 | 215 | for (var i = 0; i < faces.Count; i++) 216 | { 217 | var face = faces[i]; 218 | var distance = FVector3.Dot(Vertices[face.A].Difference, face.Normal); 219 | 220 | if (distance < closest.Distance) 221 | { 222 | closest.Distance = distance; 223 | closest.Face = face; 224 | } 225 | } 226 | 227 | return closest; 228 | } 229 | 230 | private static FVector3 CalculateFaceNormal(FVector3 a, FVector3 b, FVector3 c) 231 | { 232 | var ab = b - a; 233 | var ac = c - a; 234 | return FVector3.Normalize(FVector3.Cross(ab, ac)); 235 | } 236 | 237 | private static void RemoveIfExistsOrAdd(List edges, PolytopeEdge edge) 238 | { 239 | var edgeIndex = -1; 240 | 241 | for (var index = 0; index < edges.Count; index++) 242 | { 243 | var pair = edges[index]; 244 | 245 | if (pair.A.Equals(edge.B) && pair.B.Equals(edge.A)) 246 | { 247 | edgeIndex = index; 248 | break; 249 | } 250 | } 251 | 252 | if (edgeIndex != -1) 253 | { 254 | edges.RemoveAt(edgeIndex); 255 | } 256 | else 257 | { 258 | edges.Add(edge); 259 | } 260 | } 261 | } 262 | } 263 | -------------------------------------------------------------------------------- /Runtime/Collisions/EPA.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 90b260cd1bd8487e914684f3ddbe2dc0 3 | timeCreated: 1738330974 -------------------------------------------------------------------------------- /Runtime/Collisions/GJK.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Runtime.CompilerServices; 3 | 4 | namespace Mathematics.Fixed 5 | { 6 | public struct Simplex 7 | { 8 | public MinkowskiDifference A; 9 | public MinkowskiDifference B; 10 | public MinkowskiDifference C; 11 | public MinkowskiDifference D; 12 | 13 | public int Stage; 14 | } 15 | 16 | public static class GJK 17 | { 18 | private static FP Tolerance => FP.CalculationsEpsilon; 19 | 20 | public struct Result 21 | { 22 | public bool CollisionHappened { get; } 23 | 24 | public Simplex Simplex { get; } 25 | 26 | public int Iterations { get; } 27 | 28 | public FVector3 Direction { get; } 29 | 30 | public Result(bool collisionHappened, Simplex simplex, int iterations, FVector3 direction) 31 | { 32 | CollisionHappened = collisionHappened; 33 | Simplex = simplex; 34 | Iterations = iterations; 35 | Direction = direction; 36 | } 37 | } 38 | 39 | public static Result Calculate(T shapeA, T shapeB, int maxIterations = 100) where T : ISupportMappable 40 | { 41 | var simplex = new Simplex(); 42 | 43 | var direction = NormalizeSafe(shapeB.Centre - shapeA.Centre, FVector3.Up); 44 | 45 | var colliding = false; 46 | var iterations = 1; 47 | while (iterations < maxIterations) 48 | { 49 | var supportPoint = MinkowskiDifference.Calculate(shapeA, shapeB, direction); 50 | 51 | simplex.D = simplex.C; 52 | simplex.C = simplex.B; 53 | simplex.B = simplex.A; 54 | simplex.A = supportPoint; 55 | 56 | if (FVector3.Dot(supportPoint.Difference, direction) <= FP.Zero) 57 | { 58 | break; 59 | } 60 | 61 | var encloseResult = TryEncloseOrigin(ref simplex, shapeA, shapeB, direction); 62 | 63 | if (encloseResult.EncloseOrigin) 64 | { 65 | colliding = true; 66 | break; 67 | } 68 | 69 | direction = encloseResult.NextDirection; 70 | simplex.Stage += 1; 71 | iterations += 1; 72 | } 73 | 74 | if (iterations >= maxIterations) 75 | { 76 | throw new Exception(); 77 | } 78 | 79 | return new Result(colliding, simplex, iterations, direction); 80 | } 81 | 82 | private static FVector3 NormalizeSafe(FVector3 vector, FVector3 defaultValue) 83 | { 84 | var result = FVector3.Normalize(vector); 85 | 86 | if (result.Equals(FVector3.Zero)) 87 | { 88 | return defaultValue; 89 | } 90 | 91 | return result; 92 | } 93 | 94 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 95 | private static FVector3 TripleProduct(FVector3 a, FVector3 b, FVector3 c) 96 | { 97 | return FVector3.Cross(FVector3.Cross(a, b), c); 98 | } 99 | 100 | private static (bool EncloseOrigin, FVector3 NextDirection) TryEncloseOrigin(ref Simplex simplex, 101 | T shapeA, T shapeB, FVector3 direction) where T : ISupportMappable 102 | { 103 | switch (simplex.Stage) 104 | { 105 | case 0: 106 | { 107 | direction = NormalizeSafe(shapeB.Centre - shapeA.Centre, FVector3.Up); 108 | break; 109 | } 110 | case 1: 111 | { 112 | // Flip the direction 113 | direction = -direction; 114 | break; 115 | } 116 | case 2: 117 | { 118 | // Line ab is the line formed by the first two vertices 119 | var ab = simplex.B.Difference - simplex.A.Difference; 120 | // Line a0 is the line from the first vertex to the origin 121 | var a0 = -simplex.A.Difference; 122 | 123 | if (FVector3.Cross(ab, a0) == FVector3.Zero) 124 | { 125 | direction = FVector3.Orthonormal(ab); 126 | } 127 | else 128 | { 129 | // Use the triple-cross-product to calculate a direction perpendicular 130 | // To line ab in the direction of the origin 131 | direction = TripleProduct(ab, a0, ab); 132 | } 133 | break; 134 | } 135 | case 3: 136 | { 137 | var ab = simplex.B.Difference - simplex.A.Difference; 138 | var ac = simplex.C.Difference - simplex.A.Difference; 139 | direction = FVector3.Cross(ab, ac); 140 | 141 | // Ensure it points toward the origin 142 | var a0 = -simplex.A.Difference; 143 | if (FVector3.Dot(direction, a0) < FP.Zero) 144 | direction = -direction; 145 | break; 146 | } 147 | case 4: 148 | { 149 | // Calculate edges of interest 150 | var ab = simplex.B.Difference - simplex.A.Difference; 151 | var ac = simplex.C.Difference - simplex.A.Difference; 152 | var ad = simplex.D.Difference - simplex.A.Difference; 153 | 154 | var bc = simplex.C.Difference - simplex.B.Difference; 155 | var bd = simplex.D.Difference - simplex.B.Difference; 156 | var ba = -ab; 157 | 158 | // ABC 159 | direction = FVector3.Normalize(FVector3.Cross(ab, ac)); 160 | if (FVector3.Dot(ad, direction) > FP.Zero) 161 | { 162 | direction = -direction; 163 | } 164 | if (FVector3.Dot(simplex.A.Difference, direction) < -Tolerance) 165 | { 166 | // Remove d 167 | simplex.Stage = 3; 168 | return (false, direction); 169 | } 170 | 171 | // ADB 172 | direction = FVector3.Normalize(FVector3.Cross(ab, ad)); 173 | if (FVector3.Dot(ac, direction) > FP.Zero) 174 | { 175 | direction = -direction; 176 | } 177 | if (FVector3.Dot(simplex.A.Difference, direction) < -Tolerance) 178 | { 179 | // Remove c 180 | simplex.C = simplex.D; 181 | simplex.Stage = 3; 182 | return (false, direction); 183 | } 184 | 185 | // ACD 186 | direction = FVector3.Normalize(FVector3.Cross(ac, ad)); 187 | if (FVector3.Dot(ab, direction) > FP.Zero) 188 | { 189 | direction = -direction; 190 | } 191 | if (FVector3.Dot(simplex.A.Difference, direction) < -Tolerance) 192 | { 193 | // Remove b 194 | simplex.B = simplex.C; 195 | simplex.C = simplex.D; 196 | simplex.Stage = 3; 197 | return (false, direction); 198 | } 199 | 200 | // BCD 201 | direction = FVector3.Normalize(FVector3.Cross(bc, bd)); 202 | if (FVector3.Dot(ba, direction) > FP.Zero) 203 | { 204 | direction = -direction; 205 | } 206 | if (FVector3.Dot(simplex.B.Difference, direction) < -Tolerance) 207 | { 208 | // Remove a 209 | simplex.A = simplex.B; 210 | simplex.B = simplex.C; 211 | simplex.C = simplex.D; 212 | simplex.Stage = 3; 213 | return (false, direction); 214 | } 215 | 216 | // origin is in center 217 | return (true, direction); 218 | } 219 | } 220 | 221 | return (false, direction); 222 | } 223 | } 224 | } 225 | -------------------------------------------------------------------------------- /Runtime/Collisions/GJK.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 28e881bbdfbe4d4ebb45d14864bd18ba 3 | timeCreated: 1738331140 -------------------------------------------------------------------------------- /Runtime/Collisions/ISupportMappable.cs: -------------------------------------------------------------------------------- 1 | namespace Mathematics.Fixed 2 | { 3 | public interface ISupportMappable 4 | { 5 | FVector3 Centre { get; } 6 | 7 | /// 8 | /// Returns furthest point of object in some direction. 9 | /// 10 | FVector3 SupportPoint(FVector3 direction); 11 | } 12 | } 13 | -------------------------------------------------------------------------------- /Runtime/Collisions/ISupportMappable.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 9d7171190e734dc2bc5806a9c081a696 3 | timeCreated: 1738331090 -------------------------------------------------------------------------------- /Runtime/Collisions/MinkowskiDifference.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Runtime.CompilerServices; 3 | 4 | namespace Mathematics.Fixed 5 | { 6 | public readonly struct MinkowskiDifference : IEquatable 7 | { 8 | public readonly FVector3 SupportA; 9 | public readonly FVector3 SupportB; 10 | public readonly FVector3 Difference; 11 | 12 | private MinkowskiDifference(FVector3 supportA, FVector3 supportB, FVector3 difference) 13 | { 14 | SupportA = supportA; 15 | SupportB = supportB; 16 | Difference = difference; 17 | } 18 | 19 | public static MinkowskiDifference Calculate(T shapeA, T shapeB, FVector3 direction) where T : ISupportMappable 20 | { 21 | var supportA = shapeA.SupportPoint(direction); 22 | var supportB = shapeB.SupportPoint(-direction); 23 | var difference = supportA - supportB; 24 | 25 | return new MinkowskiDifference(supportA, supportB, difference); 26 | } 27 | 28 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 29 | public bool Equals(MinkowskiDifference other) 30 | { 31 | return Difference.Equals(other.Difference); 32 | } 33 | 34 | public override bool Equals(object obj) 35 | { 36 | return obj is MinkowskiDifference other && Equals(other); 37 | } 38 | 39 | public override int GetHashCode() 40 | { 41 | return Difference.GetHashCode(); 42 | } 43 | } 44 | } 45 | -------------------------------------------------------------------------------- /Runtime/Collisions/MinkowskiDifference.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: c936f1316945423a9525b2cfe64ef753 3 | timeCreated: 1738331037 -------------------------------------------------------------------------------- /Runtime/Cordic.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 267e8cc5b2554e6ba773b6b0f7e3d090 3 | timeCreated: 1738603683 -------------------------------------------------------------------------------- /Runtime/Cordic/FCordic.Lut.cs: -------------------------------------------------------------------------------- 1 | // ReSharper disable all ShiftExpressionResultEqualsZero 2 | 3 | using Unity.IL2CPP.CompilerServices; 4 | 5 | namespace Mathematics.Fixed 6 | { 7 | public static partial class FCordic 8 | { 9 | /// 10 | /// 64 iterations of Math.Atan(2^-i) where i = 0 .. 63. 11 | /// 12 | public static readonly long[] RawAtans = 13 | { 14 | 7244019458077122560L >> (63 - FP.FractionalBits), 15 | 4276394391812611584L >> (63 - FP.FractionalBits), 16 | 2259529351110384896L >> (63 - FP.FractionalBits), 17 | 1146972379345827584L >> (63 - FP.FractionalBits), 18 | 575711906690464384L >> (63 - FP.FractionalBits), 19 | 288136606096737440L >> (63 - FP.FractionalBits), 20 | 144103461669513648L >> (63 - FP.FractionalBits), 21 | 72056128076108984L >> (63 - FP.FractionalBits), 22 | 36028613768703708L >> (63 - FP.FractionalBits), 23 | 18014375603042168L >> (63 - FP.FractionalBits), 24 | 9007196391431100L >> (63 - FP.FractionalBits), 25 | 4503599269456606L >> (63 - FP.FractionalBits), 26 | 2251799768946007L >> (63 - FP.FractionalBits), 27 | 1125899901250218L >> (63 - FP.FractionalBits), 28 | 562949952722261L >> (63 - FP.FractionalBits), 29 | 281474976623274L >> (63 - FP.FractionalBits), 30 | 140737488344405L >> (63 - FP.FractionalBits), 31 | 70368744176298L >> (63 - FP.FractionalBits), 32 | 35184372088661L >> (63 - FP.FractionalBits), 33 | 17592186044394L >> (63 - FP.FractionalBits), 34 | 8796093022205L >> (63 - FP.FractionalBits), 35 | 4398046511103L >> (63 - FP.FractionalBits), 36 | 2199023255551L >> (63 - FP.FractionalBits), 37 | 1099511627775L >> (63 - FP.FractionalBits), 38 | 549755813887L >> (63 - FP.FractionalBits), 39 | 274877906943L >> (63 - FP.FractionalBits), 40 | 137438953471L >> (63 - FP.FractionalBits), 41 | 68719476736L >> (63 - FP.FractionalBits), 42 | 34359738368L >> (63 - FP.FractionalBits), 43 | 17179869184L >> (63 - FP.FractionalBits), 44 | 8589934592L >> (63 - FP.FractionalBits), 45 | 4294967296L >> (63 - FP.FractionalBits), 46 | 2147483648L >> (63 - FP.FractionalBits), 47 | 1073741824L >> (63 - FP.FractionalBits), 48 | 536870912L >> (63 - FP.FractionalBits), 49 | 268435456L >> (63 - FP.FractionalBits), 50 | 134217728L >> (63 - FP.FractionalBits), 51 | 67108864L >> (63 - FP.FractionalBits), 52 | 33554432L >> (63 - FP.FractionalBits), 53 | 16777216L >> (63 - FP.FractionalBits), 54 | 8388608L >> (63 - FP.FractionalBits), 55 | 4194304L >> (63 - FP.FractionalBits), 56 | 2097152L >> (63 - FP.FractionalBits), 57 | 1048576L >> (63 - FP.FractionalBits), 58 | 524288L >> (63 - FP.FractionalBits), 59 | 262144L >> (63 - FP.FractionalBits), 60 | 131072L >> (63 - FP.FractionalBits), 61 | 65536L >> (63 - FP.FractionalBits), 62 | 32768L >> (63 - FP.FractionalBits), 63 | 16384L >> (63 - FP.FractionalBits), 64 | 8192L >> (63 - FP.FractionalBits), 65 | 4096L >> (63 - FP.FractionalBits), 66 | 2048L >> (63 - FP.FractionalBits), 67 | 1024L >> (63 - FP.FractionalBits), 68 | 512L >> (63 - FP.FractionalBits), 69 | 256L >> (63 - FP.FractionalBits), 70 | 128L >> (63 - FP.FractionalBits), 71 | 64L >> (63 - FP.FractionalBits), 72 | 32L >> (63 - FP.FractionalBits), 73 | 16L >> (63 - FP.FractionalBits), 74 | 8L >> (63 - FP.FractionalBits), 75 | 4L >> (63 - FP.FractionalBits), 76 | 2L >> (63 - FP.FractionalBits), 77 | 1L >> (63 - FP.FractionalBits), 78 | }; 79 | } 80 | } 81 | -------------------------------------------------------------------------------- /Runtime/Cordic/FCordic.Lut.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: de2865b0d90f416e96cf9d6e6533ac29 3 | timeCreated: 1738598351 -------------------------------------------------------------------------------- /Runtime/Cordic/FCordic.Unrolled.cs: -------------------------------------------------------------------------------- 1 | using System.Runtime.CompilerServices; 2 | using Unity.IL2CPP.CompilerServices; 3 | 4 | // ReSharper disable all ShiftExpressionResultEqualsZero 5 | namespace Mathematics.Fixed 6 | { 7 | [Il2CppSetOption(Option.NullChecks, false)] 8 | [Il2CppSetOption(Option.ArrayBoundsChecks, false)] 9 | [Il2CppSetOption(Option.DivideByZeroChecks, false)] 10 | public static partial class FCordic 11 | { 12 | /// 13 | /// z = [0, Pi/2] 14 | /// 15 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 16 | public static void CordicCircular16(ref long xRef, ref long yRef, ref long zRef) 17 | { 18 | var x = xRef; 19 | var y = yRef; 20 | var z = zRef; 21 | 22 | if (z >= 0) 23 | { 24 | var xNext = x - (y >> 0); 25 | y = y + (x >> 0); 26 | x = xNext; 27 | z -= 7244019458077122560L >> (63 - FP.FractionalBits); 28 | } 29 | else 30 | { 31 | var xNext = x + (y >> 0); 32 | y = y - (x >> 0); 33 | x = xNext; 34 | z += 7244019458077122560L >> (63 - FP.FractionalBits); 35 | } 36 | if (z >= 0) 37 | { 38 | var xNext1 = x - (y >> 1); 39 | y = y + (x >> 1); 40 | x = xNext1; 41 | z -= 4276394391812611584L >> (63 - FP.FractionalBits); 42 | } 43 | else 44 | { 45 | var xNext2 = x + (y >> 1); 46 | y = y - (x >> 1); 47 | x = xNext2; 48 | z += 4276394391812611584L >> (63 - FP.FractionalBits); 49 | } 50 | if (z >= 0) 51 | { 52 | var xNext3 = x - (y >> 2); 53 | y = y + (x >> 2); 54 | x = xNext3; 55 | z -= 2259529351110384896L >> (63 - FP.FractionalBits); 56 | } 57 | else 58 | { 59 | var xNext4 = x + (y >> 2); 60 | y = y - (x >> 2); 61 | x = xNext4; 62 | z += 2259529351110384896L >> (63 - FP.FractionalBits); 63 | } 64 | if (z >= 0) 65 | { 66 | var xNext5 = x - (y >> 3); 67 | y = y + (x >> 3); 68 | x = xNext5; 69 | z -= 1146972379345827584L >> (63 - FP.FractionalBits); 70 | } 71 | else 72 | { 73 | var xNext6 = x + (y >> 3); 74 | y = y - (x >> 3); 75 | x = xNext6; 76 | z += 1146972379345827584L >> (63 - FP.FractionalBits); 77 | } 78 | if (z >= 0) 79 | { 80 | var xNext7 = x - (y >> 4); 81 | y = y + (x >> 4); 82 | x = xNext7; 83 | z -= 575711906690464384L >> (63 - FP.FractionalBits); 84 | } 85 | else 86 | { 87 | var xNext8 = x + (y >> 4); 88 | y = y - (x >> 4); 89 | x = xNext8; 90 | z += 575711906690464384L >> (63 - FP.FractionalBits); 91 | } 92 | if (z >= 0) 93 | { 94 | var xNext9 = x - (y >> 5); 95 | y = y + (x >> 5); 96 | x = xNext9; 97 | z -= 288136606096737440L >> (63 - FP.FractionalBits); 98 | } 99 | else 100 | { 101 | var xNext10 = x + (y >> 5); 102 | y = y - (x >> 5); 103 | x = xNext10; 104 | z += 288136606096737440L >> (63 - FP.FractionalBits); 105 | } 106 | if (z >= 0) 107 | { 108 | var xNext11 = x - (y >> 6); 109 | y = y + (x >> 6); 110 | x = xNext11; 111 | z -= 144103461669513648L >> (63 - FP.FractionalBits); 112 | } 113 | else 114 | { 115 | var xNext12 = x + (y >> 6); 116 | y = y - (x >> 6); 117 | x = xNext12; 118 | z += 144103461669513648L >> (63 - FP.FractionalBits); 119 | } 120 | if (z >= 0) 121 | { 122 | var xNext13 = x - (y >> 7); 123 | y = y + (x >> 7); 124 | x = xNext13; 125 | z -= 72056128076108984L >> (63 - FP.FractionalBits); 126 | } 127 | else 128 | { 129 | var xNext14 = x + (y >> 7); 130 | y = y - (x >> 7); 131 | x = xNext14; 132 | z += 72056128076108984L >> (63 - FP.FractionalBits); 133 | } 134 | if (z >= 0) 135 | { 136 | var xNext15 = x - (y >> 8); 137 | y = y + (x >> 8); 138 | x = xNext15; 139 | z -= 36028613768703708L >> (63 - FP.FractionalBits); 140 | } 141 | else 142 | { 143 | var xNext16 = x + (y >> 8); 144 | y = y - (x >> 8); 145 | x = xNext16; 146 | z += 36028613768703708L >> (63 - FP.FractionalBits); 147 | } 148 | if (z >= 0) 149 | { 150 | var xNext17 = x - (y >> 9); 151 | y = y + (x >> 9); 152 | x = xNext17; 153 | z -= 18014375603042168L >> (63 - FP.FractionalBits); 154 | } 155 | else 156 | { 157 | var xNext18 = x + (y >> 9); 158 | y = y - (x >> 9); 159 | x = xNext18; 160 | z += 18014375603042168L >> (63 - FP.FractionalBits); 161 | } 162 | if (z >= 0) 163 | { 164 | var xNext19 = x - (y >> 10); 165 | y = y + (x >> 10); 166 | x = xNext19; 167 | z -= 9007196391431100L >> (63 - FP.FractionalBits); 168 | } 169 | else 170 | { 171 | var xNext20 = x + (y >> 10); 172 | y = y - (x >> 10); 173 | x = xNext20; 174 | z += 9007196391431100L >> (63 - FP.FractionalBits); 175 | } 176 | if (z >= 0) 177 | { 178 | var xNext21 = x - (y >> 11); 179 | y = y + (x >> 11); 180 | x = xNext21; 181 | z -= 4503599269456606L >> (63 - FP.FractionalBits); 182 | } 183 | else 184 | { 185 | var xNext22 = x + (y >> 11); 186 | y = y - (x >> 11); 187 | x = xNext22; 188 | z += 4503599269456606L >> (63 - FP.FractionalBits); 189 | } 190 | if (z >= 0) 191 | { 192 | var xNext23 = x - (y >> 12); 193 | y = y + (x >> 12); 194 | x = xNext23; 195 | z -= 2251799768946007L >> (63 - FP.FractionalBits); 196 | } 197 | else 198 | { 199 | var xNext24 = x + (y >> 12); 200 | y = y - (x >> 12); 201 | x = xNext24; 202 | z += 2251799768946007L >> (63 - FP.FractionalBits); 203 | } 204 | if (z >= 0) 205 | { 206 | var xNext25 = x - (y >> 13); 207 | y = y + (x >> 13); 208 | x = xNext25; 209 | z -= 1125899901250218L >> (63 - FP.FractionalBits); 210 | } 211 | else 212 | { 213 | var xNext26 = x + (y >> 13); 214 | y = y - (x >> 13); 215 | x = xNext26; 216 | z += 1125899901250218L >> (63 - FP.FractionalBits); 217 | } 218 | if (z >= 0) 219 | { 220 | var xNext27 = x - (y >> 14); 221 | y = y + (x >> 14); 222 | x = xNext27; 223 | z -= 562949952722261L >> (63 - FP.FractionalBits); 224 | } 225 | else 226 | { 227 | var xNext28 = x + (y >> 14); 228 | y = y - (x >> 14); 229 | x = xNext28; 230 | z += 562949952722261L >> (63 - FP.FractionalBits); 231 | } 232 | if (z >= 0) 233 | { 234 | var xNext29 = x - (y >> 15); 235 | y = y + (x >> 15); 236 | x = xNext29; 237 | z -= 281474976623274L >> (63 - FP.FractionalBits); 238 | } 239 | else 240 | { 241 | var xNext30 = x + (y >> 15); 242 | y = y - (x >> 15); 243 | x = xNext30; 244 | z += 281474976623274L >> (63 - FP.FractionalBits); 245 | } 246 | 247 | xRef = x; 248 | yRef = y; 249 | zRef = z; 250 | } 251 | 252 | /// 253 | /// z = [0, Pi/2] 254 | /// 255 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 256 | public static void CordicVectoring16(ref long xRef, ref long yRef, ref long zRef, long target) 257 | { 258 | var x = xRef; 259 | var y = yRef; 260 | var z = zRef; 261 | 262 | if (y < target) 263 | { 264 | var xNext = x - (y >> 0); 265 | y = y + (x >> 0); 266 | x = xNext; 267 | z -= 7244019458077122560L >> (63 - FP.FractionalBits); 268 | } 269 | else 270 | { 271 | var xNext = x + (y >> 0); 272 | y = y - (x >> 0); 273 | x = xNext; 274 | z += 7244019458077122560L >> (63 - FP.FractionalBits); 275 | } 276 | if (y < target) 277 | { 278 | var xNext1 = x - (y >> 1); 279 | y = y + (x >> 1); 280 | x = xNext1; 281 | z -= 4276394391812611584L >> (63 - FP.FractionalBits); 282 | } 283 | else 284 | { 285 | var xNext2 = x + (y >> 1); 286 | y = y - (x >> 1); 287 | x = xNext2; 288 | z += 4276394391812611584L >> (63 - FP.FractionalBits); 289 | } 290 | if (y < target) 291 | { 292 | var xNext3 = x - (y >> 2); 293 | y = y + (x >> 2); 294 | x = xNext3; 295 | z -= 2259529351110384896L >> (63 - FP.FractionalBits); 296 | } 297 | else 298 | { 299 | var xNext4 = x + (y >> 2); 300 | y = y - (x >> 2); 301 | x = xNext4; 302 | z += 2259529351110384896L >> (63 - FP.FractionalBits); 303 | } 304 | if (y < target) 305 | { 306 | var xNext5 = x - (y >> 3); 307 | y = y + (x >> 3); 308 | x = xNext5; 309 | z -= 1146972379345827584L >> (63 - FP.FractionalBits); 310 | } 311 | else 312 | { 313 | var xNext6 = x + (y >> 3); 314 | y = y - (x >> 3); 315 | x = xNext6; 316 | z += 1146972379345827584L >> (63 - FP.FractionalBits); 317 | } 318 | if (y < target) 319 | { 320 | var xNext7 = x - (y >> 4); 321 | y = y + (x >> 4); 322 | x = xNext7; 323 | z -= 575711906690464384L >> (63 - FP.FractionalBits); 324 | } 325 | else 326 | { 327 | var xNext8 = x + (y >> 4); 328 | y = y - (x >> 4); 329 | x = xNext8; 330 | z += 575711906690464384L >> (63 - FP.FractionalBits); 331 | } 332 | if (y < target) 333 | { 334 | var xNext9 = x - (y >> 5); 335 | y = y + (x >> 5); 336 | x = xNext9; 337 | z -= 288136606096737440L >> (63 - FP.FractionalBits); 338 | } 339 | else 340 | { 341 | var xNext10 = x + (y >> 5); 342 | y = y - (x >> 5); 343 | x = xNext10; 344 | z += 288136606096737440L >> (63 - FP.FractionalBits); 345 | } 346 | if (y < target) 347 | { 348 | var xNext11 = x - (y >> 6); 349 | y = y + (x >> 6); 350 | x = xNext11; 351 | z -= 144103461669513648L >> (63 - FP.FractionalBits); 352 | } 353 | else 354 | { 355 | var xNext12 = x + (y >> 6); 356 | y = y - (x >> 6); 357 | x = xNext12; 358 | z += 144103461669513648L >> (63 - FP.FractionalBits); 359 | } 360 | if (y < target) 361 | { 362 | var xNext13 = x - (y >> 7); 363 | y = y + (x >> 7); 364 | x = xNext13; 365 | z -= 72056128076108984L >> (63 - FP.FractionalBits); 366 | } 367 | else 368 | { 369 | var xNext14 = x + (y >> 7); 370 | y = y - (x >> 7); 371 | x = xNext14; 372 | z += 72056128076108984L >> (63 - FP.FractionalBits); 373 | } 374 | if (y < target) 375 | { 376 | var xNext15 = x - (y >> 8); 377 | y = y + (x >> 8); 378 | x = xNext15; 379 | z -= 36028613768703708L >> (63 - FP.FractionalBits); 380 | } 381 | else 382 | { 383 | var xNext16 = x + (y >> 8); 384 | y = y - (x >> 8); 385 | x = xNext16; 386 | z += 36028613768703708L >> (63 - FP.FractionalBits); 387 | } 388 | if (y < target) 389 | { 390 | var xNext17 = x - (y >> 9); 391 | y = y + (x >> 9); 392 | x = xNext17; 393 | z -= 18014375603042168L >> (63 - FP.FractionalBits); 394 | } 395 | else 396 | { 397 | var xNext18 = x + (y >> 9); 398 | y = y - (x >> 9); 399 | x = xNext18; 400 | z += 18014375603042168L >> (63 - FP.FractionalBits); 401 | } 402 | if (y < target) 403 | { 404 | var xNext19 = x - (y >> 10); 405 | y = y + (x >> 10); 406 | x = xNext19; 407 | z -= 9007196391431100L >> (63 - FP.FractionalBits); 408 | } 409 | else 410 | { 411 | var xNext20 = x + (y >> 10); 412 | y = y - (x >> 10); 413 | x = xNext20; 414 | z += 9007196391431100L >> (63 - FP.FractionalBits); 415 | } 416 | if (y < target) 417 | { 418 | var xNext21 = x - (y >> 11); 419 | y = y + (x >> 11); 420 | x = xNext21; 421 | z -= 4503599269456606L >> (63 - FP.FractionalBits); 422 | } 423 | else 424 | { 425 | var xNext22 = x + (y >> 11); 426 | y = y - (x >> 11); 427 | x = xNext22; 428 | z += 4503599269456606L >> (63 - FP.FractionalBits); 429 | } 430 | if (y < target) 431 | { 432 | var xNext23 = x - (y >> 12); 433 | y = y + (x >> 12); 434 | x = xNext23; 435 | z -= 2251799768946007L >> (63 - FP.FractionalBits); 436 | } 437 | else 438 | { 439 | var xNext24 = x + (y >> 12); 440 | y = y - (x >> 12); 441 | x = xNext24; 442 | z += 2251799768946007L >> (63 - FP.FractionalBits); 443 | } 444 | if (y < target) 445 | { 446 | var xNext25 = x - (y >> 13); 447 | y = y + (x >> 13); 448 | x = xNext25; 449 | z -= 1125899901250218L >> (63 - FP.FractionalBits); 450 | } 451 | else 452 | { 453 | var xNext26 = x + (y >> 13); 454 | y = y - (x >> 13); 455 | x = xNext26; 456 | z += 1125899901250218L >> (63 - FP.FractionalBits); 457 | } 458 | if (y < target) 459 | { 460 | var xNext27 = x - (y >> 14); 461 | y = y + (x >> 14); 462 | x = xNext27; 463 | z -= 562949952722261L >> (63 - FP.FractionalBits); 464 | } 465 | else 466 | { 467 | var xNext28 = x + (y >> 14); 468 | y = y - (x >> 14); 469 | x = xNext28; 470 | z += 562949952722261L >> (63 - FP.FractionalBits); 471 | } 472 | if (y < target) 473 | { 474 | var xNext29 = x - (y >> 15); 475 | y = y + (x >> 15); 476 | x = xNext29; 477 | z -= 281474976623274L >> (63 - FP.FractionalBits); 478 | } 479 | else 480 | { 481 | var xNext30 = x + (y >> 15); 482 | y = y - (x >> 15); 483 | x = xNext30; 484 | z += 281474976623274L >> (63 - FP.FractionalBits); 485 | } 486 | 487 | xRef = x; 488 | yRef = y; 489 | zRef = z; 490 | } 491 | 492 | private static void CordicCircularUnrollTemplate(ref long xRef, ref long yRef, ref long zRef, long target) 493 | { 494 | var x = xRef; 495 | var y = yRef; 496 | var z = zRef; 497 | 498 | IterationStep(0, 7244019458077122560L >> (63 - FP.FractionalBits)); 499 | IterationStep(1, 4276394391812611584L >> (63 - FP.FractionalBits)); 500 | IterationStep(2, 2259529351110384896L >> (63 - FP.FractionalBits)); 501 | IterationStep(3, 1146972379345827584L >> (63 - FP.FractionalBits)); 502 | IterationStep(4, 575711906690464384L >> (63 - FP.FractionalBits)); 503 | IterationStep(5, 288136606096737440L >> (63 - FP.FractionalBits)); 504 | IterationStep(6, 144103461669513648L >> (63 - FP.FractionalBits)); 505 | IterationStep(7, 72056128076108984L >> (63 - FP.FractionalBits)); 506 | IterationStep(8, 36028613768703708L >> (63 - FP.FractionalBits)); 507 | IterationStep(9, 18014375603042168L >> (63 - FP.FractionalBits)); 508 | IterationStep(10, 9007196391431100L >> (63 - FP.FractionalBits)); 509 | IterationStep(11, 4503599269456606L >> (63 - FP.FractionalBits)); 510 | IterationStep(12, 2251799768946007L >> (63 - FP.FractionalBits)); 511 | IterationStep(13, 1125899901250218L >> (63 - FP.FractionalBits)); 512 | IterationStep(14, 562949952722261L >> (63 - FP.FractionalBits)); 513 | IterationStep(15, 281474976623274L >> (63 - FP.FractionalBits)); 514 | IterationStep(16, 140737488344405L >> (63 - FP.FractionalBits)); 515 | IterationStep(17, 70368744176298L >> (63 - FP.FractionalBits)); 516 | IterationStep(18, 35184372088661L >> (63 - FP.FractionalBits)); 517 | IterationStep(19, 17592186044394L >> (63 - FP.FractionalBits)); 518 | IterationStep(20, 8796093022205L >> (63 - FP.FractionalBits)); 519 | IterationStep(21, 4398046511103L >> (63 - FP.FractionalBits)); 520 | IterationStep(22, 2199023255551L >> (63 - FP.FractionalBits)); 521 | IterationStep(23, 1099511627775L >> (63 - FP.FractionalBits)); 522 | IterationStep(24, 549755813887L >> (63 - FP.FractionalBits)); 523 | IterationStep(25, 274877906943L >> (63 - FP.FractionalBits)); 524 | IterationStep(26, 137438953471L >> (63 - FP.FractionalBits)); 525 | IterationStep(27, 68719476736L >> (63 - FP.FractionalBits)); 526 | IterationStep(28, 34359738368L >> (63 - FP.FractionalBits)); 527 | IterationStep(29, 17179869184L >> (63 - FP.FractionalBits)); 528 | IterationStep(30, 8589934592L >> (63 - FP.FractionalBits)); 529 | IterationStep(31, 4294967296L >> (63 - FP.FractionalBits)); 530 | IterationStep(32, 2147483648L >> (63 - FP.FractionalBits)); 531 | IterationStep(33, 1073741824L >> (63 - FP.FractionalBits)); 532 | IterationStep(34, 536870912L >> (63 - FP.FractionalBits)); 533 | IterationStep(35, 268435456L >> (63 - FP.FractionalBits)); 534 | IterationStep(36, 134217728L >> (63 - FP.FractionalBits)); 535 | IterationStep(37, 67108864L >> (63 - FP.FractionalBits)); 536 | IterationStep(38, 33554432L >> (63 - FP.FractionalBits)); 537 | IterationStep(39, 16777216L >> (63 - FP.FractionalBits)); 538 | IterationStep(40, 8388608L >> (63 - FP.FractionalBits)); 539 | IterationStep(41, 4194304L >> (63 - FP.FractionalBits)); 540 | IterationStep(42, 2097152L >> (63 - FP.FractionalBits)); 541 | IterationStep(43, 1048576L >> (63 - FP.FractionalBits)); 542 | IterationStep(44, 524288L >> (63 - FP.FractionalBits)); 543 | IterationStep(45, 262144L >> (63 - FP.FractionalBits)); 544 | IterationStep(46, 131072L >> (63 - FP.FractionalBits)); 545 | IterationStep(47, 65536L >> (63 - FP.FractionalBits)); 546 | IterationStep(48, 32768L >> (63 - FP.FractionalBits)); 547 | IterationStep(49, 16384L >> (63 - FP.FractionalBits)); 548 | IterationStep(50, 8192L >> (63 - FP.FractionalBits)); 549 | IterationStep(51, 4096L >> (63 - FP.FractionalBits)); 550 | IterationStep(52, 2048L >> (63 - FP.FractionalBits)); 551 | IterationStep(53, 1024L >> (63 - FP.FractionalBits)); 552 | IterationStep(54, 512L >> (63 - FP.FractionalBits)); 553 | IterationStep(55, 256L >> (63 - FP.FractionalBits)); 554 | IterationStep(56, 128L >> (63 - FP.FractionalBits)); 555 | IterationStep(57, 64L >> (63 - FP.FractionalBits)); 556 | IterationStep(58, 32L >> (63 - FP.FractionalBits)); 557 | IterationStep(59, 16L >> (63 - FP.FractionalBits)); 558 | IterationStep(60, 8L >> (63 - FP.FractionalBits)); 559 | IterationStep(61, 4L >> (63 - FP.FractionalBits)); 560 | IterationStep(62, 2L >> (63 - FP.FractionalBits)); 561 | IterationStep(63, 1L >> (63 - FP.FractionalBits)); 562 | 563 | xRef = x; 564 | yRef = y; 565 | zRef = z; 566 | 567 | void IterationStep(int i, long atan) 568 | { 569 | if (z >= 0) 570 | { 571 | var xNext = x - (y >> i); 572 | y = y + (x >> i); 573 | x = xNext; 574 | z -= atan; 575 | } 576 | else 577 | { 578 | var xNext = x + (y >> i); 579 | y = y - (x >> i); 580 | x = xNext; 581 | z += atan; 582 | } 583 | } 584 | } 585 | 586 | private static void CordicVectoringUnrollTemplate(ref long xRef, ref long yRef, ref long zRef, long target) 587 | { 588 | var x = xRef; 589 | var y = yRef; 590 | var z = zRef; 591 | 592 | IterationStep(0, 7244019458077122560L >> (63 - FP.FractionalBits)); 593 | IterationStep(1, 4276394391812611584L >> (63 - FP.FractionalBits)); 594 | IterationStep(2, 2259529351110384896L >> (63 - FP.FractionalBits)); 595 | IterationStep(3, 1146972379345827584L >> (63 - FP.FractionalBits)); 596 | IterationStep(4, 575711906690464384L >> (63 - FP.FractionalBits)); 597 | IterationStep(5, 288136606096737440L >> (63 - FP.FractionalBits)); 598 | IterationStep(6, 144103461669513648L >> (63 - FP.FractionalBits)); 599 | IterationStep(7, 72056128076108984L >> (63 - FP.FractionalBits)); 600 | IterationStep(8, 36028613768703708L >> (63 - FP.FractionalBits)); 601 | IterationStep(9, 18014375603042168L >> (63 - FP.FractionalBits)); 602 | IterationStep(10, 9007196391431100L >> (63 - FP.FractionalBits)); 603 | IterationStep(11, 4503599269456606L >> (63 - FP.FractionalBits)); 604 | IterationStep(12, 2251799768946007L >> (63 - FP.FractionalBits)); 605 | IterationStep(13, 1125899901250218L >> (63 - FP.FractionalBits)); 606 | IterationStep(14, 562949952722261L >> (63 - FP.FractionalBits)); 607 | IterationStep(15, 281474976623274L >> (63 - FP.FractionalBits)); 608 | IterationStep(16, 140737488344405L >> (63 - FP.FractionalBits)); 609 | IterationStep(17, 70368744176298L >> (63 - FP.FractionalBits)); 610 | IterationStep(18, 35184372088661L >> (63 - FP.FractionalBits)); 611 | IterationStep(19, 17592186044394L >> (63 - FP.FractionalBits)); 612 | IterationStep(20, 8796093022205L >> (63 - FP.FractionalBits)); 613 | IterationStep(21, 4398046511103L >> (63 - FP.FractionalBits)); 614 | IterationStep(22, 2199023255551L >> (63 - FP.FractionalBits)); 615 | IterationStep(23, 1099511627775L >> (63 - FP.FractionalBits)); 616 | IterationStep(24, 549755813887L >> (63 - FP.FractionalBits)); 617 | IterationStep(25, 274877906943L >> (63 - FP.FractionalBits)); 618 | IterationStep(26, 137438953471L >> (63 - FP.FractionalBits)); 619 | IterationStep(27, 68719476736L >> (63 - FP.FractionalBits)); 620 | IterationStep(28, 34359738368L >> (63 - FP.FractionalBits)); 621 | IterationStep(29, 17179869184L >> (63 - FP.FractionalBits)); 622 | IterationStep(30, 8589934592L >> (63 - FP.FractionalBits)); 623 | IterationStep(31, 4294967296L >> (63 - FP.FractionalBits)); 624 | IterationStep(32, 2147483648L >> (63 - FP.FractionalBits)); 625 | IterationStep(33, 1073741824L >> (63 - FP.FractionalBits)); 626 | IterationStep(34, 536870912L >> (63 - FP.FractionalBits)); 627 | IterationStep(35, 268435456L >> (63 - FP.FractionalBits)); 628 | IterationStep(36, 134217728L >> (63 - FP.FractionalBits)); 629 | IterationStep(37, 67108864L >> (63 - FP.FractionalBits)); 630 | IterationStep(38, 33554432L >> (63 - FP.FractionalBits)); 631 | IterationStep(39, 16777216L >> (63 - FP.FractionalBits)); 632 | IterationStep(40, 8388608L >> (63 - FP.FractionalBits)); 633 | IterationStep(41, 4194304L >> (63 - FP.FractionalBits)); 634 | IterationStep(42, 2097152L >> (63 - FP.FractionalBits)); 635 | IterationStep(43, 1048576L >> (63 - FP.FractionalBits)); 636 | IterationStep(44, 524288L >> (63 - FP.FractionalBits)); 637 | IterationStep(45, 262144L >> (63 - FP.FractionalBits)); 638 | IterationStep(46, 131072L >> (63 - FP.FractionalBits)); 639 | IterationStep(47, 65536L >> (63 - FP.FractionalBits)); 640 | IterationStep(48, 32768L >> (63 - FP.FractionalBits)); 641 | IterationStep(49, 16384L >> (63 - FP.FractionalBits)); 642 | IterationStep(50, 8192L >> (63 - FP.FractionalBits)); 643 | IterationStep(51, 4096L >> (63 - FP.FractionalBits)); 644 | IterationStep(52, 2048L >> (63 - FP.FractionalBits)); 645 | IterationStep(53, 1024L >> (63 - FP.FractionalBits)); 646 | IterationStep(54, 512L >> (63 - FP.FractionalBits)); 647 | IterationStep(55, 256L >> (63 - FP.FractionalBits)); 648 | IterationStep(56, 128L >> (63 - FP.FractionalBits)); 649 | IterationStep(57, 64L >> (63 - FP.FractionalBits)); 650 | IterationStep(58, 32L >> (63 - FP.FractionalBits)); 651 | IterationStep(59, 16L >> (63 - FP.FractionalBits)); 652 | IterationStep(60, 8L >> (63 - FP.FractionalBits)); 653 | IterationStep(61, 4L >> (63 - FP.FractionalBits)); 654 | IterationStep(62, 2L >> (63 - FP.FractionalBits)); 655 | IterationStep(63, 1L >> (63 - FP.FractionalBits)); 656 | 657 | xRef = x; 658 | yRef = y; 659 | zRef = z; 660 | 661 | void IterationStep(int i, long atan) 662 | { 663 | if (y < target) 664 | { 665 | var xNext = x - (y >> i); 666 | y = y + (x >> i); 667 | x = xNext; 668 | z -= atan; 669 | } 670 | else 671 | { 672 | var xNext = x + (y >> i); 673 | y = y - (x >> i); 674 | x = xNext; 675 | z += atan; 676 | } 677 | } 678 | } 679 | } 680 | } 681 | -------------------------------------------------------------------------------- /Runtime/Cordic/FCordic.Unrolled.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 6dd4cde740a342deba9d3e3ac55e1a29 3 | timeCreated: 1738766968 -------------------------------------------------------------------------------- /Runtime/Cordic/FCordic.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Runtime.CompilerServices; 3 | using Unity.IL2CPP.CompilerServices; 4 | 5 | namespace Mathematics.Fixed 6 | { 7 | [Il2CppEagerStaticClassConstruction] 8 | [Il2CppSetOption(Option.NullChecks, false)] 9 | [Il2CppSetOption(Option.ArrayBoundsChecks, false)] 10 | [Il2CppSetOption(Option.DivideByZeroChecks, false)] 11 | public static partial class FCordic 12 | { 13 | public const int Precision = FP.FractionalBits; 14 | 15 | // CORDIC cosine constant 0.60725... 16 | public const long InvGainBase63 = 5600919740058907648; 17 | public const long InvGain = InvGainBase63 >> (63 - FP.FractionalBits); 18 | 19 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 20 | public static long Sin(long angle) 21 | { 22 | angle %= FP.TwoPiRaw; // Map to [-2*Pi, 2*Pi) 23 | 24 | if (angle < 0) 25 | { 26 | angle += FP.TwoPiRaw; // Map to [0, 2*Pi) 27 | } 28 | 29 | var flipVertical = angle >= FP.PiRaw; 30 | if (flipVertical) 31 | { 32 | angle -= FP.PiRaw; // Map to [0, Pi) 33 | } 34 | 35 | var flipHorizontal = angle >= FP.HalfPiRaw; 36 | if (flipHorizontal) 37 | { 38 | angle = FP.PiRaw - angle; // Map to [0, Pi/2] 39 | } 40 | 41 | var sin = 0L; 42 | var cos = InvGain; 43 | CordicCircular16(ref cos, ref sin, ref angle); 44 | 45 | return flipVertical ? -sin : sin; 46 | } 47 | 48 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 49 | public static void SinCos(long angle, out long sin, out long cos) 50 | { 51 | angle %= FP.TwoPiRaw; // Map to [-2*Pi, 2*Pi) 52 | 53 | if (angle < 0) 54 | { 55 | angle += FP.TwoPiRaw; // Map to [0, 2*Pi) 56 | } 57 | 58 | var flipVertical = angle >= FP.PiRaw; 59 | if (flipVertical) 60 | { 61 | angle -= FP.PiRaw; // Map to [0, Pi) 62 | } 63 | 64 | var flipHorizontal = angle >= FP.HalfPiRaw; 65 | if (flipHorizontal) 66 | { 67 | angle = FP.PiRaw - angle; // Map to [0, Pi/2] 68 | } 69 | 70 | sin = 0L; 71 | cos = InvGain; 72 | CordicCircular16(ref cos, ref sin, ref angle); 73 | 74 | if (flipVertical) 75 | { 76 | sin = -sin; 77 | } 78 | 79 | if (flipVertical != flipHorizontal) 80 | { 81 | cos = -cos; 82 | } 83 | } 84 | 85 | /// 86 | /// Precise cordic. 87 | /// Angle is [0, HalfPi]. 88 | /// 89 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 90 | public static void SinCosZeroToHalfPi(long angle, out long sin, out long cos) 91 | { 92 | sin = 0L; 93 | cos = InvGain; 94 | CordicCircular(ref cos, ref sin, ref angle); 95 | } 96 | 97 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 98 | public static long Tan(long angle) 99 | { 100 | angle = angle % FP.PiRaw; // Map to [-Pi, Pi) 101 | 102 | if (angle < 0) 103 | { 104 | angle += FP.PiRaw; // Map to [0, Pi) 105 | } 106 | 107 | var flipVertical = angle >= FP.HalfPiRaw; 108 | if (flipVertical) 109 | { 110 | angle = FP.PiRaw - angle; // Map to [0, Pi/2] 111 | } 112 | 113 | var sin = 0L; 114 | var cos = InvGain; 115 | CordicCircular16(ref cos, ref sin, ref angle); 116 | 117 | var result = FP.Div(sin, cos); 118 | 119 | return flipVertical ? FMath.SafeNeg(result) : result; 120 | } 121 | 122 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 123 | public static long Atan(long a) 124 | { 125 | var x = FP.OneRaw; 126 | var z = 0L; 127 | CordicVectoring16(ref x, ref a, ref z, 0); 128 | 129 | return z; 130 | } 131 | 132 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 133 | public static long Atan2(long y, long x) 134 | { 135 | var tan = FP.Div(y, x); 136 | 137 | if (x > 0) 138 | { 139 | return Atan(tan); 140 | } 141 | 142 | if (x < 0) 143 | { 144 | if (y >= 0) 145 | { 146 | y = Atan(tan) + FP.HalfPiRaw; 147 | } 148 | else 149 | { 150 | y = Atan(tan) - FP.HalfPiRaw; 151 | } 152 | return y; 153 | } 154 | 155 | if (y > 0) 156 | { 157 | return FP.HalfPiRaw; 158 | } 159 | 160 | if (y == 0) 161 | { 162 | return 0; 163 | } 164 | 165 | return -FP.HalfPiRaw; 166 | } 167 | 168 | /// 169 | /// Precise cordic [-1, 1]. 170 | /// 171 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 172 | public static long Asin(long sin) 173 | { 174 | if (sin < -FP.OneRaw || sin > FP.OneRaw) 175 | { 176 | throw new ArgumentOutOfRangeException(nameof(sin)); 177 | } 178 | 179 | var flipVertical = sin < 0; 180 | if (flipVertical) 181 | { 182 | sin = -sin; // Map to [0, 1] 183 | } 184 | 185 | var result = AsinZeroToOne(sin); 186 | 187 | return flipVertical ? -result : result; 188 | } 189 | 190 | /// 191 | /// Precise cordic [-1, 1]. 192 | /// 193 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 194 | public static long AsinZeroToOne(long sin) 195 | { 196 | var x = FP.OneRaw; 197 | var y = 0L; 198 | var z = 0L; 199 | CordicVectoringDoubleIteration(ref x, ref y, ref z, sin); 200 | 201 | return z; 202 | } 203 | 204 | /// 205 | /// See cordit1 from http://www.voidware.com/cordic.htm.
206 | /// z is [0, Pi/2]. 207 | /// 208 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 209 | public static void CordicCircular(ref long xRef, ref long yRef, ref long zRef) 210 | { 211 | var x = xRef; 212 | var y = yRef; 213 | var z = zRef; 214 | 215 | for (var i = 0; i < Precision; ++i) 216 | { 217 | if (z >= 0) 218 | { 219 | var xNext = x - (y >> i); 220 | y = y + (x >> i); 221 | x = xNext; 222 | z -= RawAtans[i]; 223 | } 224 | else 225 | { 226 | var xNext = x + (y >> i); 227 | y = y - (x >> i); 228 | x = xNext; 229 | z += RawAtans[i]; 230 | } 231 | } 232 | 233 | xRef = x; 234 | yRef = y; 235 | zRef = z; 236 | } 237 | 238 | /// 239 | /// See cordit1 from http://www.voidware.com/cordic.htm.
240 | /// z is [0, Pi/2]. 241 | /// 242 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 243 | public static void CordicVectoring(ref long xRef, ref long yRef, ref long zRef, long target) 244 | { 245 | var x = xRef; 246 | var y = yRef; 247 | var z = zRef; 248 | 249 | for (int i = 0; i < Precision; i++) 250 | { 251 | if (y < target) 252 | { 253 | var xNext = x - (y >> i); 254 | y = y + (x >> i); 255 | x = xNext; 256 | z -= RawAtans[i]; 257 | } 258 | else 259 | { 260 | var xNext = x + (y >> i); 261 | y = y - (x >> i); 262 | x = xNext; 263 | z += RawAtans[i]; 264 | } 265 | } 266 | 267 | xRef = x; 268 | yRef = y; 269 | zRef = z; 270 | } 271 | 272 | /// 273 | /// See cordit1 from http://www.voidware.com/cordic.htm.
274 | /// Double iteration method https://stackoverflow.com/questions/25976656/cordic-arcsine-implementation-fails. 275 | /// 276 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 277 | public static void CordicVectoringDoubleIteration(ref long xRef, ref long yRef, ref long zRef, long target) 278 | { 279 | var x = xRef; 280 | var y = yRef; 281 | var z = zRef; 282 | 283 | for (int i = 0; i < Precision; i++) 284 | { 285 | if (y < target == x >= 0) 286 | { 287 | var xNext = x - (y >> i); 288 | var yNext = y + (x >> i); 289 | x = xNext - (yNext >> i); 290 | y = yNext + (xNext >> i); 291 | 292 | var atan = RawAtans[i]; 293 | z += atan + atan; 294 | target += target >> (i + i); 295 | } 296 | else 297 | { 298 | var xNext = x + (y >> i); 299 | var yNext = y - (x >> i); 300 | x = xNext + (yNext >> i); 301 | y = yNext - (xNext >> i); 302 | 303 | var atan = RawAtans[i]; 304 | z -= atan + atan; 305 | target += target >> (i + i); 306 | } 307 | } 308 | 309 | xRef = x; 310 | yRef = y; 311 | zRef = z; 312 | } 313 | } 314 | } 315 | -------------------------------------------------------------------------------- /Runtime/Cordic/FCordic.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 8f4c061883e44ca8b08fb5ef38e84f68 3 | timeCreated: 1738603677 -------------------------------------------------------------------------------- /Runtime/FConversions.cs: -------------------------------------------------------------------------------- 1 | using System.Runtime.CompilerServices; 2 | 3 | namespace Mathematics.Fixed 4 | { 5 | public static class FConversions 6 | { 7 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 8 | public static FP ToFP(this long value) 9 | { 10 | return FP.FromRaw(value << FP.FractionalBits); 11 | } 12 | 13 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 14 | public static FP ToFP(this int value) 15 | { 16 | return FP.FromRaw((long)value << FP.FractionalBits); 17 | } 18 | 19 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 20 | public static FP ToFP(this float value) 21 | { 22 | return FP.FromRaw((long)(value * FP.OneRaw)); 23 | } 24 | 25 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 26 | public static FP ToFP(this double value) 27 | { 28 | return FP.FromRaw((long)(value * FP.OneRaw)); 29 | } 30 | 31 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 32 | public static int ToInt(this FP value) 33 | { 34 | return (int)(value.RawValue >> FP.FractionalBits); 35 | } 36 | 37 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 38 | public static long ToLong(this FP value) 39 | { 40 | return value.RawValue >> FP.FractionalBits; 41 | } 42 | 43 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 44 | public static float ToFloat(this FP value) 45 | { 46 | return (float)value.RawValue / FP.OneRaw; 47 | } 48 | 49 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 50 | public static double ToDouble(this FP value) 51 | { 52 | return (double)value.RawValue / FP.OneRaw; 53 | } 54 | } 55 | } 56 | -------------------------------------------------------------------------------- /Runtime/FConversions.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: d3f19e0c362848a88c86b2e6e1d7f554 3 | timeCreated: 1738065813 -------------------------------------------------------------------------------- /Runtime/FMath.Lut.cs: -------------------------------------------------------------------------------- 1 | namespace Mathematics.Fixed 2 | { 3 | public static partial class FMath 4 | { 5 | public const int SinPrecision = 16; // Corelate with lut size. Must be <= FP.FractionalBits. 6 | public const int SinLutShift = FP.FractionalBits - SinPrecision; 7 | private const int SinLutSize = (int)(FP.HalfPiRaw >> SinLutShift); // [0, HalfPi) 8 | 9 | public const int TanPrecision = 18; // Corelate with lut size. Must be <= FP.FractionalBits. 10 | public const int TanLutShift = FP.FractionalBits - TanPrecision; 11 | private const int TanLutSize = (int)(FP.HalfPiRaw >> TanLutShift); // [0, HalfPi) 12 | 13 | public const int AsinPrecision = 16; // Corelate with lut size. Must be <= FP.FractionalPlaces. 14 | public const int AsinLutShift = FP.FractionalBits - AsinPrecision; 15 | private const int AsinLutSize = (int)(FP.OneRaw >> AsinLutShift); // [0, 1) 16 | 17 | public const int SqrtPrecision01 = 16; // Corelate with lut size. Must be <= FP.FractionalPlaces. 18 | public const int SqrtLutShift01 = FP.FractionalBits - SqrtPrecision01; 19 | private const int SqrtLutSize01 = (int)(FP.OneRaw >> SqrtLutShift01); // [0, 1) 20 | 21 | public readonly static FP[] SinLut; 22 | public readonly static FP[] TanLut; 23 | public readonly static FP[] AsinLut; 24 | public readonly static long[] SqrtLutRaw; 25 | 26 | static FMath() 27 | { 28 | SinLut = GenerateSinLut(); 29 | TanLut = GenerateTanLut(); 30 | AsinLut = GenerateAsinLut(); 31 | SqrtLutRaw = GenerateSqrtLut(); 32 | } 33 | 34 | private static FP[] GenerateSinLut() 35 | { 36 | var lut = new FP[SinLutSize + 1]; 37 | lut[^1] = FP.One; 38 | 39 | for (var i = 0; i < SinLutSize; i++) 40 | { 41 | var angle = i.ToFP() / (SinLutSize - 1) * FP.HalfPi; 42 | 43 | FCordic.SinCosZeroToHalfPi(angle.RawValue, out var sin, out var cos); 44 | 45 | lut[i] = FP.FromRaw(sin); 46 | } 47 | 48 | return lut; 49 | } 50 | 51 | private static FP[] GenerateTanLut() 52 | { 53 | var lut = new FP[TanLutSize + 1]; 54 | lut[^1] = FP.MaxValue; 55 | 56 | for (var i = 0; i < TanLutSize; i++) 57 | { 58 | var angle = i.ToFP() / (TanLutSize - 1) * FP.HalfPi; 59 | 60 | FCordic.SinCosZeroToHalfPi(angle.RawValue, out var sin, out var cos); 61 | 62 | lut[i] = FP.FromRaw(FP.Div(sin, cos)); 63 | } 64 | 65 | return lut; 66 | } 67 | 68 | private static FP[] GenerateAsinLut() 69 | { 70 | var lut = new FP[AsinLutSize + 1]; 71 | lut[^1] = FP.HalfPi; 72 | 73 | for (var i = 0; i < AsinLutSize; i++) 74 | { 75 | var sin = i.ToFP() / (AsinLutSize - 1); 76 | 77 | var angle = FCordic.AsinZeroToOne(sin.RawValue); 78 | 79 | lut[i] = FP.FromRaw(angle); 80 | } 81 | 82 | return lut; 83 | } 84 | 85 | private static long[] GenerateSqrtLut() 86 | { 87 | var lut = new long[SqrtLutSize01 + 1]; 88 | lut[^1] = FP.OneRaw; 89 | 90 | for (var i = 0; i < SqrtLutSize01; i++) 91 | { 92 | var value = i.ToFP() / (SqrtLutSize01 - 1); 93 | 94 | lut[i] = SqrtPrecise(value.RawValue); 95 | } 96 | 97 | return lut; 98 | } 99 | } 100 | } 101 | -------------------------------------------------------------------------------- /Runtime/FMath.Lut.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: af3e291e7c6e43b281677dd4471544ef 3 | timeCreated: 1738500254 -------------------------------------------------------------------------------- /Runtime/FMath.Raw.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Runtime.CompilerServices; 3 | 4 | namespace Mathematics.Fixed 5 | { 6 | public static partial class FMath 7 | { 8 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 9 | public static long SafeNeg(long x) 10 | { 11 | return x == FP.MinValueRaw ? FP.MaxValueRaw : -x; 12 | } 13 | 14 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 15 | public static long CopySign(long to, long from) 16 | { 17 | var signTo = to >> FP.AllBitsWithoutSign; 18 | var absTo = (to + signTo) ^ signTo; 19 | var sign = from >> FP.AllBitsWithoutSign; 20 | return (absTo ^ sign) - sign; 21 | } 22 | 23 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 24 | public static long Floor(long value) 25 | { 26 | return value & FP.IntegerSignMask; 27 | } 28 | 29 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 30 | public static long Abs(long value) 31 | { 32 | var mask = value >> FP.AllBitsWithoutSign; 33 | return (value + mask) ^ mask; 34 | } 35 | 36 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 37 | public static long Sqrt(long x) 38 | { 39 | if (x <= FP.OneRaw) 40 | { 41 | if (x < 0) 42 | { 43 | throw new ArgumentOutOfRangeException(nameof(x), "Negative value passed to Sqrt."); 44 | } 45 | return SqrtLutRaw[(int)(x >> SqrtLutShift01)]; 46 | } 47 | 48 | // Math behind the algorithm: 49 | // x = m x 2^n, where m is in the LUT range [0,1] 50 | // sqrt(x) = sqrt(m) x 2^(n/2) 51 | // n = log2(x) 52 | // m = x / 2^n 53 | // sqrt(m) ≈ LUT(m) 54 | 55 | // Approximate upper bound of log2(x) using bit length. Essentially ceil(log2(x)). 56 | // We just want proper scaling to the LUT range, not a precise log2(x). 57 | var log2 = FP.IntegerBitsWithSign - FP.LeadingZeroCount((ulong)x); 58 | 59 | // Ensure n is even so that no fraction is lost when dividing n by 2. 60 | var n = log2 + (log2 & 1); 61 | var halfN = n >> 1; 62 | 63 | var m = x >> n; 64 | var sqrtM = SqrtLutRaw[(int)(m >> SqrtLutShift01)]; 65 | 66 | return sqrtM << halfN; 67 | } 68 | 69 | /// 70 | /// Calculates the square root of a fixed-point number. 71 | /// Has absolute precision when <= 31. Otherwise 72 | /// may have some rare minor inaccuracies, that are tied to absolute precision.
73 | /// If any, inaccuracies are in range [0, Epsilon * 1000). 74 | /// 75 | /// 76 | /// Thrown if the input is negative. 77 | /// 78 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 79 | public static long SqrtPrecise(long x) 80 | { 81 | if (x < 0) 82 | { 83 | throw new ArgumentOutOfRangeException(nameof(x), "Negative value passed to Sqrt."); 84 | } 85 | 86 | var value = (ulong)x; 87 | var result = 0UL; 88 | 89 | const int correctionForOdd = FP.FractionalBits & 1; 90 | 91 | // Find highest power of 4 <= num. 92 | var bit = 1UL << (FP.AllBits - 2 + correctionForOdd); 93 | while (bit > value) 94 | { 95 | bit >>= 2; 96 | } 97 | 98 | while (bit != 0) 99 | { 100 | var t = result + bit; 101 | result >>= 1; 102 | if (value >= t) 103 | { 104 | value -= t; 105 | result += bit; 106 | } 107 | bit >>= 2; 108 | } 109 | 110 | // & (FP.AllBits - 1) is a correction when FractionalBits == 0. 111 | bit = 1UL << ((FP.FractionalBits - 2 + correctionForOdd) & (FP.AllBits - 1)); 112 | 113 | #pragma warning disable CS0162 // Unreachable code detected 114 | if (FP.FractionalBits < FP.AllBits / 2) // Faster case for FP.FractionalBits <= 31. 115 | { 116 | value <<= FP.FractionalBits; 117 | result <<= FP.FractionalBits; 118 | } 119 | else 120 | { 121 | LeftShift128(out var valueHigh, ref value, FP.FractionalBits); 122 | LeftShift128(out var resultHigh, ref result, FP.FractionalBits); 123 | 124 | var t = result + bit; 125 | 126 | // Exit early if we can continue with a standart 64-bit version. 127 | while (bit != 0 && (valueHigh != 0 || resultHigh != 0 || t < result)) 128 | { 129 | AddToNew128(out var tHigh, out t, ref resultHigh, ref result, bit); 130 | RightShift128(ref resultHigh, ref result, 1); 131 | if (valueHigh > tHigh || (valueHigh == tHigh && value >= t)) 132 | { 133 | Sub128(ref valueHigh, ref value, ref tHigh, ref t); 134 | Add128(ref resultHigh, ref result, bit); 135 | } 136 | bit >>= 2; 137 | } 138 | } 139 | #pragma warning restore CS0162 // Unreachable code detected 140 | 141 | while (bit != 0) 142 | { 143 | var t = result + bit; 144 | result >>= 1; 145 | if (value >= t) 146 | { 147 | value -= t; 148 | result += bit; 149 | } 150 | bit >>= 2; 151 | } 152 | 153 | // Rounding up. 154 | if (value > result) 155 | { 156 | result++; 157 | } 158 | 159 | return (long)result; 160 | 161 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 162 | void LeftShift128(out ulong high, ref ulong low, int shift) 163 | { 164 | high = low >> (FP.AllBits - shift); 165 | low <<= shift; 166 | } 167 | 168 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 169 | void RightShift128(ref ulong high, ref ulong low, int shift) 170 | { 171 | low = (high << (FP.AllBits - shift)) | (low >> shift); 172 | high >>= shift; 173 | } 174 | 175 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 176 | void Add128(ref ulong highA, ref ulong lowA, ulong b) 177 | { 178 | var sum = lowA + b; 179 | if (sum < lowA) 180 | { 181 | ++highA; 182 | } 183 | lowA = sum; 184 | } 185 | 186 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 187 | void AddToNew128(out ulong highC, out ulong lowC, ref ulong highA, ref ulong lowA, ulong b) 188 | { 189 | lowC = lowA + b; 190 | highC = highA; 191 | if (lowC < lowA) 192 | { 193 | ++highC; 194 | } 195 | } 196 | 197 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 198 | void Sub128(ref ulong highA, ref ulong lowA, ref ulong highB, ref ulong lowB) 199 | { 200 | if (lowA < lowB) 201 | { 202 | --highA; 203 | } 204 | lowA -= lowB; 205 | highA -= highB; 206 | } 207 | } 208 | } 209 | } 210 | -------------------------------------------------------------------------------- /Runtime/FMath.Raw.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 89555ea2111b409dab0664401eb67462 3 | timeCreated: 1738864867 -------------------------------------------------------------------------------- /Runtime/FMath.Trig.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Runtime.CompilerServices; 3 | 4 | namespace Mathematics.Fixed 5 | { 6 | public static partial class FMath 7 | { 8 | /// 9 | /// Sin of the angle. 10 | /// Accuracy degrade when operating with huge values. 11 | /// 12 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 13 | public static FP Sin(FAngle angle) 14 | { 15 | return Sin(angle.Radians); 16 | } 17 | 18 | /// 19 | /// Sin of the angle in radians. 20 | /// Accuracy degrade when operating with huge values. 21 | /// 22 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 23 | public static FP Sin(FP radians) 24 | { 25 | var rawRadians = radians.RawValue % FP.TwoPiRaw; // Map to [-2*Pi, 2*Pi) 26 | 27 | if (rawRadians < 0) 28 | { 29 | rawRadians += FP.TwoPiRaw; // Map to [0, 2*Pi) 30 | } 31 | 32 | var flipVertical = rawRadians >= FP.PiRaw; 33 | if (flipVertical) 34 | { 35 | rawRadians -= FP.PiRaw; // Map to [0, Pi) 36 | } 37 | 38 | var flipHorizontal = rawRadians >= FP.HalfPiRaw; 39 | if (flipHorizontal) 40 | { 41 | rawRadians = FP.PiRaw - rawRadians; // Map to [0, Pi/2] 42 | } 43 | 44 | var lutIndex = (int)(rawRadians >> SinLutShift); 45 | 46 | var sinValue = SinLut[lutIndex]; 47 | 48 | return flipVertical ? -sinValue : sinValue; 49 | } 50 | 51 | /// 52 | /// Cos of the angle. 53 | /// Accuracy degrade when operating with huge values. 54 | /// 55 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 56 | public static FP Cos(FAngle angle) 57 | { 58 | return Cos(angle.Radians); 59 | } 60 | 61 | /// 62 | /// Cos of the angle in radians. 63 | /// Accuracy degrade when operating with huge values. 64 | /// 65 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 66 | public static FP Cos(FP radians) 67 | { 68 | var rawRadians = radians.RawValue; 69 | 70 | if (rawRadians > 0) 71 | { 72 | rawRadians += -FP.PiRaw - FP.HalfPiRaw; 73 | } 74 | else 75 | { 76 | rawRadians += FP.HalfPiRaw; 77 | } 78 | 79 | return Sin(FP.FromRaw(rawRadians)); 80 | } 81 | 82 | /// 83 | /// Tan of the angle. 84 | /// Accuracy degrades when operating with huge values, and when the result is big itself. 85 | /// 86 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 87 | public static FP Tan(FAngle angle) 88 | { 89 | return Tan(angle.Radians); 90 | } 91 | 92 | /// 93 | /// Tan of the angle in radians. 94 | /// Accuracy degrades when operating with huge values, and when the result is big itself. 95 | /// 96 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 97 | public static FP Tan(FP radians) 98 | { 99 | var rawRadians = radians.RawValue % FP.PiRaw; // Map to [-Pi, Pi) 100 | 101 | if (rawRadians < 0) 102 | { 103 | rawRadians += FP.PiRaw; // Map to [0, Pi) 104 | } 105 | 106 | var flipVertical = rawRadians >= FP.HalfPiRaw; 107 | if (flipVertical) 108 | { 109 | rawRadians = FP.PiRaw - rawRadians; // Map to [0, Pi/2] 110 | } 111 | 112 | var lutIndex = (int)(rawRadians >> TanLutShift); 113 | 114 | var tanValue = TanLut[lutIndex]; 115 | 116 | return flipVertical ? -tanValue : tanValue; 117 | } 118 | 119 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 120 | public static FP Asin(FP sin) 121 | { 122 | var rawValue = sin.RawValue; 123 | 124 | if (rawValue < -FP.OneRaw || rawValue > FP.OneRaw) 125 | { 126 | throw new ArgumentOutOfRangeException(nameof(sin)); 127 | } 128 | 129 | var flipVertical = rawValue < 0; 130 | if (flipVertical) 131 | { 132 | rawValue = -rawValue; // Map to [0, 1] 133 | } 134 | 135 | var lutIndex = (int)(rawValue >> AsinLutShift); 136 | 137 | var asinValue = AsinLut[lutIndex]; 138 | 139 | return flipVertical ? -asinValue : asinValue; 140 | } 141 | 142 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 143 | public static FP Acos(FP cos) 144 | { 145 | var asin = Asin(cos).RawValue; 146 | return FP.FromRaw(FP.HalfPiRaw - asin); 147 | } 148 | 149 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 150 | public static FP Atan(FP tan) 151 | { 152 | return FP.FromRaw(FCordic.Atan(tan.RawValue)); 153 | } 154 | 155 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 156 | public static FP Atan2(FP y, FP x) 157 | { 158 | return FP.FromRaw(FCordic.Atan2(y.RawValue, x.RawValue)); 159 | } 160 | } 161 | } 162 | -------------------------------------------------------------------------------- /Runtime/FMath.Trig.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 1323dc4276e0461f872a84e3f5d1aaef 3 | timeCreated: 1737303762 -------------------------------------------------------------------------------- /Runtime/FMath.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Runtime.CompilerServices; 3 | using Unity.IL2CPP.CompilerServices; 4 | 5 | namespace Mathematics.Fixed 6 | { 7 | [Il2CppEagerStaticClassConstruction] 8 | [Il2CppSetOption(Option.NullChecks, false)] 9 | [Il2CppSetOption(Option.ArrayBoundsChecks, false)] 10 | [Il2CppSetOption(Option.DivideByZeroChecks, false)] 11 | public static partial class FMath 12 | { 13 | /// 14 | /// Returns a number indicating the sign of a Fix64 number. 15 | /// Returns 1 if the value is positive or 0, and -1 if it is negative. 16 | /// 17 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 18 | public static FP Sign(FP value) 19 | { 20 | return value.RawValue < 0 ? FP.MinusOne : FP.One; 21 | } 22 | 23 | /// 24 | /// Returns a number indicating the sign of a Fix64 number. 25 | /// Returns 1 if the value is positive, 0 if is 0, and -1 if it is negative. 26 | /// 27 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 28 | public static FP SignZero(FP value) 29 | { 30 | if (value.RawValue < 0) 31 | { 32 | return FP.MinusOne; 33 | } 34 | if (value.RawValue > 0) 35 | { 36 | return FP.One; 37 | } 38 | return FP.Zero; 39 | } 40 | 41 | /// 42 | /// Returns a number indicating the sign of a Fix64 number. 43 | /// Returns 1 if the value is positive or 0, and -1 if it is negative. 44 | /// 45 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 46 | public static int SignInt(FP value) 47 | { 48 | return value.RawValue < 0 ? -1 : 1; 49 | } 50 | 51 | /// 52 | /// Returns a number indicating the sign of a Fix64 number. 53 | /// Returns 1 if the value is positive, 0 if is 0, and -1 if it is negative. 54 | /// 55 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 56 | public static int SignZeroInt(FP value) 57 | { 58 | if (value.RawValue < 0) 59 | { 60 | return -1; 61 | } 62 | if (value.RawValue > 0) 63 | { 64 | return 1; 65 | } 66 | return 0; 67 | } 68 | 69 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 70 | public static FP CopySign(FP to, FP from) 71 | { 72 | return FP.FromRaw(CopySign(to.RawValue, from.RawValue)); 73 | } 74 | 75 | /// 76 | /// Returns the absolute value of a Fix64 number. 77 | /// FastAbs(Fix64.MinValue) is undefined. 78 | /// 79 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 80 | public static FP Abs(FP value) 81 | { 82 | var mask = value.RawValue >> FP.AllBitsWithoutSign; 83 | return FP.FromRaw((value.RawValue + mask) ^ mask); 84 | } 85 | 86 | /// 87 | /// Returns the absolute value of a Fix64 number. 88 | /// Note: Abs(Fix64.MinValue) == Fix64.MaxValue. 89 | /// 90 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 91 | public static FP SafeAbs(FP value) 92 | { 93 | if (value.RawValue == FP.MinValueRaw) 94 | { 95 | return FP.MaxValue; 96 | } 97 | 98 | var mask = value.RawValue >> FP.AllBitsWithoutSign; 99 | return FP.FromRaw((value.RawValue + mask) ^ mask); 100 | } 101 | 102 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 103 | public static FP Max(FP x, FP y) 104 | { 105 | var rawResult = x.RawValue; 106 | if (y.RawValue > x.RawValue) 107 | { 108 | rawResult = y.RawValue; 109 | } 110 | return FP.FromRaw(rawResult); 111 | } 112 | 113 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 114 | public static FP Min(FP x, FP y) 115 | { 116 | var rawResult = x.RawValue; 117 | if (y.RawValue < x.RawValue) 118 | { 119 | rawResult = y.RawValue; 120 | } 121 | return FP.FromRaw(rawResult); 122 | } 123 | 124 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 125 | public static FP Clamp(FP value, FP min, FP max) 126 | { 127 | if (value.RawValue < min.RawValue) 128 | { 129 | return min; 130 | } 131 | if (value.RawValue > max.RawValue) 132 | { 133 | return max; 134 | } 135 | return value; 136 | } 137 | 138 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 139 | public static FP Clamp01(FP value) 140 | { 141 | if (value.RawValue < 0) 142 | { 143 | return FP.Zero; 144 | } 145 | if (value.RawValue > FP.OneRaw) 146 | { 147 | return FP.One; 148 | } 149 | return value; 150 | } 151 | 152 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 153 | public static FP Lerp(FP start, FP end, FP t) 154 | { 155 | if (t.RawValue < 0) 156 | { 157 | t.RawValue = 0L; 158 | } 159 | if (t.RawValue > FP.OneRaw) 160 | { 161 | t.RawValue = FP.OneRaw; 162 | } 163 | return start + t * (end - start); 164 | } 165 | 166 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 167 | public static FP LerpUnclamped(FP start, FP end, FP t) 168 | { 169 | return start + t * (end - start); 170 | } 171 | 172 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 173 | public static FP MoveTowards(FP current, FP target, FP maxDelta) 174 | { 175 | return Abs(target - current) <= maxDelta ? target : current + SignInt(target - current) * maxDelta; 176 | } 177 | 178 | /// 179 | /// Compares two values with some epsilon and returns true if they are similar. 180 | /// 181 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 182 | public static bool ApproximatelyEqual(FP x, FP y) 183 | { 184 | return ApproximatelyEqual(x, y, FP.CalculationsEpsilonSqr); 185 | } 186 | 187 | /// 188 | /// Compares two values with some epsilon and returns true if they are similar. 189 | /// 190 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 191 | public static bool ApproximatelyEqual(FP x, FP y, FP epsilon) 192 | { 193 | var difference = Abs(x - y); 194 | return difference <= epsilon; 195 | } 196 | 197 | /// 198 | /// Returns the largest integer less than or equal to the specified number. 199 | /// 200 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 201 | public static FP Floor(FP value) 202 | { 203 | return FP.FromRaw(value.RawValue & FP.IntegerSignMask); 204 | } 205 | 206 | /// 207 | /// Returns the largest integer less than or equal to the specified number. 208 | /// 209 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 210 | public static int FloorToInt(FP value) 211 | { 212 | return value.ToInt(); 213 | } 214 | 215 | /// 216 | /// Returns the smallest integral value that is greater than or equal to the specified number. 217 | /// 218 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 219 | public static FP Ceil(FP value) 220 | { 221 | var hasFractionalPart = (value.RawValue & FP.FractionalMask) != 0; 222 | return hasFractionalPart ? Floor(value) + FP.One : value; 223 | } 224 | 225 | /// 226 | /// Returns the smallest integral value that is greater than or equal to the specified number. 227 | /// 228 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 229 | public static int CeilToInt(FP value) 230 | { 231 | var hasFractionalPart = (value.RawValue & FP.FractionalMask) != 0; 232 | return hasFractionalPart ? value.ToInt() + 1 : value.ToInt(); 233 | } 234 | 235 | /// 236 | /// Rounds a value to the nearest integral value. 237 | /// If the value is halfway between an even and an uneven value, returns the even value. 238 | /// 239 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 240 | public static FP Round(FP value) 241 | { 242 | var fractionalPart = value.RawValue & FP.FractionalMask; 243 | var integralPart = Floor(value); 244 | if (fractionalPart < FP.HalfRaw) 245 | { 246 | return integralPart; 247 | } 248 | 249 | if (fractionalPart > FP.HalfRaw) 250 | { 251 | return integralPart + FP.One; 252 | } 253 | 254 | // If number is halfway between two values, round to the nearest even number. 255 | // This is the method used by System.Math.Round(). 256 | return (integralPart.RawValue & FP.OneRaw) == 0 257 | ? integralPart 258 | : integralPart + FP.One; 259 | } 260 | 261 | /// 262 | /// Rounds a value to the nearest integral value. 263 | /// If the value is halfway between an even and an uneven value, returns the even value. 264 | /// 265 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 266 | public static int RoundToInt(FP value) 267 | { 268 | return Round(value).ToInt(); 269 | } 270 | 271 | /// 272 | /// Calculates the square root of a fixed-point number, using LUT. 273 | /// Accuracy degrade when operating with huge values. 274 | /// 275 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 276 | public static FP Sqrt(FP x) 277 | { 278 | return FP.FromRaw(Sqrt(x.RawValue)); 279 | } 280 | 281 | /// 282 | /// Calculates the square root of a fixed-point number. 283 | /// Has absolute precision when <= 31. Otherwise 284 | /// may have some rare minor inaccuracies, that are tied to absolute precision.
285 | /// If any, inaccuracies are in range [0, Epsilon * 1000). 286 | /// 287 | /// 288 | /// Thrown if the input is negative. 289 | /// 290 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 291 | public static FP SqrtPrecise(FP x) 292 | { 293 | return FP.FromRaw(SqrtPrecise(x.RawValue)); 294 | } 295 | 296 | /// 297 | /// Returns 2 raised to the specified power. 298 | /// Provides at least 6 decimals of accuracy. 299 | /// 300 | public static FP Pow2(FP x) 301 | { 302 | if (x.RawValue == 0) 303 | { 304 | return FP.One; 305 | } 306 | 307 | // Avoid negative arguments by exploiting that exp(-x) = 1/exp(x). 308 | var neg = x.RawValue < 0; 309 | if (neg) 310 | { 311 | x = -x; 312 | } 313 | 314 | if (x == FP.One) 315 | { 316 | return neg ? FP.One / 2 : FP.Two; 317 | } 318 | 319 | if (x >= FP.Log2Max) 320 | { 321 | return neg ? FP.One / FP.MaxValue : FP.MaxValue; 322 | } 323 | 324 | if (x <= FP.Log2Min) 325 | { 326 | return neg ? FP.MaxValue : FP.Zero; 327 | } 328 | 329 | /* The algorithm is based on the power series for exp(x): 330 | * http://en.wikipedia.org/wiki/Exponential_function#Formal_definition 331 | * 332 | * From term n, we get term n+1 by multiplying with x/n. 333 | * When the sum term drops to zero, we can stop summing. 334 | */ 335 | 336 | var integerPart = Floor(x).ToInt(); 337 | // Take fractional part of exponent 338 | x = FP.FromRaw(x.RawValue & FP.FractionalMask); 339 | 340 | var result = FP.One; 341 | var term = FP.One; 342 | var i = 1; 343 | while (term.RawValue != 0) 344 | { 345 | term = x * term * FP.Ln2 / i; 346 | result += term; 347 | i++; 348 | } 349 | 350 | result = FP.FromRaw(result.RawValue << integerPart); 351 | if (neg) 352 | { 353 | result = FP.One / result; 354 | } 355 | 356 | return result; 357 | } 358 | 359 | /// 360 | /// Returns the base-2 logarithm of a specified number. 361 | /// Provides at least 9 decimals of accuracy. 362 | /// 363 | /// 364 | /// The argument was non-positive 365 | /// 366 | internal static FP Log2(FP x) 367 | { 368 | if (x.RawValue <= 0) 369 | { 370 | throw new ArgumentOutOfRangeException("Non-positive value passed to Ln", "x"); 371 | } 372 | 373 | // This implementation is based on Clay. S. Turner's fast binary logarithm 374 | // algorithm (C. S. Turner, "A Fast Binary Logarithm Algorithm", IEEE Signal 375 | // Processing Mag., pp. 124,140, Sep. 2010.) 376 | 377 | long b = 1U << (FP.FractionalBits - 1); 378 | long y = 0; 379 | 380 | var rawX = x.RawValue; 381 | while (rawX < FP.OneRaw) 382 | { 383 | rawX <<= 1; 384 | y -= FP.OneRaw; 385 | } 386 | 387 | while (rawX >= (FP.OneRaw << 1)) 388 | { 389 | rawX >>= 1; 390 | y += FP.OneRaw; 391 | } 392 | 393 | var z = FP.FromRaw(rawX); 394 | 395 | for (var i = 0; i < FP.FractionalBits; i++) 396 | { 397 | z = z * z; 398 | if (z.RawValue >= (FP.OneRaw << 1)) 399 | { 400 | z = FP.FromRaw(z.RawValue >> 1); 401 | y += b; 402 | } 403 | 404 | b >>= 1; 405 | } 406 | 407 | return FP.FromRaw(y); 408 | } 409 | 410 | /// 411 | /// Returns the natural logarithm of a specified number. 412 | /// Provides at least 7 decimals of accuracy. 413 | /// 414 | /// 415 | /// The argument was non-positive 416 | /// 417 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 418 | public static FP Ln(FP x) 419 | { 420 | return Log2(x) * FP.Ln2; 421 | } 422 | 423 | /// 424 | /// Returns a specified number raised to the specified power. 425 | /// Provides about 5 digits of accuracy for the result. 426 | /// 427 | /// 428 | /// The base was zero, with a negative exponent 429 | /// 430 | /// 431 | /// The base was negative, with a non-zero exponent 432 | /// 433 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 434 | public static FP Pow(FP b, FP exp) 435 | { 436 | if (b == FP.One) 437 | { 438 | return FP.One; 439 | } 440 | 441 | if (exp.RawValue == 0) 442 | { 443 | return FP.One; 444 | } 445 | 446 | if (b.RawValue == 0) 447 | { 448 | if (exp.RawValue < 0) 449 | { 450 | throw new DivideByZeroException(); 451 | } 452 | 453 | return FP.Zero; 454 | } 455 | 456 | var log2 = Log2(b); 457 | return Pow2(exp * log2); 458 | } 459 | } 460 | } 461 | -------------------------------------------------------------------------------- /Runtime/FMath.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: e29075f1f0584422bad011d775af4019 3 | timeCreated: 1683039520 -------------------------------------------------------------------------------- /Runtime/FP.Constants.cs: -------------------------------------------------------------------------------- 1 | using System.Runtime.CompilerServices; 2 | 3 | namespace Mathematics.Fixed 4 | { 5 | public partial struct FP 6 | { 7 | public const int AllBits = sizeof(long) * 8; 8 | 9 | public const int AllBitsWithoutSign = AllBits - 1; 10 | public const int IntegerBits = AllBitsWithoutSign - FractionalBits; 11 | public const int IntegerBitsWithSign = IntegerBits + 1; 12 | public const long FractionalMask = long.MaxValue >> (AllBitsWithoutSign - FractionalBits); 13 | public const long IntegerSignMask = long.MaxValue << FractionalBits; 14 | public const long IntegerFractionalMask = long.MaxValue; 15 | public const long SignMask = long.MinValue; 16 | 17 | public const long EpsilonRaw = 1L; 18 | public const long CalculationsEpsilonSqrRaw = EpsilonRaw * CalculationsEpsilonScaling; 19 | public const long CalculationsEpsilonRaw = CalculationsEpsilonSqrRaw * CalculationsEpsilonScaling; 20 | 21 | public const long MaxValueRaw = long.MaxValue; 22 | public const long MinValueRaw = long.MinValue; 23 | 24 | public const long OneRaw = 1L << FractionalBits; 25 | public const long MinusOneRaw = IntegerSignMask; 26 | public const long TwoRaw = OneRaw * 2; 27 | public const long ThreeRaw = OneRaw * 3; 28 | public const long HalfRaw = OneRaw / 2; 29 | public const long QuarterRaw = OneRaw / 4; 30 | 31 | // 61 is the largest N such that (long)(PI * 2^N) < long.MaxValue. 32 | public const long PiBase61 = 7244019458077122560L; // (long)(3.141592653589793 * (1L << 61)); 33 | public const long PiRaw = PiBase61 >> (61 - FractionalBits); 34 | public const long HalfPiRaw = PiRaw / 2; 35 | public const long TwoPiRaw = PiRaw * 2; 36 | 37 | public const long Deg2RadBase61 = 40244552544872904L; // (long)(0.017453292519943295 * (1L << 61)); 38 | public const long Rad2DegBase57 = 8257192040480628736L; // (long)(57.29577951308232 * (1L << 57)); 39 | public const long Deg2RadRaw = Deg2RadBase61 >> (61 - FractionalBits); 40 | public const long Rad2DegRaw = Rad2DegBase57 >> (57 - FractionalBits); 41 | 42 | public const long Ln2Base61 = 1598288580650331904L; // (long)(0.6931471805599453 * (1L << 61)); 43 | public const long Ln2Raw = Ln2Base61 >> (61 - FractionalBits); 44 | public const long Log2MaxRaw = IntegerBits * OneRaw; 45 | public const long Log2MinRaw = -(IntegerBits + 1) * OneRaw; 46 | 47 | public const long Sqrt2Base61 = 3260954456333195264L; // (long)(1.414213562373095 * (1L << 61)); 48 | public const long Sqrt2Raw = Sqrt2Base61 >> (61 - FractionalBits); 49 | 50 | public static FP Epsilon 51 | { 52 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 53 | get => FromRaw(EpsilonRaw); 54 | } 55 | 56 | /// 57 | /// Epsilon for linear operations. 58 | /// 59 | public static FP CalculationsEpsilon 60 | { 61 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 62 | get => FromRaw(CalculationsEpsilonRaw); 63 | } 64 | 65 | /// 66 | /// More precise epsilon for operations with squares involved. 67 | /// 68 | public static FP CalculationsEpsilonSqr 69 | { 70 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 71 | get => FromRaw(CalculationsEpsilonSqrRaw); 72 | } 73 | 74 | public static FP MaxValue 75 | { 76 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 77 | get => FromRaw(MaxValueRaw); 78 | } 79 | 80 | public static FP MinValue 81 | { 82 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 83 | get => FromRaw(MinValueRaw); 84 | } 85 | 86 | public static FP One 87 | { 88 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 89 | get => FromRaw(OneRaw); 90 | } 91 | 92 | public static FP Two 93 | { 94 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 95 | get => FromRaw(TwoRaw); 96 | } 97 | 98 | public static FP Three 99 | { 100 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 101 | get => FromRaw(TwoRaw); 102 | } 103 | 104 | public static FP Zero 105 | { 106 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 107 | get => FromRaw(0L); 108 | } 109 | 110 | public static FP MinusOne 111 | { 112 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 113 | get => FromRaw(MinusOneRaw); 114 | } 115 | 116 | public static FP Half 117 | { 118 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 119 | get => FromRaw(HalfRaw); 120 | } 121 | 122 | public static FP Quarter 123 | { 124 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 125 | get => FromRaw(QuarterRaw); 126 | } 127 | 128 | public static FP Pi 129 | { 130 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 131 | get => FromRaw(PiRaw); 132 | } 133 | 134 | public static FP HalfPi 135 | { 136 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 137 | get => FromRaw(HalfPiRaw); 138 | } 139 | 140 | public static FP TwoPi 141 | { 142 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 143 | get => FromRaw(TwoPiRaw); 144 | } 145 | 146 | public static FP Rad2Deg 147 | { 148 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 149 | get => FromRaw(Rad2DegRaw); 150 | } 151 | 152 | public static FP Deg2Rad 153 | { 154 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 155 | get => FromRaw(Deg2RadRaw); 156 | } 157 | 158 | /// 159 | /// Represents the maximum number for which a logarithmic operation in base 2 is valid or meaningful in this system. 160 | /// 161 | public static FP Log2Max 162 | { 163 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 164 | get => FromRaw(Log2MaxRaw); 165 | } 166 | 167 | /// 168 | /// Represents the minimum valid value (or negative logarithm) in base 2 for the system. 169 | /// 170 | public static FP Log2Min 171 | { 172 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 173 | get => FromRaw(Log2MinRaw); 174 | } 175 | 176 | public static FP Ln2 177 | { 178 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 179 | get => FromRaw(Ln2Raw); 180 | } 181 | } 182 | } 183 | -------------------------------------------------------------------------------- /Runtime/FP.Constants.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 2a7512037d624efb8df03a8008ad9a10 3 | timeCreated: 1737301840 -------------------------------------------------------------------------------- /Runtime/FP.Raw.cs: -------------------------------------------------------------------------------- 1 | using System.Runtime.CompilerServices; 2 | 3 | namespace Mathematics.Fixed 4 | { 5 | public partial struct FP 6 | { 7 | /// 8 | /// Adds x and y. Performs saturating addition, i.e. in case of overflow, 9 | /// rounds to MinValue or MaxValue depending on sign of operands. 10 | /// 11 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 12 | public static long Add(long x, long y) 13 | { 14 | var sum = x + y; 15 | // Overflow occurs if x and y have the same sign and sum has a different sign. 16 | if ((~(x ^ y) & (x ^ sum)) < 0) 17 | { 18 | sum = x > 0 ? MaxValueRaw : MinValueRaw; 19 | } 20 | 21 | return sum; 22 | } 23 | 24 | /// 25 | /// Subtracts y from x. Performs saturating substraction, i.e. in case of overflow, 26 | /// rounds to MinValue or MaxValue depending on sign of operands. 27 | /// 28 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 29 | public static long Sub(long x, long y) 30 | { 31 | var diff = x - y; 32 | // Overflow occurs if x and y have different signs and x and diff have different signs. 33 | if (((x ^ y) & (x ^ diff)) < 0) 34 | { 35 | diff = x < 0 ? MinValueRaw : MaxValueRaw; 36 | } 37 | 38 | return diff; 39 | } 40 | 41 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 42 | public static long Negate(long x) 43 | { 44 | return x == MinValueRaw ? MaxValueRaw : -x; 45 | } 46 | 47 | /// 48 | /// Performs multiplication with overflow checking.
49 | /// 50 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 51 | public static long Mul(long x, long y) 52 | { 53 | var maskX = x >> AllBitsWithoutSign; 54 | var maskY = y >> AllBitsWithoutSign; 55 | var absX = (ulong)((x + maskX) ^ maskX); 56 | var absY = (ulong)((y + maskY) ^ maskY); 57 | 58 | var sign = maskX ^ maskY; 59 | 60 | Mul64To128(absX, absY, out ulong hi, out ulong lo); 61 | 62 | var shiftedHi = hi >> FractionalBits; 63 | var shiftedLo = hi << (AllBits - FractionalBits) | lo >> FractionalBits; 64 | 65 | if (shiftedLo > MaxValueRaw || shiftedHi > 0) 66 | { 67 | return sign < 0 ? MinValueRaw : MaxValueRaw; 68 | } 69 | 70 | return ((long)shiftedLo ^ sign) - sign; 71 | } 72 | 73 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 74 | public static long MulScalar(long x, int scalar) 75 | { 76 | var result = x * scalar; 77 | var expectedSign = x ^ scalar; 78 | 79 | // Overflow occurs if expected and result sign are different. 80 | if (result != 0 && (expectedSign ^ result) < 0) 81 | { 82 | return expectedSign > 0 ? MaxValueRaw : MinValueRaw; 83 | } 84 | 85 | return result; 86 | } 87 | 88 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 89 | public static long DivScalar(long x, int scalar) 90 | { 91 | // There is a single edge case. 92 | return x == MinValueRaw && scalar == -1 ? MaxValueRaw : x / scalar; 93 | } 94 | 95 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 96 | public static long Mod(long x, long y) 97 | { 98 | // There is a single edge case. 99 | return x == MinValueRaw & y == -1 ? 0 : x % y; 100 | } 101 | 102 | /// 103 | /// Performs division with overflow checking.
104 | /// Precision loss may occur when the numerator and denominator have significantly different magnitudes. 105 | /// 106 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 107 | public static long Div(long x, long y) 108 | { 109 | if (x == 0) 110 | { 111 | return 0; 112 | } 113 | 114 | var maskX = x >> AllBitsWithoutSign; 115 | var maskY = y >> AllBitsWithoutSign; 116 | var absX = (ulong)((x + maskX) ^ maskX); 117 | var absY = (ulong)((y + maskY) ^ maskY); 118 | var sign = maskX ^ maskY; 119 | 120 | var xLzc = LeadingZeroCount(absX); 121 | var xShift = xLzc > FractionalBits ? FractionalBits : xLzc; 122 | var yShift = FractionalBits - xShift; 123 | 124 | var scaledX = absX << xShift; 125 | var scaledY = absY >> yShift; 126 | 127 | var nonZeroY = scaledY == 0 ? 1 : scaledY; 128 | 129 | var quotient = scaledX / nonZeroY; 130 | 131 | var overflowValue = sign < 0 ? MinValueRaw : MaxValueRaw; 132 | var signedQuotient = ((long)quotient ^ sign) - sign; 133 | var result = quotient > MaxValueRaw ? overflowValue : signedQuotient; 134 | 135 | var finalResult = scaledY == 0 ? overflowValue : result; 136 | 137 | return finalResult; 138 | } 139 | 140 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 141 | public static long DivRem(long x, long y, out long reminder) 142 | { 143 | if (x == 0) 144 | { 145 | reminder = y; 146 | return 0; 147 | } 148 | 149 | var maskX = x >> AllBitsWithoutSign; 150 | var maskY = y >> AllBitsWithoutSign; 151 | var absX = (ulong)((x + maskX) ^ maskX); 152 | var absY = (ulong)((y + maskY) ^ maskY); 153 | var sign = maskX ^ maskY; 154 | 155 | var xLzc = LeadingZeroCount(absX); 156 | var xShift = xLzc > FractionalBits ? FractionalBits : xLzc; 157 | var yShift = FractionalBits - xShift; 158 | 159 | var scaledX = absX << xShift; 160 | var scaledY = absY >> yShift; 161 | 162 | if (scaledY == 0) 163 | { 164 | reminder = 0; 165 | return sign < 0 ? MinValueRaw : MaxValueRaw; 166 | } 167 | 168 | var quotient = scaledX / scaledY; 169 | 170 | if (quotient > MaxValueRaw) 171 | { 172 | reminder = 0; 173 | return sign < 0 ? MinValueRaw : MaxValueRaw; 174 | } 175 | 176 | reminder = ((long)(scaledX - quotient * scaledY) ^ sign) - sign; 177 | return ((long)quotient ^ sign) - sign; 178 | } 179 | 180 | /// 181 | /// All credits goes to this guy: 182 | /// https://www.codeproject.com/Tips/618570/UInt-Multiplication-Squaring 183 | /// 184 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 185 | private static void Mul64To128(ulong op1, ulong op2, out ulong hi, out ulong lo) 186 | { 187 | var u1 = op1 & 0xFFFFFFFF; 188 | var v1 = op2 & 0xFFFFFFFF; 189 | var t = u1 * v1; 190 | var w3 = t & 0xFFFFFFFF; 191 | var k = t >> 32; 192 | 193 | op1 >>= 32; 194 | t = op1 * v1 + k; 195 | k = t & 0xFFFFFFFF; 196 | var w1 = t >> 32; 197 | 198 | op2 >>= 32; 199 | t = u1 * op2 + k; 200 | k = t >> 32; 201 | 202 | hi = op1 * op2 + w1 + k; 203 | lo = (t << 32) | w3; 204 | } 205 | 206 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 207 | public static int LeadingZeroCount(ulong x) 208 | { 209 | var hi = (uint)(x >> 32); 210 | 211 | if (hi == 0) 212 | { 213 | return 32 + LeadingZeroCount((uint)x); 214 | } 215 | else 216 | { 217 | return LeadingZeroCount(hi); 218 | } 219 | } 220 | 221 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 222 | public static int LeadingZeroCount(uint x) 223 | { 224 | x |= x >> 1; 225 | x |= x >> 2; 226 | x |= x >> 4; 227 | x |= x >> 8; 228 | x |= x >> 16; 229 | 230 | x -= x >> 1 & 0x55555555; 231 | x = (x >> 2 & 0x33333333) + (x & 0x33333333); 232 | x = (x >> 4) + x & 0x0f0f0f0f; 233 | x += x >> 8; 234 | x += x >> 16; 235 | 236 | return 32 - (int)(x & 0x3F); 237 | } 238 | } 239 | } 240 | -------------------------------------------------------------------------------- /Runtime/FP.Raw.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: a758a8fde9f94340ab6c168f39cc9c3b 3 | timeCreated: 1738577072 -------------------------------------------------------------------------------- /Runtime/FP.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Runtime.CompilerServices; 3 | using Unity.IL2CPP.CompilerServices; 4 | 5 | namespace Mathematics.Fixed 6 | { 7 | [Il2CppSetOption(Option.NullChecks, false)] 8 | [Il2CppSetOption(Option.ArrayBoundsChecks, false)] 9 | [Il2CppSetOption(Option.DivideByZeroChecks, false)] 10 | [Serializable] 11 | public partial struct FP : IEquatable, IComparable, IFormattable 12 | { 13 | public const int FractionalBits = 31; 14 | public const int CalculationsEpsilonScaling = 10; 15 | 16 | public long RawValue; 17 | 18 | private FP(long rawValue) 19 | { 20 | RawValue = rawValue; 21 | } 22 | 23 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 24 | public static unsafe FP FromRaw(long rawValue) 25 | { 26 | return *(FP*)(&rawValue); 27 | } 28 | 29 | /// 30 | /// Adds x and y without performing overflow checking. 31 | /// 32 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 33 | public static FP operator +(FP x, FP y) 34 | { 35 | var result = Add(x.RawValue, y.RawValue); 36 | return FromRaw(result); 37 | } 38 | 39 | /// 40 | /// Subtracts y from x without performing overflow checking. 41 | /// 42 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 43 | public static FP operator -(FP x, FP y) 44 | { 45 | var result = Sub(x.RawValue, y.RawValue); 46 | return FromRaw(result); 47 | } 48 | 49 | /// 50 | /// Performs multiplication with checking for overflow. 51 | /// 52 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 53 | public static FP operator *(FP x, FP y) 54 | { 55 | var result = Mul(x.RawValue, y.RawValue); 56 | return FromRaw(result); 57 | } 58 | 59 | /// 60 | /// Performs fast multiplication with checking for overflow. 61 | /// 62 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 63 | public static FP operator *(FP x, int scalar) 64 | { 65 | var result = MulScalar(x.RawValue, scalar); 66 | return FromRaw(result); 67 | } 68 | 69 | /// 70 | /// Performs fast multiplication with checking for overflow. 71 | /// 72 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 73 | public static FP operator *(int scalar, FP x) 74 | { 75 | return x * scalar; 76 | } 77 | 78 | /// 79 | /// Division by 0 returns min or max value, depending on the sign of the numerator. 80 | /// It has some rare minor inaccuracies, and they are tied to absolute precision.
81 | /// Inaccuracies are in range [0, Epsilon * 2000). 82 | /// 83 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 84 | public static FP operator /(FP x, FP y) 85 | { 86 | var result = Div(x.RawValue, y.RawValue); 87 | return FromRaw(result); 88 | } 89 | 90 | /// 91 | /// Performs fast division with checking for overflow. 92 | /// 93 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 94 | public static FP operator /(FP x, int scalar) 95 | { 96 | var result = DivScalar(x.RawValue, scalar); 97 | return FromRaw(result); 98 | } 99 | 100 | /// 101 | /// Performs modulo with checking for overflow. 102 | /// 103 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 104 | public static FP operator %(FP x, FP y) 105 | { 106 | var result = Mod(x.RawValue, y.RawValue); 107 | return FromRaw(result); 108 | } 109 | 110 | /// 111 | /// Negate x with overflow checking. 112 | /// 113 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 114 | public static FP operator -(FP x) 115 | { 116 | var result = Negate(x.RawValue); 117 | return FromRaw(result); 118 | } 119 | 120 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 121 | public static bool operator >(FP x, FP y) 122 | { 123 | return x.RawValue > y.RawValue; 124 | } 125 | 126 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 127 | public static bool operator >(FP x, int y) 128 | { 129 | return x.RawValue > (long)y << FractionalBits; 130 | } 131 | 132 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 133 | public static bool operator >(int y, FP x) 134 | { 135 | return (long)y << FractionalBits > x.RawValue; 136 | } 137 | 138 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 139 | public static bool operator <(FP x, FP y) 140 | { 141 | return x.RawValue < y.RawValue; 142 | } 143 | 144 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 145 | public static bool operator <(FP x, int y) 146 | { 147 | return x.RawValue < (long)y << FractionalBits; 148 | } 149 | 150 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 151 | public static bool operator <(int y, FP x) 152 | { 153 | return (long)y << FractionalBits < x.RawValue; 154 | } 155 | 156 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 157 | public static bool operator >=(FP x, FP y) 158 | { 159 | return x.RawValue >= y.RawValue; 160 | } 161 | 162 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 163 | public static bool operator >=(FP x, int y) 164 | { 165 | return x.RawValue >= (long)y << FractionalBits; 166 | } 167 | 168 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 169 | public static bool operator >=(int y, FP x) 170 | { 171 | return (long)y << FractionalBits >= x.RawValue; 172 | } 173 | 174 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 175 | public static bool operator <=(FP x, FP y) 176 | { 177 | return x.RawValue <= y.RawValue; 178 | } 179 | 180 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 181 | public static bool operator <=(FP x, int y) 182 | { 183 | return x.RawValue <= (long)y << FractionalBits; 184 | } 185 | 186 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 187 | public static bool operator <=(int y, FP x) 188 | { 189 | return (long)y << FractionalBits <= x.RawValue; 190 | } 191 | 192 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 193 | public static bool operator ==(FP x, FP y) 194 | { 195 | return x.RawValue == y.RawValue; 196 | } 197 | 198 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 199 | public static bool operator ==(FP a, int b) 200 | { 201 | return a.RawValue == (long)b << FractionalBits; 202 | } 203 | 204 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 205 | public static bool operator ==(int b, FP a) 206 | { 207 | return (long)b << FractionalBits == a.RawValue; 208 | } 209 | 210 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 211 | public static bool operator !=(FP x, FP y) 212 | { 213 | return x.RawValue != y.RawValue; 214 | } 215 | 216 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 217 | public static bool operator !=(FP a, int b) 218 | { 219 | return a.RawValue != (long)b << FractionalBits; 220 | } 221 | 222 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 223 | public static bool operator !=(int b, FP a) 224 | { 225 | return (long)b << FractionalBits != a.RawValue; 226 | } 227 | 228 | public override bool Equals(object obj) 229 | { 230 | return obj is FP fp && fp.RawValue == RawValue; 231 | } 232 | 233 | public override int GetHashCode() 234 | { 235 | return RawValue.GetHashCode(); 236 | } 237 | 238 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 239 | public bool Equals(FP other) 240 | { 241 | return RawValue == other.RawValue; 242 | } 243 | 244 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 245 | public int CompareTo(FP other) 246 | { 247 | return RawValue.CompareTo(other.RawValue); 248 | } 249 | 250 | public string ToString(string format, IFormatProvider formatProvider) => 251 | this.ToDouble().ToString(format, formatProvider); 252 | 253 | public string ToString(string format) => this.ToDouble().ToString(format); 254 | 255 | public string ToString(IFormatProvider provider) => this.ToDouble().ToString(provider); 256 | 257 | public override string ToString() => this.ToDouble().ToString("G", System.Globalization.CultureInfo.InvariantCulture); 258 | } 259 | } 260 | -------------------------------------------------------------------------------- /Runtime/FP.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 5ea5081c23054c87ba641e3c09075ebd 3 | timeCreated: 1682947655 -------------------------------------------------------------------------------- /Runtime/FixedPoint.asmdef: -------------------------------------------------------------------------------- 1 | { 2 | "name": "FixedPoint", 3 | "rootNamespace": "FixedPoint", 4 | "references": [], 5 | "includePlatforms": [], 6 | "excludePlatforms": [], 7 | "allowUnsafeCode": true, 8 | "overrideReferences": false, 9 | "precompiledReferences": [], 10 | "autoReferenced": true, 11 | "defineConstraints": [], 12 | "versionDefines": [], 13 | "noEngineReferences": true 14 | } -------------------------------------------------------------------------------- /Runtime/FixedPoint.asmdef.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 401db8312f319734b8ce2e50f0180af2 3 | AssemblyDefinitionImporter: 4 | externalObjects: {} 5 | userData: 6 | assetBundleName: 7 | assetBundleVariant: 8 | -------------------------------------------------------------------------------- /Runtime/IL2CPP.cs: -------------------------------------------------------------------------------- 1 | // ReSharper disable UnusedAutoPropertyAccessor.Global 2 | 3 | namespace Unity.IL2CPP.CompilerServices 4 | { 5 | using System; 6 | 7 | [Flags] 8 | internal enum Option 9 | { 10 | NullChecks = 1, 11 | ArrayBoundsChecks = 2, 12 | DivideByZeroChecks = 3 13 | } 14 | 15 | [AttributeUsage(AttributeTargets.Class | AttributeTargets.Struct | AttributeTargets.Method | AttributeTargets.Property, AllowMultiple = true)] 16 | internal class Il2CppSetOptionAttribute : Attribute 17 | { 18 | public Option Option { get; } 19 | public object Value { get; } 20 | 21 | public Il2CppSetOptionAttribute(Option option, object value) 22 | { 23 | Option = option; 24 | Value = value; 25 | } 26 | } 27 | 28 | [AttributeUsage(AttributeTargets.Class | AttributeTargets.Struct, Inherited = false)] 29 | internal class Il2CppEagerStaticClassConstructionAttribute : Attribute 30 | { 31 | } 32 | } 33 | -------------------------------------------------------------------------------- /Runtime/IL2CPP.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: f203b11044224b0784cb8edc586b1f44 3 | timeCreated: 1738616113 -------------------------------------------------------------------------------- /Runtime/Structs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: dbf889a579e14ab3b362b1db21d8614e 3 | timeCreated: 1737208040 -------------------------------------------------------------------------------- /Runtime/Structs/FAngle.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Runtime.CompilerServices; 3 | 4 | namespace Mathematics.Fixed 5 | { 6 | /// 7 | /// Representation of an angle. 8 | /// 9 | [Serializable] 10 | public struct FAngle : IEquatable 11 | { 12 | public FP Radians; 13 | 14 | private FAngle(FP radians) 15 | { 16 | Radians = radians; 17 | } 18 | 19 | public FP Degrees 20 | { 21 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 22 | get => Radians * FP.Rad2Deg; 23 | } 24 | 25 | public FRotation2D Counterclockwise 26 | { 27 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 28 | get => new FRotation2D(this); 29 | } 30 | 31 | public FRotation2D Clockwise 32 | { 33 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 34 | get => new FRotation2D(new FAngle(-Radians)); 35 | } 36 | 37 | public static FAngle Zero 38 | { 39 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 40 | get => new FAngle(FP.Zero); 41 | } 42 | 43 | public static FAngle TwoPI 44 | { 45 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 46 | get => new FAngle(FP.TwoPi); 47 | } 48 | 49 | public static FAngle HalfPI 50 | { 51 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 52 | get => new FAngle(FP.HalfPi); 53 | } 54 | 55 | public static FAngle PI 56 | { 57 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 58 | get => new FAngle(FP.Pi); 59 | } 60 | 61 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 62 | public static FAngle FromDegrees(FP degrees) 63 | { 64 | return new FAngle(degrees * FP.Deg2Rad); 65 | } 66 | 67 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 68 | public static FAngle FromRadians(FP radians) 69 | { 70 | return new FAngle(radians); 71 | } 72 | 73 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 74 | public static FAngle Normalize360(FAngle angle) 75 | { 76 | var angleRad = angle.Radians % FP.TwoPi; 77 | 78 | if (angleRad < FP.Zero) 79 | { 80 | angleRad += FP.TwoPi; 81 | } 82 | else if (angleRad >= FP.TwoPi) 83 | { 84 | angleRad -= FP.TwoPi; 85 | } 86 | 87 | return new FAngle(angleRad); 88 | } 89 | 90 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 91 | public static FAngle Normalize180(FAngle angle) 92 | { 93 | var angleRad = angle.Radians % FP.TwoPi; 94 | 95 | if (angleRad <= -FP.Pi) 96 | { 97 | angleRad += FP.TwoPi; 98 | } 99 | else if (angleRad > FP.Pi) 100 | { 101 | angleRad -= FP.TwoPi; 102 | } 103 | 104 | return new FAngle(angleRad); 105 | } 106 | 107 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 108 | public static FAngle Lerp360(FAngle from, FAngle to, FP factor) 109 | { 110 | var difference = Normalize180(to - from); 111 | 112 | return Normalize360(from + difference * FMath.Clamp01(factor)); 113 | } 114 | 115 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 116 | public static FAngle Lerp180(FAngle from, FAngle to, FP factor) 117 | { 118 | var difference = Normalize180(to - from); 119 | 120 | return Normalize180(from + difference * FMath.Clamp01(factor)); 121 | } 122 | 123 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 124 | public static FAngle Abs(FAngle angle) 125 | { 126 | return new FAngle(FMath.Abs(angle.Radians)); 127 | } 128 | 129 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 130 | public static FAngle Delta(FAngle from, FAngle to) 131 | { 132 | return Normalize180(to - from); 133 | } 134 | 135 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 136 | public static FAngle UnsignedDelta(FAngle from, FAngle to) 137 | { 138 | return Abs(Delta(from, to)); 139 | } 140 | 141 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 142 | public static FAngle MoveTowards(FAngle from, FAngle to, FAngle maxDelta) 143 | { 144 | var delta = Delta(from, to); 145 | 146 | if (-maxDelta < delta && delta < maxDelta) 147 | return to; 148 | 149 | return FromRadians(FMath.MoveTowards(from.Radians, (from + delta).Radians, maxDelta.Radians)); 150 | } 151 | 152 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 153 | public static FAngle Max(FAngle a, FAngle b) 154 | { 155 | return FromRadians(FMath.Max(a.Radians, b.Radians)); 156 | } 157 | 158 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 159 | public static FAngle Min(FAngle a, FAngle b) 160 | { 161 | return FromRadians(FMath.Min(a.Radians, b.Radians)); 162 | } 163 | 164 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 165 | public static FAngle operator *(FAngle angle, FP value) 166 | { 167 | return new FAngle(angle.Radians * value); 168 | } 169 | 170 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 171 | public static FAngle operator *(FP value, FAngle angle) 172 | { 173 | return angle * value; 174 | } 175 | 176 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 177 | public static FAngle operator *(FAngle angle, int value) 178 | { 179 | return new FAngle(angle.Radians * value); 180 | } 181 | 182 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 183 | public static FAngle operator *(int value, FAngle angle) 184 | { 185 | return angle * value; 186 | } 187 | 188 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 189 | public static FAngle operator /(FAngle angle, FP value) 190 | { 191 | return new FAngle(angle.Radians / value); 192 | } 193 | 194 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 195 | public static FAngle operator /(FAngle angle, int value) 196 | { 197 | return new FAngle(angle.Radians / value); 198 | } 199 | 200 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 201 | public static FAngle operator +(FAngle a, FAngle b) 202 | { 203 | return new FAngle(a.Radians + b.Radians); 204 | } 205 | 206 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 207 | public static FAngle operator -(FAngle a) 208 | { 209 | return new FAngle(-a.Radians); 210 | } 211 | 212 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 213 | public static FAngle operator -(FAngle a, FAngle b) 214 | { 215 | return -b + a; 216 | } 217 | 218 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 219 | public static bool operator ==(FAngle a, FAngle b) 220 | { 221 | return FMath.ApproximatelyEqual(a.Radians, b.Radians); 222 | } 223 | 224 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 225 | public static bool operator !=(FAngle a, FAngle b) 226 | { 227 | return !(a == b); 228 | } 229 | 230 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 231 | public static bool operator >=(FAngle a, FAngle b) 232 | { 233 | return a.Radians >= b.Radians; 234 | } 235 | 236 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 237 | public static bool operator <=(FAngle a, FAngle b) 238 | { 239 | return a.Radians <= b.Radians; 240 | } 241 | 242 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 243 | public static bool operator >(FAngle a, FAngle b) 244 | { 245 | return a.Radians > b.Radians; 246 | } 247 | 248 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 249 | public static bool operator <(FAngle a, FAngle b) 250 | { 251 | return a.Radians < b.Radians; 252 | } 253 | 254 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 255 | public override bool Equals(object obj) 256 | { 257 | return obj is FAngle other && Equals(other); 258 | } 259 | 260 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 261 | public bool Equals(FAngle other) => Radians.Equals(other.Radians); 262 | 263 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 264 | public override int GetHashCode() => Radians.GetHashCode(); 265 | 266 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 267 | public override string ToString() => $"{Degrees}°"; 268 | } 269 | } 270 | -------------------------------------------------------------------------------- /Runtime/Structs/FAngle.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 2afcad945e9e49b1a619c5cc36474a85 3 | timeCreated: 1737212872 -------------------------------------------------------------------------------- /Runtime/Structs/FQuaternion.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Runtime.CompilerServices; 3 | 4 | namespace Mathematics.Fixed 5 | { 6 | [Serializable] 7 | public struct FQuaternion : IEquatable, IFormattable 8 | { 9 | public FP X; 10 | public FP Y; 11 | public FP Z; 12 | public FP W; 13 | 14 | /// 15 | /// Constructs a unit quaternion from four FP values. Use if you know what you are doing. 16 | /// 17 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 18 | public FQuaternion(FP x, FP y, FP z, FP w) 19 | { 20 | X = x; 21 | Y = y; 22 | Z = z; 23 | W = w; 24 | } 25 | 26 | /// 27 | /// The identity rotation. 28 | /// 29 | public static FQuaternion Identity 30 | { 31 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 32 | get => new FQuaternion(FP.Zero, FP.Zero, FP.Zero, FP.One); 33 | } 34 | 35 | /// 36 | /// Returns true if the given quaternion is exactly equal to this quaternion. 37 | /// 38 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 39 | public override bool Equals(object other) => other is FQuaternion softQuaternion && Equals(softQuaternion); 40 | 41 | /// 42 | /// Returns true if the given quaternion is exactly equal to this quaternion. 43 | /// 44 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 45 | public bool Equals(FQuaternion other) 46 | { 47 | return X == other.X && Y == other.Y && Z == other.Z && W == other.W; 48 | } 49 | 50 | public override string ToString() => 51 | ToString("F2", System.Globalization.CultureInfo.InvariantCulture.NumberFormat); 52 | 53 | public string ToString(string format) => 54 | ToString(format, System.Globalization.CultureInfo.InvariantCulture.NumberFormat); 55 | 56 | public string ToString(IFormatProvider provider) => ToString("F2", provider); 57 | 58 | public string ToString(string format, IFormatProvider formatProvider) 59 | { 60 | return string.Format("({0}, {1}, {2}, {3})", X.ToString(format, formatProvider), 61 | Y.ToString(format, formatProvider), Z.ToString(format, formatProvider), 62 | Y.ToString(format, formatProvider)); 63 | } 64 | 65 | public override int GetHashCode() => 66 | X.GetHashCode() ^ Y.GetHashCode() << 2 ^ Z.GetHashCode() >> 2 ^ W.GetHashCode() >> 1; 67 | 68 | /// 69 | /// The componentwise addition. 70 | /// 71 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 72 | public static FQuaternion operator +(FQuaternion a, FQuaternion b) 73 | { 74 | return new FQuaternion(a.X + b.X, a.Y + b.Y, a.Z + b.Z, a.W + b.W); 75 | } 76 | 77 | /// 78 | /// The componentwise negotiation. 79 | /// 80 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 81 | public static FQuaternion operator -(FQuaternion a) 82 | { 83 | return new FQuaternion(-a.X, -a.Y, -a.Z, -a.W); 84 | } 85 | 86 | /// 87 | /// The componentwise subtraction. 88 | /// 89 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 90 | public static FQuaternion operator -(FQuaternion a, FQuaternion b) 91 | { 92 | return -b + a; 93 | } 94 | 95 | /// 96 | /// The quaternions multiplication. 97 | /// 98 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 99 | public static FQuaternion operator *(FQuaternion a, FQuaternion b) 100 | { 101 | return new FQuaternion( 102 | a.W * b.X + a.X * b.W + a.Y * b.Z - a.Z * b.Y, 103 | a.W * b.Y + a.Y * b.W + a.Z * b.X - a.X * b.Z, 104 | a.W * b.Z + a.Z * b.W + a.X * b.Y - a.Y * b.X, 105 | a.W * b.W - a.X * b.X - a.Y * b.Y - a.Z * b.Z); 106 | } 107 | 108 | /// 109 | /// The componentwise multiplication. 110 | /// 111 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 112 | public static FQuaternion operator *(FQuaternion a, FP b) 113 | { 114 | return new FQuaternion(a.X * b, a.Y * b, a.Z * b, a.W * b); 115 | } 116 | 117 | /// 118 | /// The componentwise multiplication. 119 | /// 120 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 121 | public static FQuaternion operator *(FP b, FQuaternion a) 122 | { 123 | return new FQuaternion(a.X * b, a.Y * b, a.Z * b, a.W * b); 124 | } 125 | 126 | /// 127 | /// Rotate vector by the quaternion. Valid only for normalized quaternions. 128 | /// 129 | public static FVector3 operator *(FQuaternion unitQuaternion, FVector3 vector) 130 | { 131 | var twoX = unitQuaternion.X * 2; 132 | var twoY = unitQuaternion.Y * 2; 133 | var twoZ = unitQuaternion.Z * 2; 134 | var xx2 = unitQuaternion.X * twoX; 135 | var yy2 = unitQuaternion.Y * twoY; 136 | var zz2 = unitQuaternion.Z * twoZ; 137 | var xy2 = unitQuaternion.X * twoY; 138 | var xz2 = unitQuaternion.X * twoZ; 139 | var yz2 = unitQuaternion.Y * twoZ; 140 | var wx2 = unitQuaternion.W * twoX; 141 | var wy2 = unitQuaternion.W * twoY; 142 | var wz2 = unitQuaternion.W * twoZ; 143 | var result = new FVector3( 144 | (FP.One - (yy2 + zz2)) * vector.X + (xy2 - wz2) * vector.Y + (xz2 + wy2) * vector.Z, 145 | (xy2 + wz2) * vector.X + (FP.One - (xx2 + zz2)) * vector.Y + (yz2 - wx2) * vector.Z, 146 | (xz2 - wy2) * vector.X + (yz2 + wx2) * vector.Y + (FP.One - (xx2 + yy2)) * vector.Z); 147 | return result; 148 | } 149 | 150 | /// 151 | /// Returns the vector transformed by the quaternion, including scale and rotation. 152 | /// Also known as sandwich product: q * vec * conj(q) 153 | /// 154 | public static FVector3 Sandwich(FQuaternion quaternion, FVector3 vector) 155 | { 156 | var twoX = quaternion.X * 2; 157 | var twoY = quaternion.Y * 2; 158 | var twoZ = quaternion.Z * 2; 159 | var xx = quaternion.X * quaternion.X; 160 | var yy = quaternion.Y * quaternion.Y; 161 | var zz = quaternion.Z * quaternion.Z; 162 | var ww = quaternion.W * quaternion.W; 163 | var xy2 = quaternion.X * twoY; 164 | var xz2 = quaternion.X * twoZ; 165 | var yz2 = quaternion.Y * twoZ; 166 | var wx2 = quaternion.W * twoX; 167 | var wy2 = quaternion.W * twoY; 168 | var wz2 = quaternion.W * twoZ; 169 | var result = new FVector3( 170 | (ww + xx - yy - zz) * vector.X + (xy2 - wz2) * vector.Y + (xz2 + wy2) * vector.Z, 171 | (xy2 + wz2) * vector.X + (ww - xx + yy - zz) * vector.Y + (yz2 - wx2) * vector.Z, 172 | (xz2 - wy2) * vector.X + (yz2 + wx2) * vector.Y + (ww - xx - yy + zz) * vector.Z); 173 | return result; 174 | } 175 | 176 | /// 177 | /// The quaternions division. 178 | /// 179 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 180 | public static FQuaternion operator /(FQuaternion a, FQuaternion b) 181 | { 182 | return a * Inverse(b); 183 | } 184 | 185 | /// 186 | /// The componentwise division. 187 | /// 188 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 189 | public static FQuaternion operator /(FQuaternion a, FP b) 190 | { 191 | var invB = FP.One / b; 192 | return new FQuaternion(a.X * invB, a.Y * invB, a.Z * invB, a.W * invB); 193 | } 194 | 195 | /// 196 | /// Returns true if quaternions are approximately equal, false otherwise. 197 | /// 198 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 199 | public static bool operator ==(FQuaternion a, FQuaternion b) => ApproximatelyEqual(a, b); 200 | 201 | /// 202 | /// Returns true if quaternions are not approximately equal, false otherwise. 203 | /// 204 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 205 | public static bool operator !=(FQuaternion a, FQuaternion b) => !(a == b); 206 | 207 | /// 208 | /// The dot product between two rotations. 209 | /// 210 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 211 | public static FP Dot(FQuaternion a, FQuaternion b) 212 | { 213 | return a.X * b.X + a.Y * b.Y + a.Z * b.Z + a.W * b.W; 214 | } 215 | 216 | /// 217 | /// The length of a quaternion. 218 | /// 219 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 220 | public static FP Length(FQuaternion a) 221 | { 222 | return FMath.Sqrt(LengthSqr(a)); 223 | } 224 | 225 | /// 226 | /// The squared length of a quaternion. 227 | /// 228 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 229 | public static FP LengthSqr(FQuaternion a) 230 | { 231 | return Dot(a, a); 232 | } 233 | 234 | /// 235 | /// The inverse of a quaternion. 236 | /// 237 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 238 | public static FQuaternion Inverse(FQuaternion a) 239 | { 240 | return new FQuaternion(-a.X, -a.Y, -a.Z, a.W); 241 | } 242 | 243 | /// 244 | /// Returns a spherical interpolation between two quaternions. 245 | /// Non-commutative, torque-minimal, constant velocity. 246 | /// 247 | public static FQuaternion Slerp(FQuaternion a, FQuaternion b, FP t, bool longPath = false) 248 | { 249 | // Calculate angle between them. 250 | var cosHalfTheta = Dot(a, b); 251 | 252 | if (longPath) 253 | { 254 | if (cosHalfTheta > FP.Zero) 255 | { 256 | b = -b; 257 | cosHalfTheta = -cosHalfTheta; 258 | } 259 | } 260 | else 261 | { 262 | if (cosHalfTheta < FP.Zero) 263 | { 264 | b = -b; 265 | cosHalfTheta = -cosHalfTheta; 266 | } 267 | } 268 | 269 | // If a = b or a = b then theta = 0 then we can return interpolation between a and b. 270 | if (FMath.Abs(cosHalfTheta) > FP.One - FP.CalculationsEpsilon) 271 | { 272 | return Nlerp(a, b, t, longPath); 273 | } 274 | 275 | var halfTheta = FMath.Acos(cosHalfTheta); 276 | var sinHalfTheta = FMath.Sin(halfTheta); 277 | 278 | var influenceA = FMath.Sin((FP.One - t) * halfTheta) / sinHalfTheta; 279 | var influenceB = FMath.Sin(t * halfTheta) / sinHalfTheta; 280 | 281 | return EnsureNormalization(a * influenceA + b * influenceB); 282 | } 283 | 284 | /// 285 | /// Returns a normalized componentwise interpolation between two quaternions. 286 | /// Commutative, torque-minimal, non-constant velocity. 287 | /// 288 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 289 | public static FQuaternion Nlerp(FQuaternion a, FQuaternion b, FP t, bool longPath = false) 290 | { 291 | return NormalizeSafe(Lerp(a, b, t, longPath)); 292 | } 293 | 294 | /// 295 | /// Returns a componentwise interpolation between two quaternions. 296 | /// 297 | public static FQuaternion Lerp(FQuaternion a, FQuaternion b, FP t, bool longPath = false) 298 | { 299 | var dot = Dot(a, b); 300 | 301 | if (longPath) 302 | { 303 | if (dot > FP.Zero) 304 | { 305 | b = -b; 306 | } 307 | } 308 | else 309 | { 310 | if (dot < FP.Zero) 311 | { 312 | b = -b; 313 | } 314 | } 315 | 316 | return a * (FP.One - t) + b * t; 317 | } 318 | 319 | /// 320 | /// Returns a rotation with the specified forward and up directions. 321 | /// If inputs are zero length or collinear or have some other weirdness, 322 | /// then rotation result will be some mix of and vectors. 323 | /// 324 | public static FQuaternion LookRotation(FVector3 forward, FVector3 up) 325 | { 326 | // Third matrix column 327 | var lookAt = FVector3.NormalizeSafe(forward, FVector3.Forward); 328 | // First matrix column 329 | var sideAxis = FVector3.NormalizeSafe(FVector3.Cross(up, lookAt), FVector3.Orthonormal(lookAt)); 330 | // Second matrix column 331 | var rotatedUp = FVector3.Cross(lookAt, sideAxis); 332 | 333 | // Sums of matrix main diagonal elements 334 | var trace1 = FP.One + sideAxis.X - rotatedUp.Y - lookAt.Z; 335 | var trace2 = FP.One - sideAxis.X + rotatedUp.Y - lookAt.Z; 336 | var trace3 = FP.One - sideAxis.X - rotatedUp.Y + lookAt.Z; 337 | 338 | // If orthonormal vectors forms identity matrix, then return identity rotation 339 | if (trace1 + trace2 + trace3 < FP.CalculationsEpsilon) 340 | { 341 | return Identity; 342 | } 343 | 344 | // Choose largest diagonal 345 | if (trace1 + FP.CalculationsEpsilon > trace2 && trace1 + FP.CalculationsEpsilon > trace3) 346 | { 347 | var s = FMath.Sqrt(trace1) * 2; 348 | var invS = FP.One / s; 349 | return new FQuaternion( 350 | FP.Quarter * s, 351 | (rotatedUp.X + sideAxis.Y) * invS, 352 | (lookAt.X + sideAxis.Z) * invS, 353 | (rotatedUp.Z - lookAt.Y) * invS); 354 | } 355 | else if (trace2 + FP.CalculationsEpsilon > trace1 && trace2 + FP.CalculationsEpsilon > trace3) 356 | { 357 | var s = FMath.Sqrt(trace2) * 2; 358 | var invS = FP.One / s; 359 | return new FQuaternion( 360 | (rotatedUp.X + sideAxis.Y) * invS, 361 | FP.Quarter * s, 362 | (lookAt.Y + rotatedUp.Z) * invS, 363 | (lookAt.X - sideAxis.Z) * invS); 364 | } 365 | else 366 | { 367 | var s = FMath.Sqrt(trace3) * 2; 368 | var invS = FP.One / s; 369 | return new FQuaternion( 370 | (lookAt.X + sideAxis.Z) * invS, 371 | (lookAt.Y + rotatedUp.Z) * invS, 372 | FP.Quarter * s, 373 | (sideAxis.Y - rotatedUp.X) * invS); 374 | } 375 | } 376 | 377 | /// 378 | /// Returns a quaternion representing a rotation around a unit axis by an angle in radians. 379 | /// The rotation direction is clockwise when looking along the rotation axis towards the origin. 380 | /// If input vector is zero length then rotation will be around forward axis. 381 | /// 382 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 383 | public static FQuaternion AxisAngleRadians(FVector3 axis, FP angle) 384 | { 385 | axis = FVector3.NormalizeSafe(axis, FVector3.Forward); 386 | var sin = FMath.Sin(FP.Half * angle); 387 | var cos = FMath.Cos(FP.Half * angle); 388 | return new FQuaternion(axis.X * sin, axis.Y * sin, axis.Z * sin, cos); 389 | } 390 | 391 | /// 392 | /// Returns a quaternion representing a euler angle in radians. 393 | /// 394 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 395 | public static FQuaternion EulerRadians(FVector3 angle) 396 | { 397 | var cr = FMath.Cos(angle.X * FP.Half); 398 | var sr = FMath.Sin(angle.X * FP.Half); 399 | var cp = FMath.Cos(angle.Y * FP.Half); 400 | var sp = FMath.Sin(angle.Y * FP.Half); 401 | var cy = FMath.Cos(angle.Z * FP.Half); 402 | var sy = FMath.Sin(angle.Z * FP.Half); 403 | 404 | return new FQuaternion( 405 | sr * cp * cy - cr * sp * sy, 406 | cr * sp * cy + sr * cp * sy, 407 | cr * cp * sy - sr * sp * cy, 408 | cr * cp * cy + sr * sp * sy); 409 | } 410 | 411 | /// 412 | /// Returns a quaternion representing a rotation around a unit axis by an angle in degrees. 413 | /// The rotation direction is clockwise when looking along the rotation axis towards the origin. 414 | /// /// If input vector is zero length then rotation will be around forward axis. 415 | /// 416 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 417 | public static FQuaternion AxisAngleDegrees(FVector3 axis, FP angle) 418 | { 419 | axis = FVector3.NormalizeSafe(axis, FVector3.Forward); 420 | var sin = FMath.Sin(FP.Half * angle * FP.Deg2Rad); 421 | var cos = FMath.Cos(FP.Half * angle * FP.Deg2Rad); 422 | return new FQuaternion(axis.X * sin, axis.Y * sin, axis.Z * sin, cos); 423 | } 424 | 425 | /// 426 | /// Compares two quaternions with and returns true if they are similar. 427 | /// 428 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 429 | public static bool ApproximatelyEqual(FQuaternion a, FQuaternion b) 430 | { 431 | return ApproximatelyEqual(a, b, FP.CalculationsEpsilonSqr); 432 | } 433 | 434 | /// 435 | /// Compares two quaternions with some epsilon and returns true if they are similar. 436 | /// 437 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 438 | public static bool ApproximatelyEqual(FQuaternion a, FQuaternion b, FP epsilon) 439 | { 440 | return FMath.Abs(Dot(a, b)) > FP.One - epsilon; 441 | } 442 | 443 | /// 444 | /// Returns a normalized version of a quaternion. 445 | /// 446 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 447 | public static FQuaternion Normalize(FQuaternion a) 448 | { 449 | var length = Length(a); 450 | return a / length; 451 | } 452 | 453 | /// 454 | /// Returns a safe normalized version of a quaternion. 455 | /// Returns the when quaternion length close to zero. 456 | /// 457 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 458 | public static FQuaternion NormalizeSafe(FQuaternion a) 459 | { 460 | return NormalizeSafe(a, Identity); 461 | } 462 | 463 | /// 464 | /// Returns a safe normalized version of a quaternion. 465 | /// Returns the given default value when quaternion length close to zero. 466 | /// 467 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 468 | public static FQuaternion NormalizeSafe(FQuaternion a, FQuaternion defaultValue) 469 | { 470 | var sqrLength = LengthSqr(a); 471 | if (sqrLength < FP.CalculationsEpsilonSqr) 472 | { 473 | return defaultValue; 474 | } 475 | return a / FMath.Sqrt(sqrLength); 476 | } 477 | 478 | /// 479 | /// Check quaternion for normalization precision error and re-normalize it if needed. 480 | /// 481 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 482 | public static FQuaternion EnsureNormalization(FQuaternion a) 483 | { 484 | var lengthSqr = LengthSqr(a); 485 | if (FMath.Abs(FP.One - lengthSqr) > FP.CalculationsEpsilonSqr) 486 | { 487 | return a / FMath.Sqrt(lengthSqr); 488 | } 489 | return a; 490 | } 491 | } 492 | } 493 | -------------------------------------------------------------------------------- /Runtime/Structs/FQuaternion.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: fc9e602cd535412787c441b3eb2b4781 3 | timeCreated: 1737226167 -------------------------------------------------------------------------------- /Runtime/Structs/FRotation2D.cs: -------------------------------------------------------------------------------- 1 | using System.Runtime.CompilerServices; 2 | 3 | namespace Mathematics.Fixed 4 | { 5 | public struct FRotation2D 6 | { 7 | public FP Sin; 8 | public FP OneMinusCos; 9 | 10 | public FRotation2D(FAngle angle) 11 | { 12 | Sin = FMath.Sin(angle.Radians); 13 | OneMinusCos = FP.One - FMath.Cos(angle.Radians); 14 | } 15 | 16 | private FRotation2D(FP sin, FP oneMinusCos) 17 | { 18 | Sin = sin; 19 | OneMinusCos = oneMinusCos; 20 | } 21 | 22 | public FP Cos 23 | { 24 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 25 | get => FP.One - OneMinusCos; 26 | } 27 | 28 | public FAngle CounterclockwiseAngle 29 | { 30 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 31 | get => FAngle.FromRadians(FMath.Atan2(Sin, Cos)); 32 | } 33 | 34 | public FAngle ClockwiseAngle 35 | { 36 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 37 | get => -CounterclockwiseAngle; 38 | } 39 | 40 | public static FRotation2D Identity 41 | { 42 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 43 | get => new FRotation2D(FP.Zero, FP.Zero); 44 | } 45 | 46 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 47 | public static FVector2 operator *(FRotation2D rotation2D, FVector2 vector) 48 | { 49 | var sin = rotation2D.Sin; 50 | var cos = rotation2D.Cos; 51 | return new FVector2( 52 | vector.X * cos - vector.Y * sin, 53 | vector.X * sin + vector.Y * cos 54 | ); 55 | } 56 | 57 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 58 | public static FRotation2D operator *(FRotation2D a, FRotation2D b) 59 | { 60 | var sinA = a.Sin; 61 | var cosA = a.Cos; 62 | var sinB = b.Sin; 63 | var cosB = b.Cos; 64 | 65 | var cos = cosA * cosB - sinA * sinB; 66 | var sin = sinA * cosB + cosA * sinB; 67 | 68 | return new FRotation2D(sin, FP.One - cos); 69 | } 70 | 71 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 72 | public static FRotation2D Inverse(FRotation2D rotation2D) 73 | { 74 | return new FRotation2D(-rotation2D.Sin, rotation2D.OneMinusCos); 75 | } 76 | 77 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 78 | public static FRotation2D FromToRotation(FVector2 fromDirection, FVector2 toDirection) 79 | { 80 | var angleRadians = FMath.Atan2(toDirection.Y, toDirection.X) - FMath.Atan2(fromDirection.Y, fromDirection.X); 81 | return new FRotation2D(FAngle.FromRadians(angleRadians)); 82 | } 83 | } 84 | } 85 | -------------------------------------------------------------------------------- /Runtime/Structs/FRotation2D.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 2fb410c414d24f9b80a281d5ecf3298f 3 | timeCreated: 1737212936 -------------------------------------------------------------------------------- /Runtime/Structs/FVector2.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Runtime.CompilerServices; 3 | 4 | namespace Mathematics.Fixed 5 | { 6 | [Serializable] 7 | public struct FVector2 : IEquatable, IFormattable 8 | { 9 | public FP X; 10 | public FP Y; 11 | 12 | /// 13 | /// Constructs a vector from two FP values. 14 | /// 15 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 16 | public FVector2(FP x, FP y) 17 | { 18 | X = x; 19 | Y = y; 20 | } 21 | 22 | /// 23 | /// Shorthand for writing FVector2(0, 0). 24 | /// 25 | public static FVector2 Zero 26 | { 27 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 28 | get => new FVector2(FP.Zero, FP.Zero); 29 | } 30 | 31 | /// 32 | /// Shorthand for writing FVector2(1, 1). 33 | /// 34 | public static FVector2 One 35 | { 36 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 37 | get => new FVector2(FP.One, FP.One); 38 | } 39 | 40 | /// 41 | /// Shorthand for writing FVector2(1, 0). 42 | /// 43 | public static FVector2 Right 44 | { 45 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 46 | get => new FVector2(FP.One, FP.Zero); 47 | } 48 | 49 | /// 50 | /// Shorthand for writing FVector2(-1, 0). 51 | /// 52 | public static FVector2 Left 53 | { 54 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 55 | get => new FVector2(FP.MinusOne, FP.Zero); 56 | } 57 | 58 | /// 59 | /// Shorthand for writing FVector2(0, 1). 60 | /// 61 | public static FVector2 Up 62 | { 63 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 64 | get => new FVector2(FP.Zero, FP.One); 65 | } 66 | 67 | /// 68 | /// Shorthand for writing FVector2(0, -1). 69 | /// 70 | public static FVector2 Down 71 | { 72 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 73 | get => new FVector2(FP.Zero, FP.MinusOne); 74 | } 75 | 76 | /// 77 | /// Returns true if the given vector is exactly equal to this vector. 78 | /// 79 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 80 | public override bool Equals(object other) => other is FVector2 otherVector && Equals(otherVector); 81 | 82 | /// 83 | /// Returns true if the given vector is exactly equal to this vector. 84 | /// 85 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 86 | public bool Equals(FVector2 other) 87 | { 88 | return X == other.X && Y == other.Y; 89 | } 90 | 91 | public override string ToString() => 92 | ToString("F2", System.Globalization.CultureInfo.InvariantCulture.NumberFormat); 93 | 94 | public string ToString(string format) => 95 | ToString(format, System.Globalization.CultureInfo.InvariantCulture.NumberFormat); 96 | 97 | public string ToString(IFormatProvider provider) => ToString("F2", provider); 98 | 99 | public string ToString(string format, IFormatProvider formatProvider) 100 | { 101 | return string.Format("({0}, {1})", X.ToString(format, formatProvider), 102 | Y.ToString(format, formatProvider)); 103 | } 104 | 105 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 106 | public override int GetHashCode() => X.GetHashCode() ^ Y.GetHashCode() << 2; 107 | 108 | /// 109 | /// Returns the componentwise addition. 110 | /// 111 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 112 | public static FVector2 operator +(FVector2 a, FVector2 b) 113 | { 114 | return new FVector2(a.X + b.X, a.Y + b.Y); 115 | } 116 | 117 | /// 118 | /// Returns the componentwise addition. 119 | /// 120 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 121 | public static FVector2 operator +(FVector2 a, FP b) 122 | { 123 | return new FVector2(a.X + b, a.Y + b); 124 | } 125 | 126 | /// 127 | /// Returns the componentwise addition. 128 | /// 129 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 130 | public static FVector2 operator +(FP a, FVector2 b) 131 | { 132 | return b + a; 133 | } 134 | 135 | /// 136 | /// Returns the componentwise negotiation. 137 | /// 138 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 139 | public static FVector2 operator -(FVector2 a) 140 | { 141 | return new FVector2(-a.X, -a.Y); 142 | } 143 | 144 | /// 145 | /// Returns the componentwise subtraction. 146 | /// 147 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 148 | public static FVector2 operator -(FVector2 a, FVector2 b) 149 | { 150 | return -b + a; 151 | } 152 | 153 | /// 154 | /// Returns the componentwise subtraction. 155 | /// 156 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 157 | public static FVector2 operator -(FVector2 a, FP b) 158 | { 159 | return -b + a; 160 | } 161 | 162 | /// 163 | /// Returns the componentwise subtraction. 164 | /// 165 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 166 | public static FVector2 operator -(FP a, FVector2 b) 167 | { 168 | return -b + a; 169 | } 170 | 171 | /// 172 | /// Returns the componentwise multiplication. 173 | /// 174 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 175 | public static FVector2 operator *(FVector2 a, FVector2 b) 176 | { 177 | return new FVector2(a.X * b.X, a.Y * b.Y); 178 | } 179 | 180 | /// 181 | /// Returns the componentwise multiplication. 182 | /// 183 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 184 | public static FVector2 operator *(FVector2 a, FP b) 185 | { 186 | return new FVector2(a.X * b, a.Y * b); 187 | } 188 | 189 | /// 190 | /// Returns the componentwise multiplication. 191 | /// 192 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 193 | public static FVector2 operator *(FP a, FVector2 b) 194 | { 195 | return b * a; 196 | } 197 | 198 | /// 199 | /// Returns the componentwise multiplication. 200 | /// 201 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 202 | public static FVector2 operator *(FVector2 a, int b) 203 | { 204 | return new FVector2(a.X * b, a.Y * b); 205 | } 206 | 207 | /// 208 | /// Returns the componentwise multiplication. 209 | /// 210 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 211 | public static FVector2 operator *(int b, FVector2 a) 212 | { 213 | return a * b; 214 | } 215 | 216 | /// 217 | /// Returns the componentwise division. 218 | /// 219 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 220 | public static FVector2 operator /(FVector2 a, FVector2 b) 221 | { 222 | return new FVector2(a.X / b.X, a.Y / b.Y); 223 | } 224 | 225 | /// 226 | /// Returns the componentwise division. 227 | /// 228 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 229 | public static FVector2 operator /(FVector2 a, FP b) 230 | { 231 | var invB = FP.One / b; 232 | return new FVector2(a.X * invB, a.Y * invB); 233 | } 234 | 235 | /// 236 | /// Returns the componentwise division. 237 | /// 238 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 239 | public static FVector2 operator /(FVector2 a, int b) 240 | { 241 | return new FVector2(a.X / b, a.Y / b); 242 | } 243 | 244 | /// 245 | /// Returns true if vectors are approximately equal, false otherwise. 246 | /// 247 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 248 | public static bool operator ==(FVector2 a, FVector2 b) => ApproximatelyEqual(a, b); 249 | 250 | /// 251 | /// Returns true if vectors are not approximately equal, false otherwise. 252 | /// 253 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 254 | public static bool operator !=(FVector2 a, FVector2 b) => !(a == b); 255 | 256 | /// 257 | /// Returns the dot product of two vectors. 258 | /// 259 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 260 | public static FP Dot(FVector2 a, FVector2 b) 261 | { 262 | return a.X * b.X + a.Y * b.Y; 263 | } 264 | 265 | /// 266 | /// Returns the length of 3D vector from cross product of two vectors. 267 | /// 268 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 269 | public static FP Cross(FVector2 a, FVector2 b) 270 | { 271 | return a.X * b.Y - a.Y * b.X; 272 | } 273 | 274 | /// 275 | /// Returns the length of a vector. 276 | /// 277 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 278 | public static FP Length(FVector2 a) 279 | { 280 | return FMath.Sqrt(LengthSqr(a)); 281 | } 282 | 283 | /// 284 | /// Returns the squared length of a vector. 285 | /// 286 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 287 | public static FP LengthSqr(FVector2 a) 288 | { 289 | return Dot(a, a); 290 | } 291 | 292 | /// 293 | /// Returns the distance between a and b. 294 | /// 295 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 296 | public static FP Distance(FVector2 a, FVector2 b) 297 | { 298 | return FMath.Sqrt(DistanceSqr(a, b)); 299 | } 300 | 301 | /// 302 | /// Returns the squared distance between a and b. 303 | /// 304 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 305 | public static FP DistanceSqr(FVector2 a, FVector2 b) 306 | { 307 | var deltaX = a.X - b.X; 308 | var deltaY = a.Y - b.Y; 309 | return deltaX * deltaX + deltaY * deltaY; 310 | } 311 | 312 | /// 313 | /// Returns a normalized version of a vector. 314 | /// 315 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 316 | public static FVector2 Normalize(FVector2 a) 317 | { 318 | var length = Length(a); 319 | return a / length; 320 | } 321 | 322 | /// 323 | /// Returns a safe normalized version of a vector. 324 | /// Returns the given default value when vector length close to zero. 325 | /// 326 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 327 | public static FVector2 NormalizeSafe(FVector2 a, FVector2 defaultValue = new FVector2()) 328 | { 329 | var lengthSqr = LengthSqr(a); 330 | if (lengthSqr < FP.CalculationsEpsilonSqr) 331 | return defaultValue; 332 | return a / FMath.Sqrt(lengthSqr); 333 | } 334 | 335 | /// 336 | /// Returns non-normalized perpendicular vector to a given one. For normalized see . 337 | /// 338 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 339 | public static FVector2 Orthogonal(FVector2 a) 340 | { 341 | return new FVector2(-a.Y, a.X); 342 | } 343 | 344 | /// 345 | /// Returns orthogonal basis vector to a given one. 346 | /// 347 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 348 | public static FVector2 Orthonormal(FVector2 a) 349 | { 350 | var orthogonal = Orthogonal(a); 351 | var length = Length(orthogonal); 352 | return orthogonal / length; 353 | } 354 | 355 | /// 356 | /// Returns a vector that is made from the largest components of two vectors. 357 | /// 358 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 359 | public static FVector2 MaxComponents(FVector2 a, FVector2 b) 360 | { 361 | return new FVector2(FMath.Max(a.X, b.X), FMath.Max(a.Y, b.Y)); 362 | } 363 | 364 | /// 365 | /// Returns a vector that is made from the smallest components of two vectors. 366 | /// 367 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 368 | public static FVector2 MinComponents(FVector2 a, FVector2 b) 369 | { 370 | return new FVector2(FMath.Min(a.X, b.X), FMath.Min(a.Y, b.Y)); 371 | } 372 | 373 | /// 374 | /// Returns the componentwise absolute value of a vector. 375 | /// 376 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 377 | public static FVector2 AbsComponents(FVector2 a) 378 | { 379 | return new FVector2(FMath.Abs(a.X), FMath.Abs(a.Y)); 380 | } 381 | 382 | /// 383 | /// Returns the componentwise signes of a vector. 384 | /// 385 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 386 | public static FVector2 SignComponents(FVector2 a) 387 | { 388 | return new FVector2(FMath.Sign(a.X), FMath.Sign(a.Y)); 389 | } 390 | 391 | /// 392 | /// Compares two vectors with and returns true if they are similar. 393 | /// 394 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 395 | public static bool ApproximatelyEqual(FVector2 a, FVector2 b) 396 | { 397 | return ApproximatelyEqual(a, b, FP.CalculationsEpsilonSqr); 398 | } 399 | 400 | /// 401 | /// Compares two vectors with some epsilon and returns true if they are similar. 402 | /// 403 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 404 | public static bool ApproximatelyEqual(FVector2 a, FVector2 b, FP epsilon) 405 | { 406 | return DistanceSqr(a, b) < epsilon; 407 | } 408 | } 409 | } 410 | -------------------------------------------------------------------------------- /Runtime/Structs/FVector2.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: beacc0ad94ba42a098bdfcac30ef6eac 3 | timeCreated: 1737213103 -------------------------------------------------------------------------------- /Runtime/Structs/FVector3.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Runtime.CompilerServices; 3 | 4 | namespace Mathematics.Fixed 5 | { 6 | [Serializable] 7 | public struct FVector3 : IEquatable, IFormattable 8 | { 9 | public FP X; 10 | public FP Y; 11 | public FP Z; 12 | 13 | /// 14 | /// Constructs a vector from three FP values. 15 | /// 16 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 17 | public FVector3(FP x, FP y, FP z) 18 | { 19 | X = x; 20 | Y = y; 21 | Z = z; 22 | } 23 | 24 | /// 25 | /// Shorthand for writing FVector3(0, 0, 0). 26 | /// 27 | public static FVector3 Zero 28 | { 29 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 30 | get => new FVector3(FP.Zero, FP.Zero, FP.Zero); 31 | } 32 | 33 | /// 34 | /// Shorthand for writing FVector3(1, 1, 1). 35 | /// 36 | public static FVector3 One 37 | { 38 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 39 | get => new FVector3(FP.One, FP.One, FP.One); 40 | } 41 | 42 | /// 43 | /// Shorthand for writing FVector3(1, 0, 0). 44 | /// 45 | public static FVector3 Right 46 | { 47 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 48 | get => new FVector3(FP.One, FP.Zero, FP.Zero); 49 | } 50 | 51 | /// 52 | /// Shorthand for writing FVector3(-1, 0, 0). 53 | /// 54 | public static FVector3 Left 55 | { 56 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 57 | get => new FVector3(FP.MinusOne, FP.Zero, FP.Zero); 58 | } 59 | 60 | /// 61 | /// Shorthand for writing FVector3(0, 1, 0). 62 | /// 63 | public static FVector3 Up 64 | { 65 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 66 | get => new FVector3(FP.Zero, FP.One, FP.Zero); 67 | } 68 | 69 | /// 70 | /// Shorthand for writing FVector3(0, -1, 0). 71 | /// 72 | public static FVector3 Down 73 | { 74 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 75 | get => new FVector3(FP.Zero, FP.MinusOne, FP.Zero); 76 | } 77 | 78 | /// 79 | /// Shorthand for writing FVector3(0, 0, 1). 80 | /// 81 | public static FVector3 Forward 82 | { 83 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 84 | get => new FVector3(FP.Zero, FP.Zero, FP.One); 85 | } 86 | 87 | /// 88 | /// Shorthand for writing FVector3(0, 0, -1). 89 | /// 90 | public static FVector3 Backward 91 | { 92 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 93 | get => new FVector3(FP.Zero, FP.Zero, FP.MinusOne); 94 | } 95 | 96 | /// 97 | /// Returns true if the given vector is exactly equal to this vector. 98 | /// 99 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 100 | public override bool Equals(object other) => other is FVector3 otherVector && Equals(otherVector); 101 | 102 | /// 103 | /// Returns true if the given vector is exactly equal to this vector. 104 | /// 105 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 106 | public bool Equals(FVector3 other) 107 | { 108 | return X == other.X && Y == other.Y && Z == other.Z; 109 | } 110 | 111 | public override string ToString() => 112 | ToString("F2", System.Globalization.CultureInfo.InvariantCulture.NumberFormat); 113 | 114 | public string ToString(string format) => 115 | ToString(format, System.Globalization.CultureInfo.InvariantCulture.NumberFormat); 116 | 117 | public string ToString(IFormatProvider provider) => ToString("F2", provider); 118 | 119 | public string ToString(string format, IFormatProvider formatProvider) 120 | { 121 | return string.Format("({0}, {1}, {2})", X.ToString(format, formatProvider), 122 | Y.ToString(format, formatProvider), Z.ToString(format, formatProvider)); 123 | } 124 | 125 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 126 | public override int GetHashCode() => X.GetHashCode() ^ Y.GetHashCode() << 2 ^ Z.GetHashCode() >> 2; 127 | 128 | /// 129 | /// Returns the componentwise addition. 130 | /// 131 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 132 | public static FVector3 operator +(FVector3 a, FVector3 b) 133 | { 134 | return new FVector3(a.X + b.X, a.Y + b.Y, a.Z + b.Z); 135 | } 136 | 137 | /// 138 | /// Returns the componentwise addition. 139 | /// 140 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 141 | public static FVector3 operator +(FVector3 a, FP b) 142 | { 143 | return new FVector3(a.X + b, a.Y + b, a.Z + b); 144 | } 145 | 146 | /// 147 | /// Returns the componentwise addition. 148 | /// 149 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 150 | public static FVector3 operator +(FP a, FVector3 b) 151 | { 152 | return b + a; 153 | } 154 | 155 | /// 156 | /// Returns the componentwise negotiation. 157 | /// 158 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 159 | public static FVector3 operator -(FVector3 a) 160 | { 161 | return new FVector3(-a.X, -a.Y, -a.Z); 162 | } 163 | 164 | /// 165 | /// Returns the componentwise subtraction. 166 | /// 167 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 168 | public static FVector3 operator -(FVector3 a, FVector3 b) 169 | { 170 | return -b + a; 171 | } 172 | 173 | /// 174 | /// Returns the componentwise subtraction. 175 | /// 176 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 177 | public static FVector3 operator -(FVector3 a, FP b) 178 | { 179 | return -b + a; 180 | } 181 | 182 | /// 183 | /// Returns the componentwise subtraction. 184 | /// 185 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 186 | public static FVector3 operator -(FP a, FVector3 b) 187 | { 188 | return -b + a; 189 | } 190 | 191 | /// 192 | /// Returns the componentwise multiplication. 193 | /// 194 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 195 | public static FVector3 operator *(FVector3 a, FVector3 b) 196 | { 197 | return new FVector3(a.X * b.X, a.Y * b.Y, a.Z * b.Z); 198 | } 199 | 200 | /// 201 | /// Returns the componentwise multiplication. 202 | /// 203 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 204 | public static FVector3 operator *(FVector3 a, FP b) 205 | { 206 | return new FVector3(a.X * b, a.Y * b, a.Z * b); 207 | } 208 | 209 | /// 210 | /// Returns the componentwise multiplication. 211 | /// 212 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 213 | public static FVector3 operator *(FP a, FVector3 b) 214 | { 215 | return b * a; 216 | } 217 | 218 | /// 219 | /// Returns the componentwise multiplication. 220 | /// 221 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 222 | public static FVector3 operator *(FVector3 a, int b) 223 | { 224 | return new FVector3(a.X * b, a.Y * b, a.Z * b); 225 | } 226 | 227 | /// 228 | /// Returns the componentwise multiplication. 229 | /// 230 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 231 | public static FVector3 operator *(int b, FVector3 a) 232 | { 233 | return a * b; 234 | } 235 | 236 | /// 237 | /// Returns the componentwise division. 238 | /// 239 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 240 | public static FVector3 operator /(FVector3 a, FVector3 b) 241 | { 242 | return new FVector3(a.X / b.X, a.Y / b.Y, a.Z / b.Z); 243 | } 244 | 245 | /// 246 | /// Returns the componentwise division. 247 | /// 248 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 249 | public static FVector3 operator /(FVector3 a, FP b) 250 | { 251 | var invB = FP.One / b; 252 | return new FVector3(a.X * invB, a.Y * invB, a.Z * invB); 253 | } 254 | 255 | /// 256 | /// Returns the componentwise division. 257 | /// 258 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 259 | public static FVector3 operator /(FVector3 a, int b) 260 | { 261 | return new FVector3(a.X / b, a.Y / b, a.Z / b); 262 | } 263 | 264 | /// 265 | /// Returns true if vectors are approximately equal, false otherwise. 266 | /// 267 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 268 | public static bool operator ==(FVector3 a, FVector3 b) => ApproximatelyEqual(a, b); 269 | 270 | /// 271 | /// Returns true if vectors are not approximately equal, false otherwise. 272 | /// 273 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 274 | public static bool operator !=(FVector3 a, FVector3 b) => !(a == b); 275 | 276 | /// 277 | /// Returns the dot product of two vectors. 278 | /// 279 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 280 | public static FP Dot(FVector3 a, FVector3 b) 281 | { 282 | return a.X * b.X + a.Y * b.Y + a.Z * b.Z; 283 | } 284 | 285 | /// 286 | /// Returns the cross product of two vectors. 287 | /// 288 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 289 | public static FVector3 Cross(FVector3 a, FVector3 b) 290 | { 291 | return new FVector3(a.Y * b.Z - a.Z * b.Y, a.Z * b.X - a.X * b.Z, a.X * b.Y - a.Y * b.X); 292 | } 293 | 294 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 295 | public static FVector3 MoveTowards(FVector3 current, FVector3 target, FP maxDistanceDelta) 296 | { 297 | var direction = target - current; 298 | 299 | var sqrLength = LengthSqr(direction); 300 | 301 | if (sqrLength == FP.Zero || maxDistanceDelta >= FP.Zero && sqrLength <= maxDistanceDelta * maxDistanceDelta) 302 | return target; 303 | 304 | var distance = FMath.Sqrt(sqrLength); 305 | 306 | return current + direction / distance * maxDistanceDelta; 307 | } 308 | 309 | /// 310 | /// Returns the length of a vector. 311 | /// 312 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 313 | public static FP Length(FVector3 a) 314 | { 315 | return FMath.Sqrt(LengthSqr(a)); 316 | } 317 | 318 | /// 319 | /// Returns the squared length of a vector. 320 | /// 321 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 322 | public static FP LengthSqr(FVector3 a) 323 | { 324 | return Dot(a, a); 325 | } 326 | 327 | /// 328 | /// Returns the distance between a and b. 329 | /// 330 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 331 | public static FP Distance(FVector3 a, FVector3 b) 332 | { 333 | return FMath.Sqrt(DistanceSqr(a, b)); 334 | } 335 | 336 | /// 337 | /// Returns the squared distance between a and b. 338 | /// 339 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 340 | public static FP DistanceSqr(FVector3 a, FVector3 b) 341 | { 342 | var deltaX = a.X - b.X; 343 | var deltaY = a.Y - b.Y; 344 | var deltaZ = a.Z - b.Z; 345 | return deltaX * deltaX + deltaY * deltaY + deltaZ * deltaZ; 346 | } 347 | 348 | /// 349 | /// Returns a normalized version of a vector. 350 | /// 351 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 352 | public static FVector3 Normalize(FVector3 a) 353 | { 354 | var length = Length(a); 355 | return a / length; 356 | } 357 | 358 | /// 359 | /// Returns a safe normalized version of a vector. 360 | /// Returns the given default value when vector length close to zero. 361 | /// 362 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 363 | public static FVector3 NormalizeSafe(FVector3 a, FVector3 defaultValue = new FVector3()) 364 | { 365 | var lengthSqr = LengthSqr(a); 366 | if (lengthSqr < FP.CalculationsEpsilonSqr) 367 | { 368 | return defaultValue; 369 | } 370 | return a / FMath.Sqrt(lengthSqr); 371 | } 372 | 373 | /// 374 | /// Returns non-normalized perpendicular vector to a given one. For normalized see . 375 | /// 376 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 377 | public static FVector3 Orthogonal(FVector3 a) 378 | { 379 | return new FVector3( 380 | FMath.CopySign(a.Z, a.X), 381 | FMath.CopySign(a.Z, a.Y), 382 | -FMath.CopySign(a.X, a.Z) - FMath.CopySign(a.Y, a.Z)); 383 | } 384 | 385 | /// 386 | /// Returns orthogonal basis vector to a given one. 387 | /// 388 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 389 | public static FVector3 Orthonormal(FVector3 a) 390 | { 391 | var length = Length(a); 392 | var s = FMath.CopySign(length, a.Z); 393 | var h = a.Z + s; 394 | return new FVector3(s * h - a.X * a.X, -a.X * a.Y, -a.X * h); 395 | } 396 | 397 | /// 398 | /// Returns a vector that is made from the largest components of two vectors. 399 | /// 400 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 401 | public static FVector3 MaxComponents(FVector3 a, FVector3 b) 402 | { 403 | return new FVector3(FMath.Max(a.X, b.X), FMath.Max(a.Y, b.Y), FMath.Max(a.Z, b.Z)); 404 | } 405 | 406 | /// 407 | /// Returns a vector that is made from the smallest components of two vectors. 408 | /// 409 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 410 | public static FVector3 MinComponents(FVector3 a, FVector3 b) 411 | { 412 | return new FVector3(FMath.Min(a.X, b.X), FMath.Min(a.Y, b.Y), FMath.Min(a.Z, b.Z)); 413 | } 414 | 415 | /// 416 | /// Returns the componentwise absolute value of a vector. 417 | /// 418 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 419 | public static FVector3 AbsComponents(FVector3 a) 420 | { 421 | return new FVector3(FMath.Abs(a.X), FMath.Abs(a.Y), FMath.Abs(a.Z)); 422 | } 423 | 424 | /// 425 | /// Returns the componentwise signes of a vector. 426 | /// 427 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 428 | public static FVector3 SignComponents(FVector3 a) 429 | { 430 | return new FVector3(FMath.Sign(a.X), FMath.Sign(a.Y), FMath.Sign(a.Z)); 431 | } 432 | 433 | /// 434 | /// Compares two vectors with and returns true if they are similar. 435 | /// 436 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 437 | public static bool ApproximatelyEqual(FVector3 a, FVector3 b) 438 | { 439 | return ApproximatelyEqual(a, b, FP.CalculationsEpsilonSqr); 440 | } 441 | 442 | /// 443 | /// Compares two vectors with some epsilon and returns true if they are similar. 444 | /// 445 | [MethodImpl(MethodImplOptions.AggressiveInlining)] 446 | public static bool ApproximatelyEqual(FVector3 a, FVector3 b, FP epsilon) 447 | { 448 | return DistanceSqr(a, b) < epsilon; 449 | } 450 | } 451 | } 452 | -------------------------------------------------------------------------------- /Runtime/Structs/FVector3.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 3f60a9232d87427b8fbb2a54f05d008f 3 | timeCreated: 1737208061 -------------------------------------------------------------------------------- /Tests.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: c795e07803008b149bea3d7edfb02aa1 3 | folderAsset: yes 4 | DefaultImporter: 5 | externalObjects: {} 6 | userData: 7 | assetBundleName: 8 | assetBundleVariant: 9 | -------------------------------------------------------------------------------- /Tests/FPTests.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Collections; 3 | using NUnit.Framework; 4 | using UnityEngine; 5 | 6 | namespace Mathematics.Fixed 7 | { 8 | [TestFixture] 9 | public class FPTests 10 | { 11 | [TestCase(FP.HalfRaw, ExpectedResult = 0)] // Round to even 12 | [TestCase(FP.HalfRaw - FP.EpsilonRaw, ExpectedResult = 0)] 13 | [TestCase(FP.HalfRaw + FP.EpsilonRaw, ExpectedResult = 1)] 14 | [TestCase(FP.OneRaw + FP.HalfRaw, ExpectedResult = 2)] // Round to even 15 | [TestCase(FP.OneRaw + FP.HalfRaw - FP.EpsilonRaw, ExpectedResult = 1)] 16 | [TestCase(FP.OneRaw + FP.HalfRaw + FP.EpsilonRaw, ExpectedResult = 2)] 17 | [TestCase(FP.PiRaw, ExpectedResult = 3)] 18 | [TestCase(FP.Rad2DegRaw, ExpectedResult = 57)] 19 | public int RoundToInt(long rawValue) 20 | { 21 | // Arrange 22 | var fp = FP.FromRaw(rawValue); 23 | 24 | // Act 25 | var result = FMath.RoundToInt(fp); 26 | 27 | // Assert 28 | return result; 29 | } 30 | 31 | [TestCase(FP.HalfRaw, ExpectedResult = 0)] 32 | [TestCase(FP.OneRaw, ExpectedResult = 1)] 33 | [TestCase(FP.HalfRaw + FP.OneRaw, ExpectedResult = 1)] 34 | [TestCase(FP.OneRaw + FP.OneRaw, ExpectedResult = 2)] 35 | [TestCase(FP.PiRaw, ExpectedResult = 3)] 36 | [TestCase(FP.Rad2DegRaw, ExpectedResult = 57)] 37 | public int WhenCastToInt_ThenFloorToInt(long rawValue) 38 | { 39 | // Arrange 40 | var fp = FP.FromRaw(rawValue); 41 | 42 | // Act 43 | var result = fp.ToInt(); 44 | 45 | // Assert 46 | return result; 47 | } 48 | 49 | [TestCase(FP.HalfRaw, ExpectedResult = 1)] 50 | [TestCase(FP.OneRaw, ExpectedResult = 1)] 51 | [TestCase(FP.HalfRaw + FP.OneRaw, ExpectedResult = 2)] 52 | [TestCase(FP.OneRaw + FP.OneRaw, ExpectedResult = 2)] 53 | [TestCase(FP.PiRaw, ExpectedResult = 4)] 54 | [TestCase(FP.Rad2DegRaw, ExpectedResult = 58)] 55 | public int CeilToInt(long rawValue) 56 | { 57 | // Arrange 58 | var fp = FP.FromRaw(rawValue); 59 | 60 | // Act 61 | var result = FMath.CeilToInt(fp); 62 | 63 | // Assert 64 | return result; 65 | } 66 | 67 | [TestCase(FP.OneRaw, ExpectedResult = FP.OneRaw)] 68 | [TestCase(FP.HalfRaw, ExpectedResult = FP.HalfRaw)] 69 | [TestCase(-FP.PiRaw, ExpectedResult = -FP.PiRaw)] 70 | [TestCase(FP.MaxValueRaw, ExpectedResult = FP.MaxValueRaw)] 71 | [TestCase(FP.MaxValueRaw - 1, ExpectedResult = FP.MaxValueRaw - 1)] 72 | [TestCase(FP.MinValueRaw, ExpectedResult = FP.MinValueRaw)] 73 | public long WhenDevidedByOne_ThenReturnTheSameNumber(long rawValue) 74 | { 75 | // Arrange 76 | var fp = FP.FromRaw(rawValue); 77 | 78 | // Act 79 | var result = fp / FP.One; 80 | 81 | // Assert 82 | return result.RawValue; 83 | } 84 | 85 | [TestCaseSource(nameof(TestCases))] 86 | public void SqrtPrecise(FP value) 87 | { 88 | if (FMath.SignInt(value) < 0) 89 | { 90 | Assert.Throws(() => FMath.SqrtPrecise(value)); 91 | } 92 | else 93 | { 94 | var expected = Math.Sqrt(value.ToDouble()); 95 | var actual = FMath.SqrtPrecise(value).ToDouble(); 96 | var delta = Math.Abs(expected - actual); 97 | 98 | if (delta > FP.Epsilon.ToDouble()) 99 | { 100 | if (delta < (FP.Epsilon * 1000).ToDouble()) // It has some rare minor inaccuracies, and they are tied to absolute precision. 101 | { 102 | Debug.LogWarning($"sqrt({value}) = {actual}, but expected {expected}. Delta = {delta}."); 103 | } 104 | else 105 | { 106 | Assert.AreEqual(expected.ToFP(), actual, $"sqrt({value}) = {actual}, but expected {expected}. Delta = {delta}."); 107 | } 108 | } 109 | } 110 | } 111 | 112 | [TestCaseSource(nameof(TestCases))] 113 | public void SqrtApprox(FP value) 114 | { 115 | if (FMath.SignInt(value) < 0) 116 | { 117 | Assert.Throws(() => FMath.Sqrt(value)); 118 | } 119 | else 120 | { 121 | var expected = Math.Sqrt(value.ToDouble()); 122 | var actual = FP.FromRaw(FMath.Sqrt(value.RawValue)).ToDouble(); 123 | var delta = Math.Abs(expected - actual); 124 | 125 | var expectedEpsilon = FP.FromRaw(2 << (FMath.SqrtLutShift01 + 2)); 126 | if (delta > expectedEpsilon.ToDouble()) 127 | { 128 | Assert.AreEqual(expected.ToFP(), actual, $"sqrt({value}) = {actual}, but expected {expected}. Delta = {delta}."); 129 | } 130 | } 131 | } 132 | 133 | [TestCaseSource(nameof(TestCases))] 134 | public void Atan(FP value) 135 | { 136 | var expected = Math.Atan(value.ToDouble()); 137 | var actual = FMath.Atan(value); 138 | var delta = Math.Abs(expected - actual.ToDouble()); 139 | 140 | if (delta > 0.00000001) 141 | { 142 | Assert.AreEqual(expected.ToFP(), actual, $"Atan({value}) = {actual}, but expected {expected}. Delta = {delta}."); 143 | } 144 | } 145 | 146 | [TestCaseSource(nameof(PairTestCases))] 147 | public void Mul(FP a, FP b) 148 | { 149 | var aDouble = a.ToDouble(); 150 | var bDouble = b.ToDouble(); 151 | 152 | var expected = aDouble * bDouble; 153 | 154 | if (expected > FP.MaxValue.ToDouble()) 155 | { 156 | expected = FP.MaxValue.ToDouble(); 157 | } 158 | else if (expected < FP.MinValue.ToDouble()) 159 | { 160 | expected = FP.MinValue.ToDouble(); 161 | } 162 | 163 | var actual = a * b; 164 | var delta = Math.Abs(expected - actual.ToDouble()); 165 | 166 | if (delta > FP.Epsilon.ToDouble()) 167 | { 168 | if (delta < (FP.Epsilon * 2000).ToDouble()) // It has some rare minor inaccuracies, and they are tied to absolute precision. 169 | { 170 | Debug.LogWarning($"{a} * {b} = {actual}, but expected {expected}. Delta = {delta}."); 171 | } 172 | else 173 | { 174 | Assert.AreEqual(expected.ToFP(), actual, $"{a} * {b} = {actual}, but expected {expected}. Delta = {delta}."); 175 | } 176 | } 177 | } 178 | 179 | [TestCaseSource(nameof(PairTestCases))] 180 | public void Division(FP a, FP b) 181 | { 182 | var aDouble = a.ToDouble(); 183 | var bDouble = b.ToDouble(); 184 | 185 | if (a == FP.Zero) 186 | { 187 | var expected = FP.Zero; 188 | Assert.AreEqual(expected, a / b); 189 | return; 190 | } 191 | 192 | if (b == FP.Zero) 193 | { 194 | // Assert.Throws(() => Ignore(a / b)); 195 | 196 | var expected = a < FP.Zero ? FP.MinValue : FP.MaxValue; 197 | Assert.AreEqual(expected, a / b); 198 | return; 199 | } 200 | 201 | { 202 | var expected = aDouble / bDouble; 203 | 204 | // Expect saturation up to max and min values 205 | if (expected > FP.MaxValue.ToDouble()) 206 | { 207 | expected = FP.MaxValue.ToDouble(); 208 | } 209 | else if (expected < FP.MinValue.ToDouble()) 210 | { 211 | expected = FP.MinValue.ToDouble(); 212 | } 213 | 214 | var actual = a / b; 215 | var delta = Math.Abs(expected - actual.ToDouble()); 216 | 217 | if (delta > FP.Epsilon.ToDouble()) 218 | { 219 | if (delta < (FP.Epsilon * 2000).ToDouble()) // It has some rare minor inaccuracies, and they are tied to absolute precision. 220 | { 221 | Debug.LogWarning($"{a} / {b} = {actual}, but expected {expected}. Delta = {delta}."); 222 | } 223 | else 224 | { 225 | Assert.AreEqual(expected.ToFP(), actual, $"{a} / {b} = {actual}, but expected {expected}. Delta = {delta}."); 226 | } 227 | } 228 | } 229 | } 230 | 231 | [TestCaseSource(nameof(PairTestCases))] 232 | public void Atan2(FP y, FP x) 233 | { 234 | var yDouble = y.ToDouble(); 235 | var xDouble = x.ToDouble(); 236 | 237 | var expected = Math.Atan2(yDouble, xDouble); 238 | var actual = FMath.Atan2(y, x); 239 | var delta = Math.Abs(expected - actual.ToDouble()); 240 | 241 | if (delta > 0.005) 242 | { 243 | Assert.AreEqual(expected.ToFP(), actual, $"Atan({y}, {x}) = {actual}, but expected {expected}. Delta = {delta}."); 244 | } 245 | } 246 | 247 | private static void Ignore(T value) 248 | { 249 | } 250 | 251 | public static IEnumerable PairTestCases() 252 | { 253 | foreach (var a in TestCases) 254 | { 255 | foreach (var b in TestCases) 256 | { 257 | yield return new object[] { a, b }; 258 | } 259 | } 260 | } 261 | 262 | public static readonly FP[] TestCases = 263 | { 264 | // Small numbers 265 | FP.FromRaw(0), 266 | FP.FromRaw(1), FP.FromRaw(2), FP.FromRaw(3), FP.FromRaw(4), FP.FromRaw(5), 267 | FP.FromRaw(6), FP.FromRaw(7), FP.FromRaw(8), FP.FromRaw(9), FP.FromRaw(10), 268 | -FP.FromRaw(1), -FP.FromRaw(2), -FP.FromRaw(3), -FP.FromRaw(4), -FP.FromRaw(5), 269 | -FP.FromRaw(6), -FP.FromRaw(7), -FP.FromRaw(8), -FP.FromRaw(9), -FP.FromRaw(-10), 270 | 271 | // Integer numbers 272 | 1.ToFP(), 2.ToFP(), 3.ToFP(), 4.ToFP(), 5.ToFP(), 6.ToFP(), 273 | (-1).ToFP(), (-2).ToFP(), (-3).ToFP(), (-4).ToFP(), (-5).ToFP(), (-6).ToFP(), 274 | 275 | // Fractions (1/2, 1/4, 1/8) 276 | FP.FromRaw(FP.OneRaw / 2), FP.FromRaw(FP.OneRaw / 4), FP.FromRaw(FP.OneRaw / 8), 277 | -FP.FromRaw(FP.OneRaw / 2), -FP.FromRaw(FP.OneRaw / 4), -FP.FromRaw(FP.OneRaw / 8), 278 | 279 | // Problematic carry 280 | FP.FromRaw(FP.OneRaw - 1), -FP.FromRaw(FP.OneRaw - 1), 281 | FP.FromRaw(FP.OneRaw + 1), -FP.FromRaw(FP.OneRaw + 1), 282 | 283 | // Problematic log2 284 | FP.FromRaw(1L << (FP.FractionalBits + FP.IntegerBits / 2)), 285 | FP.FromRaw((1L << (FP.FractionalBits + FP.IntegerBits / 2)) - 1), 286 | FP.FromRaw((1L << (FP.FractionalBits + FP.IntegerBits / 2)) + 1), 287 | 288 | // PIs 289 | FP.Pi, FP.HalfPi, -FP.HalfPi, 290 | 291 | // Smallest and largest values 292 | FP.MaxValue, FP.MinValue, 293 | 294 | // Large random numbers 295 | FP.FromRaw(6791302811978701836), FP.FromRaw(-8192141831180282065), FP.FromRaw(6222617001063736300), FP.FromRaw(-7871200276881732034), 296 | FP.FromRaw(8249382838880205112), FP.FromRaw(-7679310892959748444), FP.FromRaw(7708113189940799513), FP.FromRaw(-5281862979887936768), 297 | FP.FromRaw(8220231180772321456), FP.FromRaw(-5204203381295869580), FP.FromRaw(6860614387764479339), FP.FromRaw(-9080626825133349457), 298 | FP.FromRaw(6658610233456189347), FP.FromRaw(-6558014273345705245), FP.FromRaw(6700571222183426493), 299 | 300 | // Small random numbers 301 | FP.FromRaw(-436730658), FP.FromRaw(-2259913246), FP.FromRaw(329347474), FP.FromRaw(2565801981), FP.FromRaw(3398143698), FP.FromRaw(137497017), FP.FromRaw(1060347500), 302 | FP.FromRaw(-3457686027), FP.FromRaw(1923669753), FP.FromRaw(2891618613), FP.FromRaw(2418874813), FP.FromRaw(2899594950), FP.FromRaw(2265950765), FP.FromRaw(-1962365447), 303 | FP.FromRaw(3077934393), 304 | 305 | // Tiny random numbers 306 | FP.FromRaw(-171), 307 | FP.FromRaw(-359), FP.FromRaw(491), FP.FromRaw(844), FP.FromRaw(158), FP.FromRaw(-413), FP.FromRaw(-422), FP.FromRaw(-737), FP.FromRaw(-575), FP.FromRaw(-330), 308 | FP.FromRaw(-376), FP.FromRaw(435), FP.FromRaw(-311), FP.FromRaw(116), FP.FromRaw(715), FP.FromRaw(-1024), FP.FromRaw(-487), FP.FromRaw(59), FP.FromRaw(724), FP.FromRaw(993) 309 | }; 310 | } 311 | } 312 | -------------------------------------------------------------------------------- /Tests/FPTests.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: c6d723e5fdd74bd598119515321b7b2c 3 | timeCreated: 1737305446 -------------------------------------------------------------------------------- /Tests/FQuaternionTests.cs: -------------------------------------------------------------------------------- 1 | using NUnit.Framework; 2 | 3 | namespace Mathematics.Fixed 4 | { 5 | [TestFixture] 6 | public class FQuaternionTests 7 | { 8 | [Test] 9 | public static void WhenDot_AndRotationsAreTheSame_ThenEqualOne() 10 | { 11 | // Arrange 12 | var rotation180 = new FQuaternion(FP.One, FP.Zero, FP.Zero, FP.Zero); 13 | 14 | // Act 15 | FP dot = FQuaternion.Dot(rotation180, rotation180); 16 | 17 | // Assert 18 | Assert.IsTrue(FMath.ApproximatelyEqual(dot, FP.One)); 19 | } 20 | 21 | [Test] 22 | public static void WhenDot_AndRotationsAreOpposite_ThenEqualNegativeOne() 23 | { 24 | // Arrange 25 | var rotation180 = new FQuaternion(FP.One, FP.Zero, FP.Zero, FP.Zero); 26 | var rotationMinus180 = new FQuaternion(-FP.One, FP.Zero, FP.Zero, FP.Zero); 27 | 28 | // Act 29 | FP negativeDot = FQuaternion.Dot(rotation180, rotationMinus180); 30 | 31 | // Assert 32 | Assert.IsTrue(FMath.ApproximatelyEqual(negativeDot, FP.MinusOne)); 33 | } 34 | 35 | [Test] 36 | public static void When180RotationAppliedToVector_ThenFlipTheVector() 37 | { 38 | // Arrange 39 | FVector3 point = FVector3.Up; 40 | FQuaternion rotation180 = new FQuaternion(FP.One, FP.Zero, FP.Zero, FP.Zero); 41 | 42 | // Act 43 | FVector3 transformed = rotation180 * point; 44 | 45 | // Assert 46 | Assert.IsTrue(transformed == FVector3.Down); 47 | } 48 | 49 | [Test] 50 | public static void WhenScaledRotationAppliedToVector_ThenScaleTheVector() 51 | { 52 | // Arrange 53 | FVector3 point = FVector3.Up; 54 | FQuaternion scaling9 = new FQuaternion(FP.Zero, FP.Zero, FP.Zero, 3.ToFP()); 55 | 56 | // Act 57 | FVector3 transformed = FQuaternion.Sandwich(scaling9, point); 58 | 59 | // Assert 60 | Assert.IsTrue(transformed == FVector3.Up * 9.ToFP()); 61 | } 62 | } 63 | } 64 | -------------------------------------------------------------------------------- /Tests/FQuaternionTests.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: e13811af8c624270a8c64f15145bf392 3 | timeCreated: 1737302950 -------------------------------------------------------------------------------- /Tests/FixedPoint.Tests.asmdef: -------------------------------------------------------------------------------- 1 | { 2 | "name": "FixedPoint.Tests", 3 | "rootNamespace": "", 4 | "references": [ 5 | "UnityEngine.TestRunner", 6 | "UnityEditor.TestRunner", 7 | "FixedPoint" 8 | ], 9 | "includePlatforms": [ 10 | "Editor" 11 | ], 12 | "excludePlatforms": [], 13 | "allowUnsafeCode": false, 14 | "overrideReferences": true, 15 | "precompiledReferences": [ 16 | "nunit.framework.dll" 17 | ], 18 | "autoReferenced": false, 19 | "defineConstraints": [ 20 | "UNITY_INCLUDE_TESTS" 21 | ], 22 | "versionDefines": [], 23 | "noEngineReferences": false 24 | } -------------------------------------------------------------------------------- /Tests/FixedPoint.Tests.asmdef.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 199b6d8fa16945242a6905f9ead07882 3 | AssemblyDefinitionImporter: 4 | externalObjects: {} 5 | userData: 6 | assetBundleName: 7 | assetBundleVariant: 8 | -------------------------------------------------------------------------------- /package.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "fixed-point", 3 | "version": "0.2.0", 4 | "displayName": "Fixed Point", 5 | "unity": "2021.2", 6 | "description": "Fixed point math.", 7 | "author": "nilpunch" 8 | } -------------------------------------------------------------------------------- /package.json.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 50cc008664b39634f95e82f0aaf864a3 3 | TextScriptImporter: 4 | externalObjects: {} 5 | userData: 6 | assetBundleName: 7 | assetBundleVariant: 8 | --------------------------------------------------------------------------------