| | | 1 | | //======================================================================= |
| | | 2 | | // PhysicsMesh.cs |
| | | 3 | | //======================================================================= |
| | | 4 | | // MIT License, Copyright (c) 2026–present David Oravsky (mrdav30) |
| | | 5 | | // See LICENSE file in the project root for full license information. |
| | | 6 | | //======================================================================= |
| | | 7 | | |
| | | 8 | | using FixedMathSharp; |
| | | 9 | | using FixedMathSharp.Geometry; |
| | | 10 | | using SwiftCollections; |
| | | 11 | | using SwiftCollections.Query; |
| | | 12 | | using System; |
| | | 13 | | using System.Collections.Generic; |
| | | 14 | | |
| | | 15 | | namespace Gravitas.Colliders |
| | | 16 | | { |
| | | 17 | | /// <summary> |
| | | 18 | | /// Owns immutable authored triangle topology and committed deterministic runtime mesh geometry. |
| | | 19 | | /// </summary> |
| | | 20 | | public partial class PhysicsMesh |
| | | 21 | | { |
| | | 22 | | private const int SupportTreeVertexThreshold = 32; |
| | | 23 | | private const int SupportTreeLeafVertexCount = 8; |
| | | 24 | | private const int SupportTreeStackCapacity = 64; |
| | | 25 | | |
| | | 26 | | /// <summary> |
| | | 27 | | /// Maximum accepted vertex count for deterministic runtime mesh construction. |
| | | 28 | | /// </summary> |
| | | 29 | | public const int MaxVertexCount = 65535; |
| | | 30 | | |
| | | 31 | | /// <summary> |
| | | 32 | | /// Maximum accepted triangle count for deterministic runtime mesh construction. |
| | | 33 | | /// </summary> |
| | | 34 | | public const int MaxTriangleCount = 131072; |
| | | 35 | | |
| | 1 | 36 | | private static readonly Fixed64 TetrahedronVolumeDivisor = (Fixed64)6; |
| | 1 | 37 | | private static readonly Fixed64 TetrahedronCentroidDivisor = (Fixed64)4; |
| | 1 | 38 | | private static readonly Fixed64 SecondMomentIntegralDivisor = (Fixed64)10; |
| | 1 | 39 | | private static readonly Fixed64 ProductMomentIntegralDivisor = (Fixed64)20; |
| | | 40 | | |
| | | 41 | | private readonly Vector3d[] _localVertices; |
| | | 42 | | |
| | | 43 | | private Vector3d[] _scaledLocalVertices; |
| | | 44 | | private Vector3d[] _preparedScaledLocalVertices; |
| | | 45 | | private int[]? _supportVertexIndices; |
| | | 46 | | private SupportTreeNode[]? _supportTreeNodes; |
| | | 47 | | private SupportTreeNode[]? _preparedSupportTreeNodes; |
| | | 48 | | private int _supportTreeNodeCount; |
| | | 49 | | |
| | | 50 | | /// <summary> |
| | | 51 | | /// Holds the source vertices in local mesh space. |
| | | 52 | | /// </summary> |
| | 161 | 53 | | internal ReadOnlySpan<Vector3d> LocalVertices => _localVertices; |
| | | 54 | | |
| | | 55 | | /// <summary> |
| | | 56 | | /// Holds the committed scaled vertices as center-relative local |
| | | 57 | | /// offsets. Collision relations apply <see cref="Rotation"/> without |
| | | 58 | | /// materializing absolute world points. |
| | | 59 | | /// </summary> |
| | | 60 | | internal ReadOnlySpan<Vector3d> ScaledLocalVertices => |
| | 485 | 61 | | _scaledLocalVertices; |
| | | 62 | | |
| | | 63 | | /// <summary> |
| | | 64 | | /// Number of vertices in the immutable local mesh topology. |
| | | 65 | | /// </summary> |
| | 1822 | 66 | | public int VertexCount => _localVertices.Length; |
| | | 67 | | |
| | | 68 | | private readonly int[] _triangles; |
| | | 69 | | /// <summary> |
| | | 70 | | /// Holds all the triangles that make up the mesh in the form of indices to the vertices array. |
| | | 71 | | /// example: Triangles[0-3] = 4,2,3 means that the first triangle of the mesh is made up of the vertices _vertic |
| | | 72 | | /// </summary> |
| | 727 | 73 | | public ReadOnlySpan<int> Triangles => _triangles; |
| | | 74 | | |
| | | 75 | | private readonly int _triangleCount; |
| | | 76 | | /// <summary>Gets the number of authored triangles.</summary> |
| | 2894 | 77 | | public int TriangleCount => _triangleCount; |
| | | 78 | | |
| | | 79 | | private readonly int[] _convexSatEdgeVertexPairs; |
| | 485 | 80 | | internal ReadOnlySpan<int> ConvexSatEdgeVertexPairs => _convexSatEdgeVertexPairs; |
| | | 81 | | |
| | | 82 | | /// <summary>Gets the mesh collision mode.</summary> |
| | | 83 | | public MeshColliderMode Mode { get; } |
| | | 84 | | |
| | | 85 | | /// <summary> |
| | | 86 | | /// Gets whether the complete exact-position-welded triangle topology is one |
| | | 87 | | /// connected, consistently wound, closed two-manifold surface. |
| | | 88 | | /// </summary> |
| | | 89 | | public bool IsClosedSurface { get; private set; } |
| | | 90 | | |
| | | 91 | | /// <summary> |
| | | 92 | | /// Gets the current triangle surface area after local scale. |
| | | 93 | | /// </summary> |
| | 54 | 94 | | public Fixed64 TotalArea => _scaledTotalArea; |
| | | 95 | | |
| | | 96 | | private int _triangleBvhBuildCount; |
| | | 97 | | private SwiftFixedBVH<int> _triangleBVH; |
| | | 98 | | private SwiftFixedBVH<int> _preparedTriangleBVH; |
| | | 99 | | |
| | | 100 | | /// <summary> |
| | | 101 | | /// Triangle acceleration structure in local mesh space. |
| | | 102 | | /// </summary> |
| | 10570 | 103 | | internal SwiftFixedBVH<int> TriangleBVH => _triangleBVH; |
| | | 104 | | |
| | | 105 | | /// <summary>Gets the number of committed triangle BVH builds.</summary> |
| | 11 | 106 | | public int TriangleBvhBuildCount => _triangleBvhBuildCount; |
| | | 107 | | |
| | 417 | 108 | | private FixedQuaternion _rotation = FixedQuaternion.Identity; |
| | | 109 | | |
| | | 110 | | /// <summary> |
| | | 111 | | /// Gets the committed world-space origin of the mesh's rigid frame. |
| | | 112 | | /// </summary> |
| | 82143 | 113 | | internal Vector3d Origin => _position; |
| | | 114 | | |
| | | 115 | | /// <summary> |
| | | 116 | | /// Gets the committed local-to-world mesh orientation. |
| | | 117 | | /// </summary> |
| | 83348 | 118 | | internal FixedQuaternion Rotation => _rotation; |
| | | 119 | | |
| | | 120 | | private FixedBoundBox _bounds; |
| | | 121 | | /// <summary>Gets the committed world-space axis-aligned bounds.</summary> |
| | 344 | 122 | | public FixedBoundBox Bounds => _bounds; |
| | | 123 | | |
| | | 124 | | private readonly FixedBoundBox _localBounds; |
| | | 125 | | |
| | | 126 | | /// <summary> |
| | | 127 | | /// Axis-aligned bounds of the source vertices in local mesh space. |
| | | 128 | | /// </summary> |
| | 651 | 129 | | public FixedBoundBox LocalBounds => _localBounds; |
| | | 130 | | |
| | | 131 | | /// <summary>Creates a convex runtime mesh at a world-space pose.</summary> |
| | | 132 | | public PhysicsMesh(Vector3d[] vertices, int[] triangles, Vector3d position, FixedQuaternion rotation) |
| | 83 | 133 | | : this(vertices, triangles, position, rotation, MeshColliderMode.Convex) { } |
| | | 134 | | |
| | | 135 | | /// <summary>Creates a runtime mesh with an explicit collision mode and world-space pose.</summary> |
| | 417 | 136 | | public PhysicsMesh( |
| | 417 | 137 | | Vector3d[] vertices, |
| | 417 | 138 | | int[] triangles, |
| | 417 | 139 | | Vector3d position, |
| | 417 | 140 | | FixedQuaternion rotation, |
| | 417 | 141 | | MeshColliderMode mode) |
| | | 142 | | { |
| | 417 | 143 | | SwiftThrowHelper.ThrowIfArgument( |
| | 417 | 144 | | mode != MeshColliderMode.Convex && mode != MeshColliderMode.Concave, |
| | 417 | 145 | | nameof(mode), |
| | 417 | 146 | | "Unsupported mesh collider mode."); |
| | | 147 | | |
| | 417 | 148 | | ValidateInput(vertices, triangles); |
| | | 149 | | |
| | 406 | 150 | | Mode = mode; |
| | 406 | 151 | | _localVertices = new Vector3d[vertices.Length]; |
| | 406 | 152 | | Array.Copy(vertices, _localVertices, vertices.Length); |
| | 406 | 153 | | _scaledLocalVertices = new Vector3d[vertices.Length]; |
| | 406 | 154 | | _preparedScaledLocalVertices = new Vector3d[vertices.Length]; |
| | 406 | 155 | | _triangles = new int[triangles.Length]; |
| | 406 | 156 | | Array.Copy(triangles, _triangles, triangles.Length); |
| | 406 | 157 | | _triangleCount = triangles.Length / 3; // 3 vertices per triangle |
| | 406 | 158 | | _triangleBVH = new SwiftFixedBVH<int>(2 * TriangleCount - 1); |
| | 406 | 159 | | _preparedTriangleBVH = new SwiftFixedBVH<int>(2 * TriangleCount - 1); |
| | 406 | 160 | | _scaledFaceAreas = new Fixed64[TriangleCount]; |
| | 406 | 161 | | _preparedScaledFaceAreas = new Fixed64[TriangleCount]; |
| | 406 | 162 | | _scaledFaceNormals = new Vector3d[TriangleCount]; |
| | 406 | 163 | | _preparedScaledFaceNormals = new Vector3d[TriangleCount]; |
| | | 164 | | |
| | 406 | 165 | | _localBounds = CalculateBounds(_localVertices); |
| | | 166 | | |
| | | 167 | | // Scale validation proves every vertex span and triangle cross product used |
| | | 168 | | // by the exact topology predicates below is representable. |
| | 406 | 169 | | int[] topologyTriangles = CreateTopologyTriangles(_localVertices, _triangles); |
| | 406 | 170 | | _convexSatEdgeVertexPairs = Mode == MeshColliderMode.Convex |
| | 406 | 171 | | ? CreateConvexSatEdgeVertexPairs(_localVertices, topologyTriangles, _triangleCount) |
| | 406 | 172 | | : Array.Empty<int>(); |
| | 397 | 173 | | if (Mode == MeshColliderMode.Concave) |
| | | 174 | | { |
| | 165 | 175 | | IsClosedSurface = EvaluateClosedVolumeTopology(topologyTriangles, out _surfaceClosureValidationResult); |
| | | 176 | | } |
| | | 177 | | |
| | 397 | 178 | | if (Mode == MeshColliderMode.Convex && _localVertices.Length > SupportTreeVertexThreshold) |
| | | 179 | | { |
| | 12 | 180 | | _supportVertexIndices = CreateSupportVertexIndices(_localVertices.Length); |
| | 12 | 181 | | _supportTreeNodes = new SupportTreeNode[(2 * _localVertices.Length) - 1]; |
| | 12 | 182 | | _preparedSupportTreeNodes = new SupportTreeNode[(2 * _localVertices.Length) - 1]; |
| | | 183 | | } |
| | | 184 | | |
| | 397 | 185 | | PrepareTransformation(position, rotation, Vector3d.One, Vector3d.One, null); |
| | 396 | 186 | | PublishPreparedTransformation(); |
| | 396 | 187 | | } |
| | | 188 | | |
| | | 189 | | /// <summary>Updates the mesh world-space position and rigid rotation.</summary> |
| | | 190 | | public void UpdatePosition(Vector3d position, FixedQuaternion rotation) |
| | | 191 | | { |
| | 330 | 192 | | PrepareTransformation(position, rotation, _ownerScale, _partScale, null); |
| | 330 | 193 | | PublishPreparedTransformation(); |
| | 330 | 194 | | } |
| | | 195 | | |
| | | 196 | | /// <summary> |
| | | 197 | | /// Updates the mesh center, normalized rigid rotation, and strictly positive authored scale. |
| | | 198 | | /// </summary> |
| | | 199 | | public void UpdateTransform(Vector3d position, FixedQuaternion rotation, Vector3d scale) |
| | | 200 | | { |
| | 23 | 201 | | PrepareTransformation(position, rotation, scale, Vector3d.One, null); |
| | 18 | 202 | | PublishPreparedTransformation(); |
| | 18 | 203 | | } |
| | | 204 | | |
| | | 205 | | private static void ValidateInput(Vector3d[] vertices, int[] triangles) |
| | | 206 | | { |
| | 417 | 207 | | SwiftThrowHelper.ThrowIfNull(vertices, nameof(vertices)); |
| | 416 | 208 | | SwiftThrowHelper.ThrowIfNull(triangles, nameof(triangles)); |
| | 415 | 209 | | SwiftThrowHelper.ThrowIfArgument(vertices.Length < 3, nameof(vertices), "Mesh must contain at least three ve |
| | 415 | 210 | | SwiftThrowHelper.ThrowIfArgument(triangles.Length == 0 || triangles.Length % 3 != 0, nameof(triangles), "Tri |
| | 413 | 211 | | SwiftThrowHelper.ThrowIfArgumentOutOfRange(vertices.Length > MaxVertexCount, vertices.Length, nameof(vertice |
| | | 212 | | |
| | 412 | 213 | | int triangleCount = triangles.Length / 3; |
| | 412 | 214 | | SwiftThrowHelper.ThrowIfArgumentOutOfRange(triangleCount > MaxTriangleCount, triangleCount, nameof(triangles |
| | | 215 | | |
| | 412 | 216 | | var referencedVertices = new bool[vertices.Length]; |
| | 261370 | 217 | | for (int i = 0; i < triangleCount; i++) |
| | | 218 | | { |
| | 130277 | 219 | | int index0 = triangles[i * 3]; |
| | 130277 | 220 | | int index1 = triangles[i * 3 + 1]; |
| | 130277 | 221 | | int index2 = triangles[i * 3 + 2]; |
| | | 222 | | |
| | 130277 | 223 | | SwiftThrowHelper.ThrowIfArgumentOutOfRange((uint)index0 >= (uint)vertices.Length, vertices.Length, nameo |
| | 130277 | 224 | | SwiftThrowHelper.ThrowIfArgumentOutOfRange((uint)index1 >= (uint)vertices.Length, vertices.Length, nameo |
| | 130277 | 225 | | SwiftThrowHelper.ThrowIfArgumentOutOfRange((uint)index2 >= (uint)vertices.Length, vertices.Length, nameo |
| | | 226 | | |
| | 130276 | 227 | | SwiftThrowHelper.ThrowIfArgument( |
| | 130276 | 228 | | index0 == index1 || index1 == index2 || index2 == index0, |
| | 130276 | 229 | | nameof(triangles), |
| | 130276 | 230 | | "Triangle indices must be unique within each triangle."); |
| | | 231 | | |
| | 130275 | 232 | | Fixed64 area = new FixedTriangle( |
| | 130275 | 233 | | vertices[index0], |
| | 130275 | 234 | | vertices[index1], |
| | 130275 | 235 | | vertices[index2]).Area; |
| | 130275 | 236 | | SwiftThrowHelper.ThrowIfArgument(area <= Fixed64.Epsilon, nameof(triangles), "Degenerate triangles are n |
| | | 237 | | |
| | 130273 | 238 | | referencedVertices[index0] = true; |
| | 130273 | 239 | | referencedVertices[index1] = true; |
| | 130273 | 240 | | referencedVertices[index2] = true; |
| | | 241 | | } |
| | | 242 | | |
| | 34880 | 243 | | for (int i = 0; i < referencedVertices.Length; i++) |
| | 17034 | 244 | | SwiftThrowHelper.ThrowIfArgument(!referencedVertices[i], nameof(vertices), "Every mesh vertex must be re |
| | 406 | 245 | | } |
| | | 246 | | |
| | | 247 | | private void BuildTriangleBVH( |
| | | 248 | | SwiftFixedBVH<int> bvh, |
| | | 249 | | ReadOnlySpan<Vector3d> vertices) |
| | | 250 | | { |
| | 453 | 251 | | bvh.Clear(); |
| | 324794 | 252 | | for (int i = 0; i < _triangleCount; i++) |
| | | 253 | | { |
| | 161944 | 254 | | int index0 = _triangles[i * 3]; |
| | 161944 | 255 | | int index1 = _triangles[i * 3 + 1]; |
| | 161944 | 256 | | int index2 = _triangles[i * 3 + 2]; |
| | 161944 | 257 | | Vector3d min = Vector3d.Min(Vector3d.Min(vertices[index0], vertices[index1]), vertices[index2]); |
| | 161944 | 258 | | Vector3d max = Vector3d.Max(Vector3d.Max(vertices[index0], vertices[index1]), vertices[index2]); |
| | 161944 | 259 | | bvh.Insert(i, new FixedBoundVolume(min, max)); |
| | | 260 | | } |
| | 453 | 261 | | } |
| | | 262 | | |
| | | 263 | | private static FixedBoundBox CalculateBounds(Vector3d[] vertices) |
| | | 264 | | { |
| | 861 | 265 | | Vector3d min = vertices[0]; |
| | 861 | 266 | | Vector3d max = vertices[0]; |
| | 100074 | 267 | | for (int i = 1; i < vertices.Length; i++) |
| | | 268 | | { |
| | 49176 | 269 | | min = Vector3d.Min(min, vertices[i]); |
| | 49176 | 270 | | max = Vector3d.Max(max, vertices[i]); |
| | | 271 | | } |
| | | 272 | | |
| | 861 | 273 | | return FixedBoundBox.FromMinMax(min, max); |
| | | 274 | | } |
| | | 275 | | |
| | | 276 | | /// <summary>Compares deterministic edge uses for topology validation.</summary> |
| | | 277 | | private static int CompareEdgeUses(EdgeUse first, EdgeUse second) |
| | | 278 | | { |
| | 1439978 | 279 | | if (first.Key != second.Key) |
| | 1394489 | 280 | | return first.Key < second.Key ? -1 : 1; |
| | | 281 | | |
| | 45489 | 282 | | if (first.TriangleIndex == second.TriangleIndex) |
| | 48 | 283 | | return 0; |
| | | 284 | | |
| | 45441 | 285 | | return first.TriangleIndex < second.TriangleIndex ? -1 : 1; |
| | | 286 | | } |
| | | 287 | | |
| | | 288 | | /// <summary>Gets the projected mesh area facing a world-space direction.</summary> |
| | | 289 | | public Fixed64 GetFrontalArea(Vector3d direction) |
| | | 290 | | { |
| | 12 | 291 | | Fixed64 directionMagnitude = direction.Magnitude; |
| | 12 | 292 | | if (directionMagnitude <= Fixed64.Epsilon) |
| | 1 | 293 | | return Fixed64.Zero; |
| | | 294 | | |
| | 11 | 295 | | Vector3d normalizedDirection = direction / directionMagnitude; |
| | 11 | 296 | | Fixed64 totalArea = Fixed64.Zero; |
| | 82 | 297 | | for (int i = 0; i < _triangleCount; i++) |
| | | 298 | | { |
| | 30 | 299 | | Fixed64 projection = Vector3d.Dot(GetFaceNormalWorld(i), normalizedDirection); |
| | 30 | 300 | | if (projection > Fixed64.Zero) |
| | 13 | 301 | | totalArea += _scaledFaceAreas[i] * FixedMath.Min(projection, Fixed64.One); |
| | | 302 | | } |
| | | 303 | | |
| | 11 | 304 | | return totalArea; |
| | | 305 | | } |
| | | 306 | | |
| | | 307 | | /// <summary>Gets one triangle's vertices in world space.</summary> |
| | | 308 | | public void GetTriangleVertices(int index, out Vector3d first, out Vector3d second, out Vector3d third) |
| | | 309 | | { |
| | 4 | 310 | | if (TryGetTriangleVertices(index, out first, out second, out third)) |
| | 3 | 311 | | return; |
| | | 312 | | |
| | 1 | 313 | | throw new InvalidOperationException( |
| | 1 | 314 | | "At least one triangle vertex lies outside the Fixed64 world-coordinate domain."); |
| | | 315 | | } |
| | | 316 | | |
| | | 317 | | /// <summary> |
| | | 318 | | /// Attempts to materialize one triangle's absolute world vertices. |
| | | 319 | | /// Canonical collision and query paths should consume scaled-local |
| | | 320 | | /// geometry instead. |
| | | 321 | | /// </summary> |
| | | 322 | | public bool TryGetTriangleVertices( |
| | | 323 | | int index, |
| | | 324 | | out Vector3d first, |
| | | 325 | | out Vector3d second, |
| | | 326 | | out Vector3d third) |
| | | 327 | | { |
| | 279 | 328 | | GetLocalTriangleVertices( |
| | 279 | 329 | | index, |
| | 279 | 330 | | out Vector3d localFirst, |
| | 279 | 331 | | out Vector3d localSecond, |
| | 279 | 332 | | out Vector3d localThird); |
| | 279 | 333 | | var firstAnchor = new FixedPointAnchor( |
| | 279 | 334 | | _position, |
| | 279 | 335 | | _rotation, |
| | 279 | 336 | | localFirst); |
| | 279 | 337 | | var secondAnchor = new FixedPointAnchor( |
| | 279 | 338 | | _position, |
| | 279 | 339 | | _rotation, |
| | 279 | 340 | | localSecond); |
| | 279 | 341 | | var thirdAnchor = new FixedPointAnchor( |
| | 279 | 342 | | _position, |
| | 279 | 343 | | _rotation, |
| | 279 | 344 | | localThird); |
| | 279 | 345 | | bool representable = |
| | 279 | 346 | | firstAnchor.TryGetPoint(out first) |
| | 279 | 347 | | & secondAnchor.TryGetPoint(out second) |
| | 279 | 348 | | & thirdAnchor.TryGetPoint(out third); |
| | 279 | 349 | | if (representable) |
| | 273 | 350 | | return true; |
| | | 351 | | |
| | 6 | 352 | | first = default; |
| | 6 | 353 | | second = default; |
| | 6 | 354 | | third = default; |
| | 6 | 355 | | return false; |
| | | 356 | | } |
| | | 357 | | |
| | | 358 | | /// <summary> |
| | | 359 | | /// Gets one triangle's committed scaled vertices in local mesh space. |
| | | 360 | | /// </summary> |
| | | 361 | | public void GetLocalTriangleVertices(int index, out Vector3d first, out Vector3d second, out Vector3d third) |
| | | 362 | | { |
| | 43624 | 363 | | SwiftThrowHelper.ThrowIfArrayIndexInvalid(index, _triangleCount, nameof(index)); |
| | | 364 | | |
| | 43624 | 365 | | int triangleIndex = index * 3; |
| | 43624 | 366 | | first = _scaledLocalVertices[_triangles[triangleIndex]]; |
| | 43624 | 367 | | second = _scaledLocalVertices[_triangles[triangleIndex + 1]]; |
| | 43624 | 368 | | third = _scaledLocalVertices[_triangles[triangleIndex + 2]]; |
| | 43624 | 369 | | } |
| | | 370 | | |
| | | 371 | | /// <summary> |
| | | 372 | | /// Gets one triangle's committed scaled normal in local mesh space. |
| | | 373 | | /// </summary> |
| | | 374 | | internal Vector3d GetScaledLocalFaceNormal(int index) |
| | | 375 | | { |
| | 970 | 376 | | SwiftThrowHelper.ThrowIfArrayIndexInvalid(index, _triangleCount, nameof(index)); |
| | 970 | 377 | | return _scaledFaceNormals[index]; |
| | | 378 | | } |
| | | 379 | | |
| | | 380 | | /// <summary> |
| | | 381 | | /// Attempts to materialize one scaled vertex in world space. |
| | | 382 | | /// </summary> |
| | | 383 | | public bool TryGetVertexWorld(int index, out Vector3d vertex) |
| | | 384 | | { |
| | 1830 | 385 | | SwiftThrowHelper.ThrowIfArrayIndexInvalid(index, _localVertices.Length, nameof(index)); |
| | 1830 | 386 | | return CreatePointAnchor(_scaledLocalVertices[index]) |
| | 1830 | 387 | | .TryGetPoint(out vertex); |
| | | 388 | | } |
| | | 389 | | |
| | | 390 | | /// <summary> |
| | | 391 | | /// Materializes one scaled vertex in world space. |
| | | 392 | | /// </summary> |
| | | 393 | | /// <exception cref="InvalidOperationException"> |
| | | 394 | | /// The conceptual vertex lies outside the representable world-coordinate |
| | | 395 | | /// domain. |
| | | 396 | | /// </exception> |
| | | 397 | | public Vector3d GetVertexWorld(int index) |
| | | 398 | | { |
| | 1823 | 399 | | if (TryGetVertexWorld(index, out Vector3d vertex)) |
| | 1822 | 400 | | return vertex; |
| | | 401 | | |
| | 1 | 402 | | throw new InvalidOperationException( |
| | 1 | 403 | | "The selected vertex lies outside the Fixed64 world-coordinate domain."); |
| | | 404 | | } |
| | | 405 | | |
| | | 406 | | /// <summary> |
| | | 407 | | /// Attempts to materialize the world-space vertex with the greatest |
| | | 408 | | /// projection onto <paramref name="direction"/>, preserving source |
| | | 409 | | /// vertex order for ties. |
| | | 410 | | /// </summary> |
| | | 411 | | public bool TryGetSupportVertexWorld( |
| | | 412 | | Vector3d direction, |
| | | 413 | | out Vector3d vertex) |
| | | 414 | | { |
| | 36 | 415 | | Vector3d localPoint = GetSupportVertexLocal(direction); |
| | 36 | 416 | | return CreatePointAnchor(localPoint).TryGetPoint(out vertex); |
| | | 417 | | } |
| | | 418 | | |
| | | 419 | | /// <summary> |
| | | 420 | | /// Materializes the world-space vertex with the greatest projection |
| | | 421 | | /// onto <paramref name="direction"/>, preserving source vertex order |
| | | 422 | | /// for ties. |
| | | 423 | | /// </summary> |
| | | 424 | | /// <exception cref="InvalidOperationException"> |
| | | 425 | | /// The conceptual support vertex lies outside the representable |
| | | 426 | | /// world-coordinate domain. |
| | | 427 | | /// </exception> |
| | | 428 | | public Vector3d GetSupportVertexWorld(Vector3d direction) |
| | | 429 | | { |
| | 35 | 430 | | if (TryGetSupportVertexWorld(direction, out Vector3d vertex)) |
| | 34 | 431 | | return vertex; |
| | | 432 | | |
| | 1 | 433 | | throw new InvalidOperationException( |
| | 1 | 434 | | "The selected support vertex lies outside the Fixed64 world-coordinate domain."); |
| | | 435 | | } |
| | | 436 | | |
| | | 437 | | internal Vector3d GetSupportVertexLocal(Vector3d direction) |
| | | 438 | | { |
| | 331 | 439 | | Vector3d localDirection = ConvertWorldDirectionToLocal(direction); |
| | 331 | 440 | | localDirection = localDirection != Vector3d.Zero |
| | 331 | 441 | | ? localDirection.Normalized |
| | 331 | 442 | | : Vector3d.Right; |
| | | 443 | | |
| | 331 | 444 | | if (_supportTreeNodes != null && _supportVertexIndices != null) |
| | 21 | 445 | | return _scaledLocalVertices[FindSupportVertexIndex(localDirection)]; |
| | | 446 | | |
| | 310 | 447 | | int bestIndex = 0; |
| | 4924 | 448 | | for (int i = 1; i < _scaledLocalVertices.Length; i++) |
| | | 449 | | { |
| | 2152 | 450 | | if (Vector3d.CompareProjection( |
| | 2152 | 451 | | _scaledLocalVertices[i], |
| | 2152 | 452 | | _scaledLocalVertices[bestIndex], |
| | 2152 | 453 | | localDirection) <= 0) |
| | | 454 | | continue; |
| | | 455 | | |
| | 624 | 456 | | bestIndex = i; |
| | | 457 | | } |
| | | 458 | | |
| | 310 | 459 | | return _scaledLocalVertices[bestIndex]; |
| | | 460 | | } |
| | | 461 | | |
| | | 462 | | private int FindSupportVertexIndex(Vector3d localDirection) |
| | | 463 | | { |
| | 21 | 464 | | int bestIndex = 0; |
| | 21 | 465 | | Span<int> stack = stackalloc int[SupportTreeStackCapacity]; |
| | 21 | 466 | | int stackCount = 0; |
| | 21 | 467 | | stack[stackCount++] = 0; |
| | | 468 | | |
| | 242 | 469 | | while (stackCount > 0) |
| | | 470 | | { |
| | 221 | 471 | | int nodeIndex = stack[--stackCount]; |
| | 221 | 472 | | SupportTreeNode node = _supportTreeNodes![nodeIndex]; |
| | 221 | 473 | | Vector3d upperPoint = GetBoundsSupportPoint(node.Min, node.Max, localDirection); |
| | 221 | 474 | | int upperComparison = Vector3d.CompareProjection( |
| | 221 | 475 | | upperPoint, |
| | 221 | 476 | | _scaledLocalVertices[bestIndex], |
| | 221 | 477 | | localDirection); |
| | 221 | 478 | | if (upperComparison < 0 || (upperComparison == 0 && node.MinVertexIndex >= bestIndex)) |
| | | 479 | | continue; |
| | | 480 | | |
| | 165 | 481 | | if (node.IsLeaf) |
| | | 482 | | { |
| | 40 | 483 | | SearchSupportLeaf(node, localDirection, ref bestIndex); |
| | 40 | 484 | | continue; |
| | | 485 | | } |
| | | 486 | | |
| | 125 | 487 | | SupportTreeNode left = _supportTreeNodes[node.Left]; |
| | 125 | 488 | | SupportTreeNode right = _supportTreeNodes[node.Right]; |
| | | 489 | | |
| | 125 | 490 | | if (ComesBeforeSupportNode(left, right, localDirection)) |
| | | 491 | | { |
| | 46 | 492 | | PushSupportNode(right, bestIndex, localDirection, stack, ref stackCount); |
| | 46 | 493 | | PushSupportNode(left, bestIndex, localDirection, stack, ref stackCount); |
| | 46 | 494 | | continue; |
| | | 495 | | } |
| | | 496 | | |
| | 79 | 497 | | PushSupportNode(left, bestIndex, localDirection, stack, ref stackCount); |
| | 79 | 498 | | PushSupportNode(right, bestIndex, localDirection, stack, ref stackCount); |
| | | 499 | | } |
| | | 500 | | |
| | 21 | 501 | | return bestIndex; |
| | | 502 | | } |
| | | 503 | | |
| | | 504 | | private void SearchSupportLeaf( |
| | | 505 | | SupportTreeNode node, |
| | | 506 | | Vector3d localDirection, |
| | | 507 | | ref int bestIndex) |
| | | 508 | | { |
| | 542 | 509 | | for (int i = 0; i < node.Count; i++) |
| | | 510 | | { |
| | 231 | 511 | | int vertexIndex = _supportVertexIndices![node.Start + i]; |
| | 231 | 512 | | Vector3d vertex = _scaledLocalVertices[vertexIndex]; |
| | 231 | 513 | | int projectionComparison = Vector3d.CompareProjection( |
| | 231 | 514 | | vertex, |
| | 231 | 515 | | _scaledLocalVertices[bestIndex], |
| | 231 | 516 | | localDirection); |
| | 231 | 517 | | if (projectionComparison < 0 || (projectionComparison == 0 && vertexIndex >= bestIndex)) |
| | | 518 | | continue; |
| | | 519 | | |
| | 42 | 520 | | bestIndex = vertexIndex; |
| | | 521 | | } |
| | 40 | 522 | | } |
| | | 523 | | |
| | | 524 | | private void PushSupportNode( |
| | | 525 | | SupportTreeNode node, |
| | | 526 | | int bestIndex, |
| | | 527 | | Vector3d localDirection, |
| | | 528 | | Span<int> stack, |
| | | 529 | | ref int stackCount) |
| | | 530 | | { |
| | 250 | 531 | | Vector3d upperPoint = GetBoundsSupportPoint(node.Min, node.Max, localDirection); |
| | 250 | 532 | | int upperComparison = Vector3d.CompareProjection( |
| | 250 | 533 | | upperPoint, |
| | 250 | 534 | | _scaledLocalVertices[bestIndex], |
| | 250 | 535 | | localDirection); |
| | 250 | 536 | | if (upperComparison < 0 || (upperComparison == 0 && node.MinVertexIndex >= bestIndex)) |
| | 50 | 537 | | return; |
| | | 538 | | |
| | 200 | 539 | | stack[stackCount++] = node.Index; |
| | 200 | 540 | | } |
| | | 541 | | |
| | | 542 | | private static bool ComesBeforeSupportNode( |
| | | 543 | | SupportTreeNode left, |
| | | 544 | | SupportTreeNode right, |
| | | 545 | | Vector3d localDirection) |
| | | 546 | | { |
| | 125 | 547 | | Vector3d leftPoint = GetBoundsSupportPoint(left.Min, left.Max, localDirection); |
| | 125 | 548 | | Vector3d rightPoint = GetBoundsSupportPoint(right.Min, right.Max, localDirection); |
| | 125 | 549 | | int projectionComparison = Vector3d.CompareProjection( |
| | 125 | 550 | | leftPoint, |
| | 125 | 551 | | rightPoint, |
| | 125 | 552 | | localDirection); |
| | 125 | 553 | | if (projectionComparison != 0) |
| | 69 | 554 | | return projectionComparison > 0; |
| | | 555 | | |
| | 56 | 556 | | return left.MinVertexIndex < right.MinVertexIndex; |
| | | 557 | | } |
| | | 558 | | |
| | | 559 | | private int BuildSupportTreeNode( |
| | | 560 | | Vector3d[] vertices, |
| | | 561 | | int[] vertexIndices, |
| | | 562 | | SupportTreeNode[] nodes, |
| | | 563 | | SupportVertexIndexComparer comparer, |
| | | 564 | | ref int nodeCount, |
| | | 565 | | int start, |
| | | 566 | | int count) |
| | | 567 | | { |
| | 4744 | 568 | | int nodeIndex = nodeCount++; |
| | | 569 | | |
| | 4744 | 570 | | CalculateSupportRangeBounds( |
| | 4744 | 571 | | vertices, |
| | 4744 | 572 | | vertexIndices, |
| | 4744 | 573 | | start, |
| | 4744 | 574 | | count, |
| | 4744 | 575 | | out Vector3d min, |
| | 4744 | 576 | | out Vector3d max, |
| | 4744 | 577 | | out int minVertexIndex); |
| | 4744 | 578 | | if (count <= SupportTreeLeafVertexCount) |
| | | 579 | | { |
| | 2378 | 580 | | nodes[nodeIndex] = SupportTreeNode.CreateLeaf( |
| | 2378 | 581 | | nodeIndex, |
| | 2378 | 582 | | min, |
| | 2378 | 583 | | max, |
| | 2378 | 584 | | start, |
| | 2378 | 585 | | count, |
| | 2378 | 586 | | minVertexIndex); |
| | 2378 | 587 | | return nodeIndex; |
| | | 588 | | } |
| | | 589 | | |
| | 2366 | 590 | | int axis = GetDominantAxis(max - min); |
| | 2366 | 591 | | comparer.Reset(vertices, axis); |
| | 2366 | 592 | | Array.Sort( |
| | 2366 | 593 | | vertexIndices, |
| | 2366 | 594 | | start, |
| | 2366 | 595 | | count, |
| | 2366 | 596 | | comparer); |
| | | 597 | | |
| | 2366 | 598 | | int leftCount = count / 2; |
| | 2366 | 599 | | int rightCount = count - leftCount; |
| | 2366 | 600 | | int leftIndex = BuildSupportTreeNode( |
| | 2366 | 601 | | vertices, |
| | 2366 | 602 | | vertexIndices, |
| | 2366 | 603 | | nodes, |
| | 2366 | 604 | | comparer, |
| | 2366 | 605 | | ref nodeCount, |
| | 2366 | 606 | | start, |
| | 2366 | 607 | | leftCount); |
| | 2366 | 608 | | int rightIndex = BuildSupportTreeNode( |
| | 2366 | 609 | | vertices, |
| | 2366 | 610 | | vertexIndices, |
| | 2366 | 611 | | nodes, |
| | 2366 | 612 | | comparer, |
| | 2366 | 613 | | ref nodeCount, |
| | 2366 | 614 | | start + leftCount, |
| | 2366 | 615 | | rightCount); |
| | 2366 | 616 | | nodes[nodeIndex] = SupportTreeNode.CreateBranch( |
| | 2366 | 617 | | nodeIndex, |
| | 2366 | 618 | | min, |
| | 2366 | 619 | | max, |
| | 2366 | 620 | | leftIndex, |
| | 2366 | 621 | | rightIndex, |
| | 2366 | 622 | | minVertexIndex); |
| | 2366 | 623 | | return nodeIndex; |
| | | 624 | | } |
| | | 625 | | |
| | | 626 | | private void RefitSupportTree( |
| | | 627 | | Vector3d[] vertices, |
| | | 628 | | SupportTreeNode[] sourceNodes, |
| | | 629 | | SupportTreeNode[] targetNodes) |
| | | 630 | | { |
| | | 631 | | // Children follow their parent in the immutable preorder topology, |
| | | 632 | | // so reverse traversal makes both child bounds available first. |
| | 10528 | 633 | | for (int i = _supportTreeNodeCount - 1; i >= 0; i--) |
| | | 634 | | { |
| | 5252 | 635 | | SupportTreeNode source = sourceNodes[i]; |
| | 5252 | 636 | | if (source.IsLeaf) |
| | | 637 | | { |
| | 2632 | 638 | | CalculateSupportRangeBounds( |
| | 2632 | 639 | | vertices, |
| | 2632 | 640 | | _supportVertexIndices!, |
| | 2632 | 641 | | source.Start, |
| | 2632 | 642 | | source.Count, |
| | 2632 | 643 | | out Vector3d min, |
| | 2632 | 644 | | out Vector3d max, |
| | 2632 | 645 | | out int minVertexIndex); |
| | 2632 | 646 | | targetNodes[i] = SupportTreeNode.CreateLeaf( |
| | 2632 | 647 | | source.Index, |
| | 2632 | 648 | | min, |
| | 2632 | 649 | | max, |
| | 2632 | 650 | | source.Start, |
| | 2632 | 651 | | source.Count, |
| | 2632 | 652 | | minVertexIndex); |
| | 2632 | 653 | | continue; |
| | | 654 | | } |
| | | 655 | | |
| | 2620 | 656 | | SupportTreeNode left = targetNodes[source.Left]; |
| | 2620 | 657 | | SupportTreeNode right = targetNodes[source.Right]; |
| | 2620 | 658 | | targetNodes[i] = SupportTreeNode.CreateBranch( |
| | 2620 | 659 | | source.Index, |
| | 2620 | 660 | | Vector3d.Min(left.Min, right.Min), |
| | 2620 | 661 | | Vector3d.Max(left.Max, right.Max), |
| | 2620 | 662 | | source.Left, |
| | 2620 | 663 | | source.Right, |
| | 2620 | 664 | | source.MinVertexIndex); |
| | | 665 | | } |
| | 12 | 666 | | } |
| | | 667 | | |
| | | 668 | | private static void CalculateSupportRangeBounds( |
| | | 669 | | Vector3d[] vertices, |
| | | 670 | | int[] vertexIndices, |
| | | 671 | | int start, |
| | | 672 | | int count, |
| | | 673 | | out Vector3d min, |
| | | 674 | | out Vector3d max, |
| | | 675 | | out int minVertexIndex) |
| | | 676 | | { |
| | 7376 | 677 | | int firstIndex = vertexIndices[start]; |
| | 7376 | 678 | | minVertexIndex = firstIndex; |
| | 7376 | 679 | | min = vertices[firstIndex]; |
| | 7376 | 680 | | max = min; |
| | 328386 | 681 | | for (int i = 1; i < count; i++) |
| | | 682 | | { |
| | 156817 | 683 | | int vertexIndex = vertexIndices[start + i]; |
| | 156817 | 684 | | Vector3d vertex = vertices[vertexIndex]; |
| | 156817 | 685 | | min = Vector3d.Min(min, vertex); |
| | 156817 | 686 | | max = Vector3d.Max(max, vertex); |
| | 156817 | 687 | | if (vertexIndex < minVertexIndex) |
| | 2442 | 688 | | minVertexIndex = vertexIndex; |
| | | 689 | | } |
| | 7376 | 690 | | } |
| | | 691 | | |
| | | 692 | | private static int[] CreateSupportVertexIndices(int vertexCount) |
| | | 693 | | { |
| | 12 | 694 | | var indices = new int[vertexCount]; |
| | 28612 | 695 | | for (int i = 0; i < indices.Length; i++) |
| | 14294 | 696 | | indices[i] = i; |
| | | 697 | | |
| | 12 | 698 | | return indices; |
| | | 699 | | } |
| | | 700 | | |
| | | 701 | | private static int GetDominantAxis(Vector3d extents) |
| | | 702 | | { |
| | 2366 | 703 | | if (extents.X >= extents.Y && extents.X >= extents.Z) |
| | 720 | 704 | | return 0; |
| | | 705 | | |
| | 1646 | 706 | | return extents.Y >= extents.Z ? 1 : 2; |
| | | 707 | | } |
| | | 708 | | |
| | | 709 | | private static Vector3d GetBoundsSupportPoint(Vector3d min, Vector3d max, Vector3d direction) => |
| | 721 | 710 | | new( |
| | 721 | 711 | | direction.X >= Fixed64.Zero ? max.X : min.X, |
| | 721 | 712 | | direction.Y >= Fixed64.Zero ? max.Y : min.Y, |
| | 721 | 713 | | direction.Z >= Fixed64.Zero ? max.Z : min.Z); |
| | | 714 | | |
| | | 715 | | /// <summary>Gets a triangle face normal in world space.</summary> |
| | | 716 | | public Vector3d GetFaceNormalWorld(int index) |
| | | 717 | | { |
| | 1380 | 718 | | SwiftThrowHelper.ThrowIfArrayIndexInvalid(index, _triangleCount, nameof(index)); |
| | 1380 | 719 | | _ = _rotation.TryRotate( |
| | 1380 | 720 | | _scaledFaceNormals[index], |
| | 1380 | 721 | | out Vector3d worldNormal); |
| | 1380 | 722 | | return worldNormal.Normalized; |
| | | 723 | | } |
| | | 724 | | |
| | | 725 | | /// <summary>Writes triangle indices overlapping world-space bounds into a caller-owned buffer.</summary> |
| | | 726 | | public void GetTrianglesInWorldBounds(FixedBoundVolume worldBounds, SwiftList<int> result) |
| | | 727 | | { |
| | 2346 | 728 | | result.FastClear(); |
| | 2346 | 729 | | FixedBoundBox localBounds = |
| | 2346 | 730 | | FixedBoundBox.FromRelativeRotatedBoundsClippedToDomain( |
| | 2346 | 731 | | Vector3d.Zero, |
| | 2346 | 732 | | FixedQuaternion.Identity, |
| | 2346 | 733 | | worldBounds.Min, |
| | 2346 | 734 | | worldBounds.Max, |
| | 2346 | 735 | | _position, |
| | 2346 | 736 | | _rotation); |
| | 2346 | 737 | | TriangleBVH.Query( |
| | 2346 | 738 | | new FixedBoundVolume(localBounds.Min, localBounds.Max), |
| | 2346 | 739 | | result); |
| | 2346 | 740 | | } |
| | | 741 | | |
| | | 742 | | /// <summary>Writes triangle indices overlapping scaled-local bounds into a caller-owned buffer.</summary> |
| | | 743 | | public void GetTrianglesInLocalBounds(FixedBoundVolume localBounds, SwiftList<int> result) |
| | | 744 | | { |
| | 8221 | 745 | | result.FastClear(); |
| | 8221 | 746 | | TriangleBVH.Query(localBounds, result); |
| | 8221 | 747 | | } |
| | | 748 | | |
| | | 749 | | /// <summary> |
| | | 750 | | /// Attempts to express a world point in the committed scaled-local |
| | | 751 | | /// mesh frame without saturating the relative displacement. |
| | | 752 | | /// </summary> |
| | | 753 | | public bool TryConvertWorldToScaledLocal( |
| | | 754 | | Vector3d worldPoint, |
| | | 755 | | out Vector3d localPoint) => |
| | 1017 | 756 | | new FixedPointAnchor( |
| | 1017 | 757 | | worldPoint, |
| | 1017 | 758 | | FixedQuaternion.Identity, |
| | 1017 | 759 | | Vector3d.Zero) |
| | 1017 | 760 | | .TryGetLocalPointIn( |
| | 1017 | 761 | | _position, |
| | 1017 | 762 | | _rotation, |
| | 1017 | 763 | | out localPoint); |
| | | 764 | | |
| | | 765 | | /// <summary> |
| | | 766 | | /// Expresses a world point in the committed scaled-local mesh frame. |
| | | 767 | | /// </summary> |
| | | 768 | | /// <exception cref="InvalidOperationException"> |
| | | 769 | | /// The exact relative displacement lies outside the representable |
| | | 770 | | /// coordinate domain. |
| | | 771 | | /// </exception> |
| | | 772 | | public Vector3d ConvertWorldToScaledLocal(Vector3d worldPoint) |
| | | 773 | | { |
| | 2 | 774 | | if (TryConvertWorldToScaledLocal(worldPoint, out Vector3d localPoint)) |
| | 1 | 775 | | return localPoint; |
| | | 776 | | |
| | 1 | 777 | | throw new InvalidOperationException( |
| | 1 | 778 | | "The world point cannot be represented in the mesh's scaled-local frame."); |
| | | 779 | | } |
| | | 780 | | |
| | | 781 | | /// <summary> |
| | | 782 | | /// Attempts to materialize a scaled-local mesh point in world space. |
| | | 783 | | /// </summary> |
| | | 784 | | public bool TryConvertScaledLocalToWorld( |
| | | 785 | | Vector3d scaledLocalPoint, |
| | | 786 | | out Vector3d worldPoint) => |
| | 543 | 787 | | CreatePointAnchor(scaledLocalPoint).TryGetPoint(out worldPoint); |
| | | 788 | | |
| | | 789 | | /// <summary> |
| | | 790 | | /// Materializes a scaled-local mesh point in world space. |
| | | 791 | | /// </summary> |
| | | 792 | | /// <exception cref="InvalidOperationException"> |
| | | 793 | | /// The conceptual world point lies outside the representable |
| | | 794 | | /// coordinate domain. |
| | | 795 | | /// </exception> |
| | | 796 | | public Vector3d ConvertScaledLocalToWorld(Vector3d scaledLocalPoint) |
| | | 797 | | { |
| | 3 | 798 | | if (TryConvertScaledLocalToWorld( |
| | 3 | 799 | | scaledLocalPoint, |
| | 3 | 800 | | out Vector3d worldPoint)) |
| | | 801 | | { |
| | 2 | 802 | | return worldPoint; |
| | | 803 | | } |
| | | 804 | | |
| | 1 | 805 | | throw new InvalidOperationException( |
| | 1 | 806 | | "The scaled-local point lies outside the Fixed64 world-coordinate domain."); |
| | | 807 | | } |
| | | 808 | | |
| | | 809 | | internal FixedPointAnchor CreatePointAnchor( |
| | | 810 | | Vector3d scaledLocalPoint) => |
| | 4147 | 811 | | new( |
| | 4147 | 812 | | _position, |
| | 4147 | 813 | | _rotation, |
| | 4147 | 814 | | scaledLocalPoint); |
| | | 815 | | |
| | | 816 | | /// <summary> |
| | | 817 | | /// Attempts to express a world-space direction in the committed |
| | | 818 | | /// scaled-local mesh frame. |
| | | 819 | | /// </summary> |
| | | 820 | | public bool TryConvertWorldDirectionToLocal( |
| | | 821 | | Vector3d worldDirection, |
| | | 822 | | out Vector3d localDirection) => |
| | 333 | 823 | | _rotation.Inverse().TryRotate( |
| | 333 | 824 | | worldDirection, |
| | 333 | 825 | | out localDirection); |
| | | 826 | | |
| | | 827 | | /// <summary> |
| | | 828 | | /// Expresses a world-space direction in the committed scaled-local |
| | | 829 | | /// mesh frame. |
| | | 830 | | /// </summary> |
| | | 831 | | public Vector3d ConvertWorldDirectionToLocal( |
| | | 832 | | Vector3d worldDirection) |
| | | 833 | | { |
| | 332 | 834 | | if (TryConvertWorldDirectionToLocal( |
| | 332 | 835 | | worldDirection, |
| | 332 | 836 | | out Vector3d localDirection)) |
| | | 837 | | { |
| | 331 | 838 | | return localDirection; |
| | | 839 | | } |
| | | 840 | | |
| | 1 | 841 | | throw new InvalidOperationException( |
| | 1 | 842 | | "The world direction cannot be represented in the mesh's scaled-local frame."); |
| | | 843 | | } |
| | | 844 | | |
| | | 845 | | private readonly struct EdgeUse |
| | | 846 | | { |
| | | 847 | | private EdgeUse(long key, int direction, int triangleIndex) |
| | | 848 | | { |
| | 390813 | 849 | | Key = key; |
| | 390813 | 850 | | Direction = direction; |
| | 390813 | 851 | | TriangleIndex = triangleIndex; |
| | 390813 | 852 | | } |
| | | 853 | | |
| | | 854 | | public long Key { get; } |
| | | 855 | | |
| | 222628 | 856 | | public int StartVertexIndex => (int)(Key >> 32); |
| | | 857 | | |
| | 222628 | 858 | | public int EndVertexIndex => (int)(uint)Key; |
| | | 859 | | |
| | | 860 | | public int Direction { get; } |
| | | 861 | | |
| | | 862 | | public int TriangleIndex { get; } |
| | | 863 | | |
| | | 864 | | public static EdgeUse Create(int start, int end, int triangleIndex) |
| | | 865 | | { |
| | 390813 | 866 | | int min = start < end ? start : end; |
| | 390813 | 867 | | int max = start < end ? end : start; |
| | 390813 | 868 | | long key = ((long)min << 32) | (uint)max; |
| | 390813 | 869 | | int direction = start == min ? 1 : -1; |
| | 390813 | 870 | | return new EdgeUse(key, direction, triangleIndex); |
| | | 871 | | } |
| | | 872 | | } |
| | | 873 | | |
| | | 874 | | private readonly struct TriangleUse |
| | | 875 | | { |
| | | 876 | | private TriangleUse(int a, int b, int c) |
| | | 877 | | { |
| | 130271 | 878 | | A = a; |
| | 130271 | 879 | | B = b; |
| | 130271 | 880 | | C = c; |
| | 130271 | 881 | | } |
| | | 882 | | |
| | | 883 | | public int A { get; } |
| | | 884 | | |
| | | 885 | | public int B { get; } |
| | | 886 | | |
| | | 887 | | public int C { get; } |
| | | 888 | | |
| | | 889 | | public static TriangleUse Create(int first, int second, int third) |
| | | 890 | | { |
| | 130271 | 891 | | int a = first; |
| | 130271 | 892 | | int b = second; |
| | 130271 | 893 | | int c = third; |
| | | 894 | | |
| | 130271 | 895 | | if (a > b) |
| | 7268 | 896 | | (a, b) = (b, a); |
| | 130271 | 897 | | if (b > c) |
| | 14818 | 898 | | (b, c) = (c, b); |
| | 130271 | 899 | | if (a > b) |
| | 6737 | 900 | | (a, b) = (b, a); |
| | | 901 | | |
| | 130271 | 902 | | return new TriangleUse(a, b, c); |
| | | 903 | | } |
| | | 904 | | } |
| | | 905 | | |
| | | 906 | | private readonly struct SupportTreeNode |
| | | 907 | | { |
| | | 908 | | private SupportTreeNode( |
| | | 909 | | int index, |
| | | 910 | | Vector3d min, |
| | | 911 | | Vector3d max, |
| | | 912 | | int left, |
| | | 913 | | int right, |
| | | 914 | | int start, |
| | | 915 | | int count, |
| | | 916 | | int minVertexIndex) |
| | | 917 | | { |
| | 9996 | 918 | | Index = index; |
| | 9996 | 919 | | Min = min; |
| | 9996 | 920 | | Max = max; |
| | 9996 | 921 | | Left = left; |
| | 9996 | 922 | | Right = right; |
| | 9996 | 923 | | Start = start; |
| | 9996 | 924 | | Count = count; |
| | 9996 | 925 | | MinVertexIndex = minVertexIndex; |
| | 9996 | 926 | | } |
| | | 927 | | |
| | | 928 | | public int Index { get; } |
| | | 929 | | |
| | | 930 | | public Vector3d Min { get; } |
| | | 931 | | |
| | | 932 | | public Vector3d Max { get; } |
| | | 933 | | |
| | | 934 | | public int Left { get; } |
| | | 935 | | |
| | | 936 | | public int Right { get; } |
| | | 937 | | |
| | | 938 | | public int Start { get; } |
| | | 939 | | |
| | | 940 | | public int Count { get; } |
| | | 941 | | |
| | | 942 | | public int MinVertexIndex { get; } |
| | | 943 | | |
| | 5417 | 944 | | public bool IsLeaf => Count > 0; |
| | | 945 | | |
| | | 946 | | public static SupportTreeNode CreateLeaf( |
| | | 947 | | int index, |
| | | 948 | | Vector3d min, |
| | | 949 | | Vector3d max, |
| | | 950 | | int start, |
| | | 951 | | int count, |
| | | 952 | | int minVertexIndex) => |
| | 5010 | 953 | | new(index, min, max, -1, -1, start, count, minVertexIndex); |
| | | 954 | | |
| | | 955 | | public static SupportTreeNode CreateBranch( |
| | | 956 | | int index, |
| | | 957 | | Vector3d min, |
| | | 958 | | Vector3d max, |
| | | 959 | | int left, |
| | | 960 | | int right, |
| | | 961 | | int minVertexIndex) => |
| | 4986 | 962 | | new(index, min, max, left, right, 0, 0, minVertexIndex); |
| | | 963 | | } |
| | | 964 | | |
| | | 965 | | private sealed class SupportVertexIndexComparer : IComparer<int> |
| | | 966 | | { |
| | 12 | 967 | | private Vector3d[] _vertices = Array.Empty<Vector3d>(); |
| | | 968 | | private int _axis; |
| | | 969 | | |
| | | 970 | | public void Reset(Vector3d[] vertices, int axis) |
| | | 971 | | { |
| | 2366 | 972 | | _vertices = vertices; |
| | 2366 | 973 | | _axis = axis; |
| | 2366 | 974 | | } |
| | | 975 | | |
| | | 976 | | public int Compare(int first, int second) |
| | | 977 | | { |
| | 1072003 | 978 | | Fixed64 firstValue = GetAxisValue(_vertices[first], _axis); |
| | 1072003 | 979 | | Fixed64 secondValue = GetAxisValue(_vertices[second], _axis); |
| | 1072003 | 980 | | int valueComparison = firstValue.CompareTo(secondValue); |
| | 1072003 | 981 | | if (valueComparison != 0) |
| | 535743 | 982 | | return valueComparison; |
| | | 983 | | |
| | 536260 | 984 | | return first.CompareTo(second); |
| | | 985 | | } |
| | | 986 | | |
| | | 987 | | private static Fixed64 GetAxisValue(Vector3d vertex, int axis) |
| | | 988 | | { |
| | 2144006 | 989 | | return axis switch |
| | 2144006 | 990 | | { |
| | 799798 | 991 | | 0 => vertex.X, |
| | 735626 | 992 | | 1 => vertex.Y, |
| | 608582 | 993 | | _ => vertex.Z |
| | 2144006 | 994 | | }; |
| | | 995 | | } |
| | | 996 | | } |
| | | 997 | | } |
| | | 998 | | } |