| | | 1 | | //======================================================================= |
| | | 2 | | // HexCoordinateUtility.cs |
| | | 3 | | //======================================================================= |
| | | 4 | | // MIT License, Copyright (c) 2024–present David Oravsky (mrdav30) |
| | | 5 | | // See LICENSE file in the project root for full license information. |
| | | 6 | | //======================================================================= |
| | | 7 | | |
| | | 8 | | using FixedMathSharp; |
| | | 9 | | using GridForge.Grids.Topology; |
| | | 10 | | using System; |
| | | 11 | | using System.Runtime.CompilerServices; |
| | | 12 | | |
| | | 13 | | namespace GridForge.Spatial; |
| | | 14 | | |
| | | 15 | | internal static class HexCoordinateUtility |
| | | 16 | | { |
| | | 17 | | internal const long Sqrt3Raw = 7439101574L; |
| | | 18 | | |
| | 1 | 19 | | internal static readonly Fixed64 Sqrt3 = Fixed64.FromRaw(Sqrt3Raw); |
| | | 20 | | |
| | 1 | 21 | | private static readonly Fixed64 RawRoundingTolerance = Fixed64.FromRaw(4096); |
| | | 22 | | |
| | | 23 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 24 | | internal static Vector3d AxialToWorldOffset(VoxelIndex index, GridTopologyMetrics metrics) |
| | | 25 | | { |
| | 2652 | 26 | | Fixed64 q = new(index.x); |
| | 2652 | 27 | | Fixed64 r = new(index.z); |
| | 2652 | 28 | | Fixed64 y = index.y * metrics.LayerHeight; |
| | | 29 | | |
| | 2652 | 30 | | if (metrics.HexOrientation == HexOrientation.FlatTop) |
| | | 31 | | { |
| | 1078 | 32 | | return new Vector3d( |
| | 1078 | 33 | | metrics.CellRadius * Fixed64.Three * Fixed64.Half * q, |
| | 1078 | 34 | | y, |
| | 1078 | 35 | | metrics.CellRadius * Sqrt3 * (r + q * Fixed64.Half)); |
| | | 36 | | } |
| | | 37 | | |
| | 1574 | 38 | | return new Vector3d( |
| | 1574 | 39 | | metrics.CellRadius * Sqrt3 * (q + r * Fixed64.Half), |
| | 1574 | 40 | | y, |
| | 1574 | 41 | | metrics.CellRadius * Fixed64.Three * Fixed64.Half * r); |
| | | 42 | | } |
| | | 43 | | |
| | | 44 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 45 | | internal static void WorldOffsetToAxial( |
| | | 46 | | Fixed64 worldX, |
| | | 47 | | Fixed64 worldZ, |
| | | 48 | | GridTopologyMetrics metrics, |
| | | 49 | | out Fixed64 q, |
| | | 50 | | out Fixed64 r) |
| | | 51 | | { |
| | 393 | 52 | | if (metrics.HexOrientation == HexOrientation.FlatTop) |
| | | 53 | | { |
| | 51 | 54 | | q = (Fixed64.Two * worldX / Fixed64.Three) / metrics.CellRadius; |
| | 51 | 55 | | r = ((Sqrt3 * worldZ / Fixed64.Three) - (worldX / Fixed64.Three)) / metrics.CellRadius; |
| | 51 | 56 | | return; |
| | | 57 | | } |
| | | 58 | | |
| | 342 | 59 | | q = ((Sqrt3 * worldX / Fixed64.Three) - (worldZ / Fixed64.Three)) / metrics.CellRadius; |
| | 342 | 60 | | r = (Fixed64.Two * worldZ / Fixed64.Three) / metrics.CellRadius; |
| | 342 | 61 | | } |
| | | 62 | | |
| | | 63 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 64 | | internal static VoxelIndex RoundAxial(Fixed64 q, Fixed64 y, Fixed64 r) |
| | | 65 | | { |
| | 114 | 66 | | RoundCube(q, r, out int roundedQ, out int roundedR); |
| | 114 | 67 | | return new VoxelIndex(roundedQ, y.FloorToInt(), roundedR); |
| | | 68 | | } |
| | | 69 | | |
| | | 70 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 71 | | internal static void RoundCube(Fixed64 q, Fixed64 r, out int roundedQ, out int roundedR) |
| | | 72 | | { |
| | 138 | 73 | | Fixed64 s = -q - r; |
| | | 74 | | |
| | 138 | 75 | | int qCandidate = RoundToInt(q); |
| | 138 | 76 | | int rCandidate = RoundToInt(r); |
| | 138 | 77 | | int sCandidate = RoundToInt(s); |
| | | 78 | | |
| | 138 | 79 | | Fixed64 qDiff = (new Fixed64(qCandidate) - q).Abs(); |
| | 138 | 80 | | Fixed64 rDiff = (new Fixed64(rCandidate) - r).Abs(); |
| | 138 | 81 | | Fixed64 sDiff = (new Fixed64(sCandidate) - s).Abs(); |
| | | 82 | | |
| | 138 | 83 | | if (qDiff > rDiff && qDiff > sDiff) |
| | 3 | 84 | | qCandidate = -rCandidate - sCandidate; |
| | 135 | 85 | | else if (rDiff > sDiff) |
| | 3 | 86 | | rCandidate = -qCandidate - sCandidate; |
| | | 87 | | |
| | 138 | 88 | | roundedQ = qCandidate; |
| | 138 | 89 | | roundedR = rCandidate; |
| | 138 | 90 | | } |
| | | 91 | | |
| | | 92 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 93 | | internal static int CeilToIntWithTolerance(Fixed64 value) |
| | | 94 | | { |
| | 373 | 95 | | int rounded = RoundToInt(value); |
| | 373 | 96 | | if ((new Fixed64(rounded) - value).Abs() <= RawRoundingTolerance) |
| | 353 | 97 | | return rounded; |
| | | 98 | | |
| | 20 | 99 | | return value.CeilToInt(); |
| | | 100 | | } |
| | | 101 | | |
| | | 102 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 103 | | private static int RoundToInt(Fixed64 value) => |
| | 787 | 104 | | (int)(FixedMath.Round(value, MidpointRounding.AwayFromZero).m_rawValue >> FixedMath.SHIFT_AMOUNT_I); |
| | | 105 | | } |