| | | 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 System; |
| | | 9 | | using System.Collections.Generic; |
| | | 10 | | using FixedMathSharp; |
| | | 11 | | using FixedMathSharp.Geometry; |
| | | 12 | | using SwiftCollections; |
| | | 13 | | using SwiftCollections.Query; |
| | | 14 | | |
| | | 15 | | namespace Gravitas.Colliders |
| | | 16 | | { |
| | | 17 | | /// <summary> |
| | | 18 | | /// Owns immutable authored triangle topology and committed deterministic runtime mesh geometry. |
| | | 19 | | /// </summary> |
| | | 20 | | /// <content>Construction, geometry inspection, and shared state for mesh transformation, topology and queries.</con |
| | | 21 | | public partial class PhysicsMesh |
| | | 22 | | { |
| | | 23 | | private const int SupportTreeVertexThreshold = 32; |
| | | 24 | | private const int SupportTreeLeafVertexCount = 8; |
| | | 25 | | private const int SupportTreeStackCapacity = 64; |
| | | 26 | | |
| | | 27 | | /// <summary> |
| | | 28 | | /// Maximum accepted vertex count for deterministic runtime mesh construction. |
| | | 29 | | /// </summary> |
| | | 30 | | public const int MaxVertexCount = 65535; |
| | | 31 | | |
| | | 32 | | /// <summary> |
| | | 33 | | /// Maximum accepted triangle count for deterministic runtime mesh construction. |
| | | 34 | | /// </summary> |
| | | 35 | | public const int MaxTriangleCount = 131072; |
| | | 36 | | |
| | | 37 | | |
| | | 38 | | private readonly Vector3d[] _localVertices; |
| | | 39 | | |
| | | 40 | | private Vector3d[] _scaledLocalVertices; |
| | | 41 | | private Vector3d[] _preparedScaledLocalVertices; |
| | | 42 | | private int[]? _supportVertexIndices; |
| | | 43 | | private SupportTreeNode[]? _supportTreeNodes; |
| | | 44 | | private SupportTreeNode[]? _preparedSupportTreeNodes; |
| | | 45 | | private int _supportTreeNodeCount; |
| | | 46 | | |
| | | 47 | | /// <summary> |
| | | 48 | | /// Holds the source vertices in local mesh space. |
| | | 49 | | /// </summary> |
| | 55051 | 50 | | internal ReadOnlySpan<Vector3d> LocalVertices => _localVertices; |
| | | 51 | | |
| | | 52 | | /// <summary> |
| | | 53 | | /// Holds the committed scaled vertices as center-relative local |
| | | 54 | | /// offsets. Collision relations apply <see cref="Rotation"/> without |
| | | 55 | | /// materializing absolute world points. |
| | | 56 | | /// </summary> |
| | 470 | 57 | | internal ReadOnlySpan<Vector3d> ScaledLocalVertices => _scaledLocalVertices; |
| | | 58 | | |
| | | 59 | | /// <summary> |
| | | 60 | | /// Number of vertices in the immutable local mesh topology. |
| | | 61 | | /// </summary> |
| | 199039 | 62 | | public int VertexCount => _localVertices.Length; |
| | | 63 | | |
| | | 64 | | private readonly int[] _triangles; |
| | | 65 | | /// <summary> |
| | | 66 | | /// Holds all the triangles that make up the mesh in the form of indices to the vertices array. |
| | | 67 | | /// example: Triangles[0-3] = 4,2,3 means that the first triangle of the mesh is made up of the vertices _vertic |
| | | 68 | | /// </summary> |
| | 134710 | 69 | | public ReadOnlySpan<int> Triangles => _triangles; |
| | | 70 | | |
| | | 71 | | private readonly int _triangleCount; |
| | | 72 | | /// <summary>Gets the number of authored triangles.</summary> |
| | 56231 | 73 | | public int TriangleCount => _triangleCount; |
| | | 74 | | |
| | | 75 | | private readonly int[] _convexSatEdgeVertexPairs; |
| | 23534 | 76 | | internal ReadOnlySpan<int> ConvexSatEdgeVertexPairs => _convexSatEdgeVertexPairs; |
| | | 77 | | |
| | | 78 | | /// <summary>Gets the mesh collision mode.</summary> |
| | | 79 | | public MeshColliderMode Mode { get; } |
| | | 80 | | |
| | | 81 | | /// <summary> |
| | | 82 | | /// Gets whether the complete exact-position-welded triangle topology is one |
| | | 83 | | /// connected, consistently wound, closed two-manifold surface. |
| | | 84 | | /// </summary> |
| | | 85 | | public bool IsClosedSurface { get; private set; } |
| | | 86 | | |
| | | 87 | | /// <summary> |
| | | 88 | | /// Gets the current triangle surface area after local scale. |
| | | 89 | | /// </summary> |
| | 2851 | 90 | | public Fixed64 TotalArea => _scaledTotalArea; |
| | | 91 | | |
| | | 92 | | private int _triangleBvhBuildCount; |
| | | 93 | | private SwiftFixedBVH<int> _triangleBVH; |
| | | 94 | | private SwiftFixedBVH<int> _preparedTriangleBVH; |
| | | 95 | | |
| | | 96 | | /// <summary> |
| | | 97 | | /// Triangle acceleration structure in local mesh space. |
| | | 98 | | /// </summary> |
| | 182695 | 99 | | internal SwiftFixedBVH<int> TriangleBVH => _triangleBVH; |
| | | 100 | | |
| | | 101 | | /// <summary>Gets the number of committed triangle BVH builds.</summary> |
| | 35 | 102 | | public int TriangleBvhBuildCount => _triangleBvhBuildCount; |
| | | 103 | | |
| | 4352 | 104 | | private FixedQuaternion _rotation = FixedQuaternion.Identity; |
| | | 105 | | |
| | | 106 | | /// <summary> |
| | | 107 | | /// Gets the committed world-space origin of the mesh's rigid frame. |
| | | 108 | | /// </summary> |
| | 237589 | 109 | | internal Vector3d Origin => _geometryFrame.IsRigid ? _geometryFrame.RigidOrigin : _position; |
| | | 110 | | |
| | | 111 | | /// <summary> |
| | | 112 | | /// Gets the committed local-to-world mesh orientation. |
| | | 113 | | /// </summary> |
| | 300375 | 114 | | internal FixedQuaternion Rotation => _rotation; |
| | | 115 | | |
| | | 116 | | private FixedBoundBox _bounds; |
| | | 117 | | /// <summary>Gets the committed world-space axis-aligned bounds.</summary> |
| | 100803 | 118 | | public FixedBoundBox Bounds => _bounds; |
| | | 119 | | |
| | | 120 | | private readonly FixedBoundBox _localBounds; |
| | | 121 | | |
| | | 122 | | /// <summary> |
| | | 123 | | /// Axis-aligned bounds of the source vertices in local mesh space. |
| | | 124 | | /// </summary> |
| | 31718 | 125 | | public FixedBoundBox LocalBounds => _localBounds; |
| | | 126 | | |
| | | 127 | | /// <summary>Creates a convex runtime mesh at a world-space pose.</summary> |
| | | 128 | | public PhysicsMesh(Vector3d[] vertices, int[] triangles, Vector3d position, FixedQuaternion rotation) |
| | 117 | 129 | | : this(vertices, triangles, position, rotation, MeshColliderMode.Convex) { } |
| | | 130 | | |
| | | 131 | | /// <summary>Creates a runtime mesh with an explicit collision mode and world-space pose.</summary> |
| | | 132 | | public PhysicsMesh( |
| | | 133 | | Vector3d[] vertices, |
| | | 134 | | int[] triangles, |
| | | 135 | | Vector3d position, |
| | | 136 | | FixedQuaternion rotation, |
| | | 137 | | MeshColliderMode mode) |
| | 362 | 138 | | : this(vertices, triangles, mode, colliderOwned: false) |
| | | 139 | | { |
| | 342 | 140 | | PrepareTransformation(position, rotation, Vector3d.One, Vector3d.One, null); |
| | 327 | 141 | | PublishPreparedTransformation(); |
| | 327 | 142 | | } |
| | | 143 | | |
| | | 144 | | private readonly bool _colliderOwned; |
| | | 145 | | |
| | | 146 | | // Collider authoring cannot commit geometry until its owner and part scales are known. |
| | 4352 | 147 | | internal PhysicsMesh(Vector3d[] vertices, int[] triangles, MeshColliderMode mode, bool colliderOwned) |
| | | 148 | | { |
| | 4352 | 149 | | _colliderOwned = colliderOwned; |
| | 4352 | 150 | | ChronicleGuard.ThrowIfArgument( |
| | 4352 | 151 | | mode != MeshColliderMode.Convex && mode != MeshColliderMode.Concave, |
| | 4352 | 152 | | nameof(mode), |
| | 4352 | 153 | | "Unsupported mesh collider mode."); |
| | | 154 | | |
| | 4352 | 155 | | ValidateInput(vertices, triangles); |
| | | 156 | | |
| | 4341 | 157 | | Mode = mode; |
| | 4341 | 158 | | _localVertices = new Vector3d[vertices.Length]; |
| | 4341 | 159 | | Array.Copy(vertices, _localVertices, vertices.Length); |
| | 4341 | 160 | | _scaledLocalVertices = new Vector3d[vertices.Length]; |
| | 4341 | 161 | | _preparedScaledLocalVertices = new Vector3d[vertices.Length]; |
| | 4341 | 162 | | _triangles = new int[triangles.Length]; |
| | 4341 | 163 | | Array.Copy(triangles, _triangles, triangles.Length); |
| | 4341 | 164 | | _triangleCount = triangles.Length / 3; // 3 vertices per triangle |
| | 4341 | 165 | | _triangleBVH = new SwiftFixedBVH<int>(2 * TriangleCount - 1); |
| | 4341 | 166 | | _preparedTriangleBVH = new SwiftFixedBVH<int>(2 * TriangleCount - 1); |
| | 4341 | 167 | | _scaledFaceAreas = new Fixed64[TriangleCount]; |
| | 4341 | 168 | | _preparedScaledFaceAreas = new Fixed64[TriangleCount]; |
| | 4341 | 169 | | _scaledFaceNormals = new Vector3d[TriangleCount]; |
| | 4341 | 170 | | _preparedScaledFaceNormals = new Vector3d[TriangleCount]; |
| | | 171 | | |
| | 4341 | 172 | | _localBounds = CalculateBounds(_localVertices); |
| | | 173 | | |
| | | 174 | | // Positive admitted scales preserve authored incidence and planar fill. |
| | 4341 | 175 | | PrepareCoplanarPatches(_localVertices); |
| | 4341 | 176 | | int[] topologyTriangles = _weldedTriangleVertices; |
| | 4341 | 177 | | _convexSatEdgeVertexPairs = Mode == MeshColliderMode.Convex |
| | 4341 | 178 | | ? CreateConvexSatEdgeVertexPairs(_localVertices, topologyTriangles, _triangleCount) |
| | 4341 | 179 | | : Array.Empty<int>(); |
| | 4332 | 180 | | if (Mode == MeshColliderMode.Concave) |
| | | 181 | | { |
| | 2976 | 182 | | IsClosedSurface = EvaluateClosedVolumeTopology(topologyTriangles, out _surfaceClosureValidationResult); |
| | | 183 | | } |
| | | 184 | | |
| | 4332 | 185 | | ReleaseTopologyPreparation(); |
| | | 186 | | |
| | 4332 | 187 | | if (Mode == MeshColliderMode.Convex && _localVertices.Length > SupportTreeVertexThreshold) |
| | | 188 | | { |
| | 16 | 189 | | _supportVertexIndices = CreateSupportVertexIndices(_localVertices.Length); |
| | 16 | 190 | | _supportTreeNodes = new SupportTreeNode[(2 * _localVertices.Length) - 1]; |
| | 16 | 191 | | _preparedSupportTreeNodes = new SupportTreeNode[(2 * _localVertices.Length) - 1]; |
| | | 192 | | } |
| | | 193 | | |
| | 4332 | 194 | | } |
| | | 195 | | |
| | | 196 | | /// <summary>Updates the mesh world-space position and rigid rotation.</summary> |
| | | 197 | | /// <exception cref="InvalidOperationException">The mesh is owned by a collider.</exception> |
| | | 198 | | public void UpdatePosition(Vector3d position, FixedQuaternion rotation) |
| | | 199 | | { |
| | 33 | 200 | | ChronicleGuard.ThrowIfTrue(_colliderOwned, nameof(PhysicsMesh), |
| | 33 | 201 | | "Collider-owned mesh transformations must be changed through the collider or its host transform."); |
| | 32 | 202 | | PrepareTransformation(position, rotation, _ownerScale, _partScale, null); |
| | 32 | 203 | | PublishPreparedTransformation(); |
| | 32 | 204 | | } |
| | | 205 | | |
| | | 206 | | /// <summary> |
| | | 207 | | /// Updates the mesh center, normalized rigid rotation, and strictly positive authored scale. |
| | | 208 | | /// </summary> |
| | | 209 | | /// <exception cref="InvalidOperationException">The mesh is owned by a collider.</exception> |
| | | 210 | | public void UpdateTransform(Vector3d position, FixedQuaternion rotation, Vector3d scale) |
| | | 211 | | { |
| | 98 | 212 | | ChronicleGuard.ThrowIfTrue(_colliderOwned, nameof(PhysicsMesh), |
| | 98 | 213 | | "Collider-owned mesh transformations must be changed through the collider or its host transform."); |
| | 97 | 214 | | PrepareTransformation(position, rotation, scale, Vector3d.One, null); |
| | 83 | 215 | | PublishPreparedTransformation(); |
| | 83 | 216 | | } |
| | | 217 | | |
| | | 218 | | private static void ValidateInput(Vector3d[] vertices, int[] triangles) |
| | | 219 | | { |
| | 4352 | 220 | | ChronicleGuard.ThrowIfNull(vertices, nameof(vertices)); |
| | 4351 | 221 | | ChronicleGuard.ThrowIfNull(triangles, nameof(triangles)); |
| | 4350 | 222 | | ChronicleGuard.ThrowIfArgument(vertices.Length < 3, nameof(vertices), "Mesh must contain at least three vert |
| | 4350 | 223 | | ChronicleGuard.ThrowIfArgument(triangles.Length == 0 || triangles.Length % 3 != 0, nameof(triangles), "Trian |
| | 4348 | 224 | | ChronicleGuard.ThrowIfArgumentOutOfRange(vertices.Length > MaxVertexCount, vertices.Length, nameof(vertices) |
| | | 225 | | |
| | 4347 | 226 | | int triangleCount = triangles.Length / 3; |
| | 4347 | 227 | | ChronicleGuard.ThrowIfArgumentOutOfRange(triangleCount > MaxTriangleCount, triangleCount, nameof(triangles), |
| | | 228 | | |
| | 4347 | 229 | | var referencedVertices = new bool[vertices.Length]; |
| | 310476 | 230 | | for (int i = 0; i < triangleCount; i++) |
| | | 231 | | { |
| | 150895 | 232 | | int index0 = triangles[i * 3]; |
| | 150895 | 233 | | int index1 = triangles[i * 3 + 1]; |
| | 150895 | 234 | | int index2 = triangles[i * 3 + 2]; |
| | | 235 | | |
| | 150895 | 236 | | ChronicleGuard.ThrowIfArgumentOutOfRange((uint)index0 >= (uint)vertices.Length, vertices.Length, nameof( |
| | 150895 | 237 | | ChronicleGuard.ThrowIfArgumentOutOfRange((uint)index1 >= (uint)vertices.Length, vertices.Length, nameof( |
| | 150895 | 238 | | ChronicleGuard.ThrowIfArgumentOutOfRange((uint)index2 >= (uint)vertices.Length, vertices.Length, nameof( |
| | | 239 | | |
| | 150894 | 240 | | ChronicleGuard.ThrowIfArgument( |
| | 150894 | 241 | | index0 == index1 || index1 == index2 || index2 == index0, |
| | 150894 | 242 | | nameof(triangles), |
| | 150894 | 243 | | "Triangle indices must be unique within each triangle."); |
| | | 244 | | |
| | 150893 | 245 | | Fixed64 area = new FixedTriangle( |
| | 150893 | 246 | | vertices[index0], |
| | 150893 | 247 | | vertices[index1], |
| | 150893 | 248 | | vertices[index2]).Area; |
| | 150893 | 249 | | ChronicleGuard.ThrowIfArgument(area <= Fixed64.Epsilon, nameof(triangles), "Degenerate triangles are not |
| | | 250 | | |
| | 150891 | 251 | | referencedVertices[index0] = true; |
| | 150891 | 252 | | referencedVertices[index1] = true; |
| | 150891 | 253 | | referencedVertices[index2] = true; |
| | | 254 | | } |
| | | 255 | | |
| | 89438 | 256 | | for (int i = 0; i < referencedVertices.Length; i++) |
| | 40378 | 257 | | ChronicleGuard.ThrowIfArgument(!referencedVertices[i], nameof(vertices), "Every mesh vertex must be refe |
| | 4341 | 258 | | } |
| | | 259 | | |
| | | 260 | | private void BuildTriangleBVH( |
| | | 261 | | SwiftFixedBVH<int> bvh, |
| | | 262 | | ReadOnlySpan<Vector3d> vertices) |
| | | 263 | | { |
| | 4440 | 264 | | bvh.Clear(); |
| | 302224 | 265 | | for (int i = 0; i < _triangleCount; i++) |
| | | 266 | | { |
| | 146672 | 267 | | int index0 = _triangles[i * 3]; |
| | 146672 | 268 | | int index1 = _triangles[i * 3 + 1]; |
| | 146672 | 269 | | int index2 = _triangles[i * 3 + 2]; |
| | 146672 | 270 | | Vector3d min = Vector3d.Min(Vector3d.Min(vertices[index0], vertices[index1]), vertices[index2]); |
| | 146672 | 271 | | Vector3d max = Vector3d.Max(Vector3d.Max(vertices[index0], vertices[index1]), vertices[index2]); |
| | 146672 | 272 | | bvh.Insert(i, new FixedBoundVolume(min, max)); |
| | | 273 | | } |
| | 4440 | 274 | | } |
| | | 275 | | |
| | | 276 | | private static FixedBoundBox CalculateBounds(Vector3d[] vertices) |
| | | 277 | | { |
| | 8785 | 278 | | Vector3d min = vertices[0]; |
| | 8785 | 279 | | Vector3d max = vertices[0]; |
| | 159312 | 280 | | for (int i = 1; i < vertices.Length; i++) |
| | | 281 | | { |
| | 70871 | 282 | | min = Vector3d.Min(min, vertices[i]); |
| | 70871 | 283 | | max = Vector3d.Max(max, vertices[i]); |
| | | 284 | | } |
| | | 285 | | |
| | 8785 | 286 | | return FixedBoundBox.FromMinMax(min, max); |
| | | 287 | | } |
| | | 288 | | |
| | | 289 | | /// <summary>Compares deterministic edge uses for topology validation.</summary> |
| | | 290 | | private static int CompareEdgeUses(EdgeUse first, EdgeUse second) |
| | | 291 | | { |
| | 9259310 | 292 | | if (first.Key != second.Key) |
| | 3685715 | 293 | | return first.Key < second.Key ? -1 : 1; |
| | | 294 | | |
| | 5573595 | 295 | | if (first.TriangleIndex == second.TriangleIndex) |
| | 1246 | 296 | | return 0; |
| | | 297 | | |
| | 5572349 | 298 | | return first.TriangleIndex < second.TriangleIndex ? -1 : 1; |
| | | 299 | | } |
| | | 300 | | |
| | | 301 | | /// <summary>Gets the projected mesh area facing a world-space direction.</summary> |
| | | 302 | | public Fixed64 GetFrontalArea(Vector3d direction) |
| | | 303 | | { |
| | 11 | 304 | | Fixed64 directionMagnitude = direction.Magnitude; |
| | 11 | 305 | | if (directionMagnitude <= Fixed64.Epsilon) |
| | 1 | 306 | | return Fixed64.Zero; |
| | | 307 | | |
| | 10 | 308 | | Vector3d normalizedDirection = direction / directionMagnitude; |
| | 10 | 309 | | Fixed64 totalArea = Fixed64.Zero; |
| | 56 | 310 | | for (int i = 0; i < _triangleCount; i++) |
| | | 311 | | { |
| | 18 | 312 | | Fixed64 projection = Vector3d.Dot(GetFaceNormalWorld(i), normalizedDirection); |
| | 18 | 313 | | if (projection > Fixed64.Zero) |
| | 11 | 314 | | totalArea += _scaledFaceAreas[i] * FixedMath.Min(projection, Fixed64.One); |
| | | 315 | | } |
| | | 316 | | |
| | 10 | 317 | | return totalArea; |
| | | 318 | | } |
| | | 319 | | |
| | | 320 | | /// <summary>Gets one triangle's vertices in world space.</summary> |
| | | 321 | | public void GetTriangleVertices(int index, out Vector3d first, out Vector3d second, out Vector3d third) |
| | | 322 | | { |
| | 5 | 323 | | if (TryGetTriangleVertices(index, out first, out second, out third)) |
| | 4 | 324 | | return; |
| | | 325 | | |
| | 1 | 326 | | throw new InvalidOperationException( |
| | 1 | 327 | | "At least one triangle vertex lies outside the Fixed64 world-coordinate domain."); |
| | | 328 | | } |
| | | 329 | | |
| | | 330 | | /// <summary> |
| | | 331 | | /// Attempts to materialize one triangle's absolute world vertices. |
| | | 332 | | /// Canonical collision and query paths should consume scaled-local |
| | | 333 | | /// geometry instead. |
| | | 334 | | /// </summary> |
| | | 335 | | public bool TryGetTriangleVertices( |
| | | 336 | | int index, |
| | | 337 | | out Vector3d first, |
| | | 338 | | out Vector3d second, |
| | | 339 | | out Vector3d third) |
| | | 340 | | { |
| | 310 | 341 | | GetGeometryTriangleVertices( |
| | 310 | 342 | | index, |
| | 310 | 343 | | out Vector3d localFirst, |
| | 310 | 344 | | out Vector3d localSecond, |
| | 310 | 345 | | out Vector3d localThird); |
| | 310 | 346 | | bool representable = |
| | 310 | 347 | | _geometryFrame.TryGetPoint(localFirst, out first) |
| | 310 | 348 | | & _geometryFrame.TryGetPoint(localSecond, out second) |
| | 310 | 349 | | & _geometryFrame.TryGetPoint(localThird, out third); |
| | 310 | 350 | | if (representable) |
| | 304 | 351 | | return true; |
| | | 352 | | |
| | 6 | 353 | | first = default; |
| | 6 | 354 | | second = default; |
| | 6 | 355 | | third = default; |
| | 6 | 356 | | return false; |
| | | 357 | | } |
| | | 358 | | |
| | | 359 | | /// <summary> |
| | | 360 | | /// Gets one triangle's committed scaled vertices in local mesh space. |
| | | 361 | | /// </summary> |
| | | 362 | | public void GetLocalTriangleVertices(int index, out Vector3d first, out Vector3d second, out Vector3d third) |
| | | 363 | | { |
| | 71 | 364 | | ChronicleGuard.ThrowIfArrayIndexInvalid(index, _triangleCount, nameof(index)); |
| | | 365 | | |
| | 71 | 366 | | int triangleIndex = index * 3; |
| | 71 | 367 | | first = _scaledLocalVertices[_triangles[triangleIndex]]; |
| | 71 | 368 | | second = _scaledLocalVertices[_triangles[triangleIndex + 1]]; |
| | 71 | 369 | | third = _scaledLocalVertices[_triangles[triangleIndex + 2]]; |
| | 71 | 370 | | } |
| | | 371 | | |
| | | 372 | | /// <summary> |
| | | 373 | | /// Gets one triangle's committed scaled normal in local mesh space. |
| | | 374 | | /// </summary> |
| | | 375 | | internal Vector3d GetScaledLocalFaceNormal(int index) |
| | | 376 | | { |
| | 7357 | 377 | | ChronicleGuard.ThrowIfArrayIndexInvalid(index, _triangleCount, nameof(index)); |
| | 7357 | 378 | | return _scaledFaceNormals[index]; |
| | | 379 | | } |
| | | 380 | | |
| | | 381 | | /// <summary> |
| | | 382 | | /// Attempts to materialize one scaled vertex in world space. |
| | | 383 | | /// </summary> |
| | | 384 | | public bool TryGetVertexWorld(int index, out Vector3d vertex) |
| | | 385 | | { |
| | 1940 | 386 | | ChronicleGuard.ThrowIfArrayIndexInvalid(index, _localVertices.Length, nameof(index)); |
| | 1940 | 387 | | return _sourceFrame.TryGetPoint(_localVertices[index], 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 | | { |
| | 1918 | 399 | | if (TryGetVertexWorld(index, out Vector3d vertex)) |
| | 1917 | 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 | | { |
| | 56 | 415 | | Vector3d localPoint = GetSupportVertexLocal(direction); |
| | 56 | 416 | | return _geometryFrame.TryGetPoint(localPoint, 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 | | { |
| | 55 | 430 | | if (TryGetSupportVertexWorld(direction, out Vector3d vertex)) |
| | 54 | 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 | | { |
| | | 439 | | SupportDirection localDirection; |
| | 8776 | 440 | | if (!_geometryFrame.IsRigid) |
| | | 441 | | { |
| | 296 | 442 | | WideAxis3 covector = _geometryFrame.GetSourceDirectionNumerators(direction); |
| | 298 | 443 | | if (covector.IsZero) covector = new WideAxis3(Signed320.ExtendValue(Signed192.Signed(1)), default, defau |
| | 296 | 444 | | localDirection = new SupportDirection(covector); |
| | | 445 | | } |
| | | 446 | | else |
| | | 447 | | { |
| | 8480 | 448 | | Vector3d compact = ConvertWorldDirectionToLocal(direction); |
| | 8480 | 449 | | localDirection = new SupportDirection(compact != Vector3d.Zero ? compact.Normalized : Vector3d.Right); |
| | | 450 | | } |
| | 8776 | 451 | | if (_supportTreeNodes != null && _supportVertexIndices != null) |
| | 41 | 452 | | return GeometryVertices[FindSupportVertexIndex(localDirection)]; |
| | | 453 | | |
| | 8735 | 454 | | int bestIndex = 0; |
| | 128932 | 455 | | for (int index = 1; index < VertexCount; index++) |
| | 62562 | 456 | | if (localDirection.Compare(GeometryVertices[index], GeometryVertices[bestIndex]) > 0) bestIndex = index; |
| | 8735 | 457 | | return GeometryVertices[bestIndex]; |
| | | 458 | | } |
| | | 459 | | |
| | | 460 | | // Exact affine support changes the covector, not the source-space index. |
| | | 461 | | // Compact calls retain their existing projection and tie behavior. |
| | | 462 | | private readonly struct SupportDirection |
| | | 463 | | { |
| | | 464 | | private readonly Vector3d _compact; |
| | | 465 | | private readonly WideAxis3 _exact; |
| | | 466 | | private readonly bool _wide; |
| | 33920 | 467 | | internal SupportDirection(Vector3d compact) { _compact = compact; _exact = default; _wide = false; } |
| | 1184 | 468 | | internal SupportDirection(WideAxis3 exact) { _compact = default; _exact = exact; _wide = true; } |
| | 56955 | 469 | | internal int Compare(Vector3d first, Vector3d second) => _wide |
| | 56955 | 470 | | ? WideArithmetic.SubtractSigned576(Project(first), Project(second)).Sign |
| | 56955 | 471 | | : Vector3d.CompareProjection(first, second, _compact); |
| | 1070 | 472 | | internal Vector3d BoundsPoint(Vector3d min, Vector3d max) => _wide |
| | 1070 | 473 | | ? new Vector3d(_exact.X.Sign >= 0 ? max.X : min.X, _exact.Y.Sign >= 0 ? max.Y : min.Y, _exact.Z.Sign >= |
| | 1070 | 474 | | : GetBoundsSupportPoint(min, max, _compact); |
| | 4482 | 475 | | private Signed576 Project(Vector3d point) => WideArithmetic.AddSigned576(WideArithmetic.AddSigned576( |
| | 4482 | 476 | | WideArithmetic.MultiplySigned320(_exact.X, Signed192.Raw(point.X)), |
| | 4482 | 477 | | WideArithmetic.MultiplySigned320(_exact.Y, Signed192.Raw(point.Y))), |
| | 4482 | 478 | | WideArithmetic.MultiplySigned320(_exact.Z, Signed192.Raw(point.Z))); |
| | | 479 | | } |
| | | 480 | | |
| | | 481 | | private int FindSupportVertexIndex(in SupportDirection localDirection) |
| | | 482 | | { |
| | 41 | 483 | | int bestIndex = 0; |
| | 41 | 484 | | Span<int> stack = stackalloc int[SupportTreeStackCapacity]; |
| | 41 | 485 | | int stackCount = 0; |
| | 41 | 486 | | stack[stackCount++] = 0; |
| | | 487 | | |
| | 387 | 488 | | while (stackCount > 0) |
| | | 489 | | { |
| | 346 | 490 | | int nodeIndex = stack[--stackCount]; |
| | 346 | 491 | | SupportTreeNode node = _supportTreeNodes![nodeIndex]; |
| | 346 | 492 | | Vector3d upperPoint = localDirection.BoundsPoint(node.Min, node.Max); |
| | 346 | 493 | | int upperComparison = localDirection.Compare(upperPoint, GeometryVertices[bestIndex]); |
| | 346 | 494 | | if (upperComparison < 0 || (upperComparison == 0 && node.MinVertexIndex >= bestIndex)) |
| | | 495 | | continue; |
| | | 496 | | |
| | 241 | 497 | | if (node.IsLeaf) |
| | | 498 | | { |
| | 60 | 499 | | SearchSupportLeaf(node, localDirection, ref bestIndex); |
| | 60 | 500 | | continue; |
| | | 501 | | } |
| | | 502 | | |
| | 181 | 503 | | SupportTreeNode left = _supportTreeNodes[node.Left]; |
| | 181 | 504 | | SupportTreeNode right = _supportTreeNodes[node.Right]; |
| | | 505 | | |
| | 181 | 506 | | if (ComesBeforeSupportNode(left, right, localDirection)) |
| | | 507 | | { |
| | 70 | 508 | | PushSupportNode(right, bestIndex, localDirection, stack, ref stackCount); |
| | 70 | 509 | | PushSupportNode(left, bestIndex, localDirection, stack, ref stackCount); |
| | 70 | 510 | | continue; |
| | | 511 | | } |
| | | 512 | | |
| | 111 | 513 | | PushSupportNode(left, bestIndex, localDirection, stack, ref stackCount); |
| | 111 | 514 | | PushSupportNode(right, bestIndex, localDirection, stack, ref stackCount); |
| | | 515 | | } |
| | | 516 | | |
| | 41 | 517 | | return bestIndex; |
| | | 518 | | } |
| | | 519 | | |
| | | 520 | | private void SearchSupportLeaf( |
| | | 521 | | SupportTreeNode node, |
| | | 522 | | in SupportDirection localDirection, |
| | | 523 | | ref int bestIndex) |
| | | 524 | | { |
| | 790 | 525 | | for (int i = 0; i < node.Count; i++) |
| | | 526 | | { |
| | 335 | 527 | | int vertexIndex = _supportVertexIndices![node.Start + i]; |
| | 335 | 528 | | Vector3d vertex = GeometryVertices[vertexIndex]; |
| | 335 | 529 | | int projectionComparison = localDirection.Compare(vertex, GeometryVertices[bestIndex]); |
| | 335 | 530 | | if (projectionComparison < 0 || (projectionComparison == 0 && vertexIndex >= bestIndex)) |
| | | 531 | | continue; |
| | | 532 | | |
| | 89 | 533 | | bestIndex = vertexIndex; |
| | | 534 | | } |
| | 60 | 535 | | } |
| | | 536 | | |
| | | 537 | | private void PushSupportNode( |
| | | 538 | | SupportTreeNode node, |
| | | 539 | | int bestIndex, |
| | | 540 | | in SupportDirection localDirection, |
| | | 541 | | Span<int> stack, |
| | | 542 | | ref int stackCount) |
| | | 543 | | { |
| | 362 | 544 | | Vector3d upperPoint = localDirection.BoundsPoint(node.Min, node.Max); |
| | 362 | 545 | | int upperComparison = localDirection.Compare(upperPoint, GeometryVertices[bestIndex]); |
| | 362 | 546 | | if (upperComparison < 0 || (upperComparison == 0 && node.MinVertexIndex >= bestIndex)) |
| | 57 | 547 | | return; |
| | | 548 | | |
| | 305 | 549 | | stack[stackCount++] = node.Index; |
| | 305 | 550 | | } |
| | | 551 | | |
| | | 552 | | private static bool ComesBeforeSupportNode( |
| | | 553 | | SupportTreeNode left, |
| | | 554 | | SupportTreeNode right, |
| | | 555 | | in SupportDirection localDirection) |
| | | 556 | | { |
| | 181 | 557 | | Vector3d leftPoint = localDirection.BoundsPoint(left.Min, left.Max); |
| | 181 | 558 | | Vector3d rightPoint = localDirection.BoundsPoint(right.Min, right.Max); |
| | 181 | 559 | | int projectionComparison = localDirection.Compare(leftPoint, rightPoint); |
| | 181 | 560 | | if (projectionComparison != 0) |
| | 117 | 561 | | return projectionComparison > 0; |
| | | 562 | | |
| | 64 | 563 | | return left.MinVertexIndex < right.MinVertexIndex; |
| | | 564 | | } |
| | | 565 | | |
| | | 566 | | private int BuildSupportTreeNode( |
| | | 567 | | Vector3d[] vertices, |
| | | 568 | | int[] vertexIndices, |
| | | 569 | | SupportTreeNode[] nodes, |
| | | 570 | | SupportVertexIndexComparer comparer, |
| | | 571 | | ref int nodeCount, |
| | | 572 | | int start, |
| | | 573 | | int count) |
| | | 574 | | { |
| | 2757 | 575 | | int nodeIndex = nodeCount++; |
| | | 576 | | |
| | 2757 | 577 | | CalculateSupportRangeBounds( |
| | 2757 | 578 | | vertices, |
| | 2757 | 579 | | vertexIndices, |
| | 2757 | 580 | | start, |
| | 2757 | 581 | | count, |
| | 2757 | 582 | | out Vector3d min, |
| | 2757 | 583 | | out Vector3d max, |
| | 2757 | 584 | | out int minVertexIndex); |
| | 2757 | 585 | | if (count <= SupportTreeLeafVertexCount) |
| | | 586 | | { |
| | 1386 | 587 | | nodes[nodeIndex] = SupportTreeNode.CreateLeaf( |
| | 1386 | 588 | | nodeIndex, |
| | 1386 | 589 | | min, |
| | 1386 | 590 | | max, |
| | 1386 | 591 | | start, |
| | 1386 | 592 | | count, |
| | 1386 | 593 | | minVertexIndex); |
| | 1386 | 594 | | return nodeIndex; |
| | | 595 | | } |
| | | 596 | | |
| | 1371 | 597 | | int axis = GetDominantAxis(max - min); |
| | 1371 | 598 | | comparer.Reset(vertices, axis); |
| | 1371 | 599 | | Array.Sort( |
| | 1371 | 600 | | vertexIndices, |
| | 1371 | 601 | | start, |
| | 1371 | 602 | | count, |
| | 1371 | 603 | | comparer); |
| | | 604 | | |
| | 1371 | 605 | | int leftCount = count / 2; |
| | 1371 | 606 | | int rightCount = count - leftCount; |
| | 1371 | 607 | | int leftIndex = BuildSupportTreeNode( |
| | 1371 | 608 | | vertices, |
| | 1371 | 609 | | vertexIndices, |
| | 1371 | 610 | | nodes, |
| | 1371 | 611 | | comparer, |
| | 1371 | 612 | | ref nodeCount, |
| | 1371 | 613 | | start, |
| | 1371 | 614 | | leftCount); |
| | 1371 | 615 | | int rightIndex = BuildSupportTreeNode( |
| | 1371 | 616 | | vertices, |
| | 1371 | 617 | | vertexIndices, |
| | 1371 | 618 | | nodes, |
| | 1371 | 619 | | comparer, |
| | 1371 | 620 | | ref nodeCount, |
| | 1371 | 621 | | start + leftCount, |
| | 1371 | 622 | | rightCount); |
| | 1371 | 623 | | nodes[nodeIndex] = SupportTreeNode.CreateBranch( |
| | 1371 | 624 | | nodeIndex, |
| | 1371 | 625 | | min, |
| | 1371 | 626 | | max, |
| | 1371 | 627 | | leftIndex, |
| | 1371 | 628 | | rightIndex, |
| | 1371 | 629 | | minVertexIndex); |
| | 1371 | 630 | | return nodeIndex; |
| | | 631 | | } |
| | | 632 | | |
| | | 633 | | private void RefitSupportTree( |
| | | 634 | | Vector3d[] vertices, |
| | | 635 | | SupportTreeNode[] sourceNodes, |
| | | 636 | | SupportTreeNode[] targetNodes) |
| | | 637 | | { |
| | | 638 | | // Children follow their parent in the immutable preorder topology, |
| | | 639 | | // so reverse traversal makes both child bounds available first. |
| | 2112 | 640 | | for (int i = _supportTreeNodeCount - 1; i >= 0; i--) |
| | | 641 | | { |
| | 1046 | 642 | | SupportTreeNode source = sourceNodes[i]; |
| | 1046 | 643 | | if (source.IsLeaf) |
| | | 644 | | { |
| | 528 | 645 | | CalculateSupportRangeBounds( |
| | 528 | 646 | | vertices, |
| | 528 | 647 | | _supportVertexIndices!, |
| | 528 | 648 | | source.Start, |
| | 528 | 649 | | source.Count, |
| | 528 | 650 | | out Vector3d min, |
| | 528 | 651 | | out Vector3d max, |
| | 528 | 652 | | out int minVertexIndex); |
| | 528 | 653 | | targetNodes[i] = SupportTreeNode.CreateLeaf( |
| | 528 | 654 | | source.Index, |
| | 528 | 655 | | min, |
| | 528 | 656 | | max, |
| | 528 | 657 | | source.Start, |
| | 528 | 658 | | source.Count, |
| | 528 | 659 | | minVertexIndex); |
| | 528 | 660 | | continue; |
| | | 661 | | } |
| | | 662 | | |
| | 518 | 663 | | SupportTreeNode left = targetNodes[source.Left]; |
| | 518 | 664 | | SupportTreeNode right = targetNodes[source.Right]; |
| | 518 | 665 | | targetNodes[i] = SupportTreeNode.CreateBranch( |
| | 518 | 666 | | source.Index, |
| | 518 | 667 | | Vector3d.Min(left.Min, right.Min), |
| | 518 | 668 | | Vector3d.Max(left.Max, right.Max), |
| | 518 | 669 | | source.Left, |
| | 518 | 670 | | source.Right, |
| | 518 | 671 | | source.MinVertexIndex); |
| | | 672 | | } |
| | 10 | 673 | | } |
| | | 674 | | |
| | | 675 | | private static void CalculateSupportRangeBounds( |
| | | 676 | | Vector3d[] vertices, |
| | | 677 | | int[] vertexIndices, |
| | | 678 | | int start, |
| | | 679 | | int count, |
| | | 680 | | out Vector3d min, |
| | | 681 | | out Vector3d max, |
| | | 682 | | out int minVertexIndex) |
| | | 683 | | { |
| | 3285 | 684 | | int firstIndex = vertexIndices[start]; |
| | 3285 | 685 | | minVertexIndex = firstIndex; |
| | 3285 | 686 | | min = vertices[firstIndex]; |
| | 3285 | 687 | | max = min; |
| | 169212 | 688 | | for (int i = 1; i < count; i++) |
| | | 689 | | { |
| | 81321 | 690 | | int vertexIndex = vertexIndices[start + i]; |
| | 81321 | 691 | | Vector3d vertex = vertices[vertexIndex]; |
| | 81321 | 692 | | min = Vector3d.Min(min, vertex); |
| | 81321 | 693 | | max = Vector3d.Max(max, vertex); |
| | 81321 | 694 | | if (vertexIndex < minVertexIndex) |
| | 1566 | 695 | | minVertexIndex = vertexIndex; |
| | | 696 | | } |
| | 3285 | 697 | | } |
| | | 698 | | |
| | | 699 | | private static int[] CreateSupportVertexIndices(int vertexCount) |
| | | 700 | | { |
| | 16 | 701 | | var indices = new int[vertexCount]; |
| | 28948 | 702 | | for (int i = 0; i < indices.Length; i++) |
| | 14458 | 703 | | indices[i] = i; |
| | | 704 | | |
| | 16 | 705 | | return indices; |
| | | 706 | | } |
| | | 707 | | |
| | | 708 | | private static int GetDominantAxis(Vector3d extents) |
| | | 709 | | { |
| | 1371 | 710 | | if (extents.X >= extents.Y && extents.X >= extents.Z) |
| | 431 | 711 | | return 0; |
| | | 712 | | |
| | 940 | 713 | | return extents.Y >= extents.Z ? 1 : 2; |
| | | 714 | | } |
| | | 715 | | |
| | | 716 | | private static Vector3d GetBoundsSupportPoint(Vector3d min, Vector3d max, Vector3d direction) => |
| | 721 | 717 | | new( |
| | 721 | 718 | | direction.X >= Fixed64.Zero ? max.X : min.X, |
| | 721 | 719 | | direction.Y >= Fixed64.Zero ? max.Y : min.Y, |
| | 721 | 720 | | direction.Z >= Fixed64.Zero ? max.Z : min.Z); |
| | | 721 | | |
| | | 722 | | /// <summary>Gets a triangle face normal in world space.</summary> |
| | | 723 | | public Vector3d GetFaceNormalWorld(int index) |
| | | 724 | | { |
| | 5441 | 725 | | ChronicleGuard.ThrowIfArrayIndexInvalid(index, _triangleCount, nameof(index)); |
| | 5441 | 726 | | if (!_scaleIsExact) |
| | | 727 | | { |
| | 213 | 728 | | GetGeometryTriangleVertices(index, out Vector3d a, out Vector3d b, out Vector3d c); |
| | 213 | 729 | | return WideTriangleRelations.GetScaledTriangleLocalNormal(new FixedTriangle(a, b, c), _sourceFrame) |
| | 213 | 730 | | .Transform(_sourceFrame.GeometryBasis).GetNormal(false); |
| | | 731 | | } |
| | 5228 | 732 | | _ = _rotation.TryRotate(_scaledFaceNormals[index], out Vector3d worldNormal); |
| | 5228 | 733 | | return worldNormal.Normalized; |
| | | 734 | | } |
| | | 735 | | |
| | | 736 | | /// <summary>Writes triangle indices overlapping world-space bounds into a caller-owned buffer.</summary> |
| | | 737 | | public void GetTrianglesInWorldBounds(FixedBoundVolume worldBounds, SwiftList<int> result) |
| | | 738 | | { |
| | 169474 | 739 | | result.FastClear(); |
| | 169474 | 740 | | if (!_geometryFrame.IsRigid) |
| | | 741 | | { |
| | | 742 | | // Collider admission requires positive scale; inverse bounds |
| | | 743 | | // clip to the source domain and cannot be singular here. |
| | 1790 | 744 | | bool inverted = _sourceFrame.TryGetSourceBounds(worldBounds.Min, worldBounds.Max, out FixedBoundBox sour |
| | | 745 | | System.Diagnostics.Debug.Assert(inverted); |
| | 1790 | 746 | | TriangleBVH.Query(new FixedBoundVolume(sourceBounds.Min, sourceBounds.Max), result); |
| | 1790 | 747 | | return; |
| | | 748 | | } |
| | 167684 | 749 | | FixedBoundBox localBounds = |
| | 167684 | 750 | | FixedBoundBox.FromRelativeRotatedBoundsClippedToDomain( |
| | 167684 | 751 | | Vector3d.Zero, |
| | 167684 | 752 | | FixedQuaternion.Identity, |
| | 167684 | 753 | | worldBounds.Min, |
| | 167684 | 754 | | worldBounds.Max, |
| | 167684 | 755 | | _geometryFrame.RigidOrigin, |
| | 167684 | 756 | | _rotation); |
| | 167684 | 757 | | TriangleBVH.Query( |
| | 167684 | 758 | | new FixedBoundVolume(localBounds.Min, localBounds.Max), |
| | 167684 | 759 | | result); |
| | 167684 | 760 | | } |
| | | 761 | | |
| | | 762 | | /// <summary>Writes triangle indices overlapping scaled-local bounds into a caller-owned buffer.</summary> |
| | | 763 | | public void GetTrianglesInLocalBounds(FixedBoundVolume localBounds, SwiftList<int> result) |
| | | 764 | | { |
| | 9 | 765 | | result.FastClear(); |
| | 9 | 766 | | if (_geometryUsesSource) |
| | | 767 | | { |
| | 3 | 768 | | bool inverted = _sourceFrame.TryGetSourceBoundsFromScaledLocal(localBounds.Min, localBounds.Max, out Fix |
| | | 769 | | System.Diagnostics.Debug.Assert(inverted); |
| | 3 | 770 | | TriangleBVH.Query(new FixedBoundVolume(source.Min, source.Max), result); |
| | | 771 | | } |
| | 6 | 772 | | else TriangleBVH.Query(localBounds, result); |
| | 6 | 773 | | } |
| | | 774 | | |
| | | 775 | | /// <summary> |
| | | 776 | | /// Attempts to express a world point in the committed scaled-local |
| | | 777 | | /// mesh frame without saturating the relative displacement. |
| | | 778 | | /// </summary> |
| | | 779 | | public bool TryConvertWorldToScaledLocal( |
| | | 780 | | Vector3d worldPoint, |
| | | 781 | | out Vector3d localPoint) => |
| | 3 | 782 | | _sourceFrame.TryGetScaledLocalPointIn(worldPoint, out localPoint); |
| | | 783 | | |
| | | 784 | | /// <summary> |
| | | 785 | | /// Expresses a world point in the committed scaled-local mesh frame. |
| | | 786 | | /// </summary> |
| | | 787 | | /// <exception cref="InvalidOperationException"> |
| | | 788 | | /// The exact relative displacement lies outside the representable |
| | | 789 | | /// coordinate domain. |
| | | 790 | | /// </exception> |
| | | 791 | | public Vector3d ConvertWorldToScaledLocal(Vector3d worldPoint) |
| | | 792 | | { |
| | 2 | 793 | | if (TryConvertWorldToScaledLocal(worldPoint, out Vector3d localPoint)) |
| | 1 | 794 | | return localPoint; |
| | | 795 | | |
| | 1 | 796 | | throw new InvalidOperationException( |
| | 1 | 797 | | "The world point cannot be represented in the mesh's scaled-local frame."); |
| | | 798 | | } |
| | | 799 | | |
| | | 800 | | /// <summary> |
| | | 801 | | /// Attempts to materialize a scaled-local mesh point in world space. |
| | | 802 | | /// </summary> |
| | | 803 | | public bool TryConvertScaledLocalToWorld( |
| | | 804 | | Vector3d scaledLocalPoint, |
| | | 805 | | out Vector3d worldPoint) => |
| | 4 | 806 | | _sourceFrame.GetScaledLocalPoint(scaledLocalPoint).TryGetPoint(out worldPoint); |
| | | 807 | | |
| | | 808 | | /// <summary> |
| | | 809 | | /// Materializes a scaled-local mesh point in world space. |
| | | 810 | | /// </summary> |
| | | 811 | | /// <exception cref="InvalidOperationException"> |
| | | 812 | | /// The conceptual world point lies outside the representable |
| | | 813 | | /// coordinate domain. |
| | | 814 | | /// </exception> |
| | | 815 | | public Vector3d ConvertScaledLocalToWorld(Vector3d scaledLocalPoint) |
| | | 816 | | { |
| | 3 | 817 | | if (TryConvertScaledLocalToWorld( |
| | 3 | 818 | | scaledLocalPoint, |
| | 3 | 819 | | out Vector3d worldPoint)) |
| | | 820 | | { |
| | 2 | 821 | | return worldPoint; |
| | | 822 | | } |
| | | 823 | | |
| | 1 | 824 | | throw new InvalidOperationException( |
| | 1 | 825 | | "The scaled-local point lies outside the Fixed64 world-coordinate domain."); |
| | | 826 | | } |
| | | 827 | | |
| | | 828 | | internal FixedPointAnchor CreatePointAnchor( |
| | | 829 | | Vector3d geometryPoint) |
| | | 830 | | { |
| | 35157 | 831 | | if (_geometryFrame.IsRigid) |
| | 34594 | 832 | | return FixedPointAnchor.FromValidatedFrame(_geometryFrame.RigidOrigin, _rotation, geometryPoint); |
| | | 833 | | |
| | | 834 | | // Production callers supply an admitted vertex or finite convex |
| | | 835 | | // witness: its offset from the mesh center is bounded by reach. |
| | 563 | 836 | | bool represented = _geometryFrame.GetPoint(geometryPoint).TryGetLocalPointIn(_position, |
| | 563 | 837 | | FixedQuaternion.Identity, out Vector3d offset); |
| | | 838 | | System.Diagnostics.Debug.Assert(represented); |
| | 563 | 839 | | return FixedPointAnchor.FromValidatedFrame(_position, FixedQuaternion.Identity, offset); |
| | | 840 | | } |
| | | 841 | | |
| | | 842 | | /// <summary> |
| | | 843 | | /// Attempts to express a world-space direction in the committed |
| | | 844 | | /// scaled-local mesh frame. |
| | | 845 | | /// </summary> |
| | | 846 | | public bool TryConvertWorldDirectionToLocal( |
| | | 847 | | Vector3d worldDirection, |
| | | 848 | | out Vector3d localDirection) => |
| | 8482 | 849 | | _rotation.Inverse().TryRotate( |
| | 8482 | 850 | | worldDirection, |
| | 8482 | 851 | | out localDirection); |
| | | 852 | | |
| | | 853 | | /// <summary> |
| | | 854 | | /// Expresses a world-space direction in the committed scaled-local |
| | | 855 | | /// mesh frame. |
| | | 856 | | /// </summary> |
| | | 857 | | public Vector3d ConvertWorldDirectionToLocal( |
| | | 858 | | Vector3d worldDirection) |
| | | 859 | | { |
| | 8481 | 860 | | if (TryConvertWorldDirectionToLocal( |
| | 8481 | 861 | | worldDirection, |
| | 8481 | 862 | | out Vector3d localDirection)) |
| | | 863 | | { |
| | 8480 | 864 | | return localDirection; |
| | | 865 | | } |
| | | 866 | | |
| | 1 | 867 | | throw new InvalidOperationException( |
| | 1 | 868 | | "The world direction cannot be represented in the mesh's scaled-local frame."); |
| | | 869 | | } |
| | | 870 | | |
| | | 871 | | private readonly struct EdgeUse |
| | | 872 | | { |
| | | 873 | | private EdgeUse(long key, int direction, int triangleIndex) |
| | | 874 | | { |
| | 944504 | 875 | | Key = key; |
| | 944504 | 876 | | Direction = direction; |
| | 944504 | 877 | | TriangleIndex = triangleIndex; |
| | 944504 | 878 | | } |
| | | 879 | | |
| | | 880 | | public long Key { get; } |
| | | 881 | | |
| | 624395 | 882 | | public int StartVertexIndex => (int)(Key >> 32); |
| | | 883 | | |
| | | 884 | | // Mask explicitly: ARM64 Tier0 can retain upper key bits in a narrowed getter return. |
| | 624395 | 885 | | public int EndVertexIndex => (int)(Key & uint.MaxValue); |
| | | 886 | | |
| | | 887 | | public int Direction { get; } |
| | | 888 | | |
| | | 889 | | public int TriangleIndex { get; } |
| | | 890 | | |
| | | 891 | | public static EdgeUse Create(int start, int end, int triangleIndex) |
| | | 892 | | { |
| | 944504 | 893 | | int min = start < end ? start : end; |
| | 944504 | 894 | | int max = start < end ? end : start; |
| | 944504 | 895 | | long key = ((long)min << 32) | (uint)max; |
| | 944504 | 896 | | int direction = start == min ? 1 : -1; |
| | 944504 | 897 | | return new EdgeUse(key, direction, triangleIndex); |
| | | 898 | | } |
| | | 899 | | } |
| | | 900 | | |
| | | 901 | | private readonly struct TriangleUse |
| | | 902 | | { |
| | | 903 | | private TriangleUse(int a, int b, int c) |
| | | 904 | | { |
| | 150889 | 905 | | A = a; |
| | 150889 | 906 | | B = b; |
| | 150889 | 907 | | C = c; |
| | 150889 | 908 | | } |
| | | 909 | | |
| | | 910 | | public int A { get; } |
| | | 911 | | |
| | | 912 | | public int B { get; } |
| | | 913 | | |
| | | 914 | | public int C { get; } |
| | | 915 | | |
| | | 916 | | public static TriangleUse Create(int first, int second, int third) |
| | | 917 | | { |
| | 150889 | 918 | | int a = first; |
| | 150889 | 919 | | int b = second; |
| | 150889 | 920 | | int c = third; |
| | | 921 | | |
| | 150889 | 922 | | if (a > b) |
| | 10137 | 923 | | (a, b) = (b, a); |
| | 150889 | 924 | | if (b > c) |
| | 23276 | 925 | | (b, c) = (c, b); |
| | 150889 | 926 | | if (a > b) |
| | 7576 | 927 | | (a, b) = (b, a); |
| | | 928 | | |
| | 150889 | 929 | | return new TriangleUse(a, b, c); |
| | | 930 | | } |
| | | 931 | | } |
| | | 932 | | |
| | | 933 | | private readonly struct SupportTreeNode |
| | | 934 | | { |
| | | 935 | | private SupportTreeNode( |
| | | 936 | | int index, |
| | | 937 | | Vector3d min, |
| | | 938 | | Vector3d max, |
| | | 939 | | int left, |
| | | 940 | | int right, |
| | | 941 | | int start, |
| | | 942 | | int count, |
| | | 943 | | int minVertexIndex) |
| | | 944 | | { |
| | 3803 | 945 | | Index = index; |
| | 3803 | 946 | | Min = min; |
| | 3803 | 947 | | Max = max; |
| | 3803 | 948 | | Left = left; |
| | 3803 | 949 | | Right = right; |
| | 3803 | 950 | | Start = start; |
| | 3803 | 951 | | Count = count; |
| | 3803 | 952 | | MinVertexIndex = minVertexIndex; |
| | 3803 | 953 | | } |
| | | 954 | | |
| | | 955 | | public int Index { get; } |
| | | 956 | | |
| | | 957 | | public Vector3d Min { get; } |
| | | 958 | | |
| | | 959 | | public Vector3d Max { get; } |
| | | 960 | | |
| | | 961 | | public int Left { get; } |
| | | 962 | | |
| | | 963 | | public int Right { get; } |
| | | 964 | | |
| | | 965 | | public int Start { get; } |
| | | 966 | | |
| | | 967 | | public int Count { get; } |
| | | 968 | | |
| | | 969 | | public int MinVertexIndex { get; } |
| | | 970 | | |
| | 1287 | 971 | | public bool IsLeaf => Count > 0; |
| | | 972 | | |
| | | 973 | | public static SupportTreeNode CreateLeaf( |
| | | 974 | | int index, |
| | | 975 | | Vector3d min, |
| | | 976 | | Vector3d max, |
| | | 977 | | int start, |
| | | 978 | | int count, |
| | | 979 | | int minVertexIndex) => |
| | 1914 | 980 | | new(index, min, max, -1, -1, start, count, minVertexIndex); |
| | | 981 | | |
| | | 982 | | public static SupportTreeNode CreateBranch( |
| | | 983 | | int index, |
| | | 984 | | Vector3d min, |
| | | 985 | | Vector3d max, |
| | | 986 | | int left, |
| | | 987 | | int right, |
| | | 988 | | int minVertexIndex) => |
| | 1889 | 989 | | new(index, min, max, left, right, 0, 0, minVertexIndex); |
| | | 990 | | } |
| | | 991 | | |
| | | 992 | | private sealed class SupportVertexIndexComparer : IComparer<int> |
| | | 993 | | { |
| | 15 | 994 | | private Vector3d[] _vertices = Array.Empty<Vector3d>(); |
| | | 995 | | private int _axis; |
| | | 996 | | |
| | | 997 | | public void Reset(Vector3d[] vertices, int axis) |
| | | 998 | | { |
| | 1371 | 999 | | _vertices = vertices; |
| | 1371 | 1000 | | _axis = axis; |
| | 1371 | 1001 | | } |
| | | 1002 | | |
| | | 1003 | | public int Compare(int first, int second) |
| | | 1004 | | { |
| | 569083 | 1005 | | Fixed64 firstValue = GetAxisValue(_vertices[first], _axis); |
| | 569083 | 1006 | | Fixed64 secondValue = GetAxisValue(_vertices[second], _axis); |
| | 569083 | 1007 | | int valueComparison = firstValue.CompareTo(secondValue); |
| | 569083 | 1008 | | if (valueComparison != 0) |
| | 286464 | 1009 | | return valueComparison; |
| | | 1010 | | |
| | 282619 | 1011 | | return first.CompareTo(second); |
| | | 1012 | | } |
| | | 1013 | | |
| | | 1014 | | private static Fixed64 GetAxisValue(Vector3d vertex, int axis) |
| | | 1015 | | { |
| | 1138166 | 1016 | | return axis switch |
| | 1138166 | 1017 | | { |
| | 431456 | 1018 | | 0 => vertex.X, |
| | 388698 | 1019 | | 1 => vertex.Y, |
| | 318012 | 1020 | | _ => vertex.Z |
| | 1138166 | 1021 | | }; |
| | | 1022 | | } |
| | | 1023 | | } |
| | | 1024 | | } |
| | | 1025 | | } |