| | | 1 | | //======================================================================= |
| | | 2 | | // LSMeshCollider.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.Runtime.CompilerServices; |
| | | 10 | | using FixedMathSharp; |
| | | 11 | | using FixedMathSharp.Geometry; |
| | | 12 | | using Gravitas.Queries; |
| | | 13 | | using SwiftCollections; |
| | | 14 | | using SwiftCollections.Query; |
| | | 15 | | |
| | | 16 | | namespace Gravitas.Colliders; |
| | | 17 | | |
| | | 18 | | /// <summary>Represents a runtime 3D collider backed by deterministic triangle-mesh geometry.</summary> |
| | | 19 | | public sealed class LSMeshCollider : LSCollider |
| | | 20 | | { |
| | | 21 | | internal StaticSurface? StaticSurface { get; set; } |
| | 3990 | 22 | | private readonly SwiftList<int> _triangleQueryBuffer = new(); |
| | | 23 | | private bool _inertiaOriginValid, _inertiaOriginExact; |
| | | 24 | | private Vector3d _inertiaOriginOffset, _inertiaOriginScale, _inertiaOrigin; |
| | | 25 | | |
| | | 26 | | /// <inheritdoc/> |
| | 2493053 | 27 | | public override ColliderType Shape => ColliderType.Mesh; |
| | | 28 | | |
| | | 29 | | /// <inheritdoc/> |
| | 25031 | 30 | | public override int Priority => ColliderSettings.GetPriority(Shape); |
| | | 31 | | |
| | | 32 | | /// <inheritdoc/> |
| | 2833 | 33 | | public override Fixed64 Area => Mesh.TotalArea; |
| | | 34 | | |
| | | 35 | | /// <summary>Gets the deterministic runtime mesh geometry.</summary> |
| | | 36 | | /// <remarks> |
| | | 37 | | /// Detached access prepares geometry at the current authored part scale. |
| | | 38 | | /// Runtime transforms are owned by the collider, body and host transform; |
| | | 39 | | /// the returned mesh cannot be transformed independently. |
| | | 40 | | /// </remarks> |
| | | 41 | | public PhysicsMesh Mesh |
| | | 42 | | { |
| | | 43 | | get |
| | | 44 | | { |
| | 1885195 | 45 | | EnsureDetachedMassGeometry(); |
| | 1885195 | 46 | | return GeometryMesh; |
| | | 47 | | } |
| | | 48 | | } |
| | | 49 | | |
| | | 50 | | internal PhysicsMesh GeometryMesh { get; } |
| | | 51 | | |
| | | 52 | | /// <summary>Gets the mesh collision mode.</summary> |
| | 60289 | 53 | | public MeshColliderMode Mode => GeometryMesh.Mode; |
| | | 54 | | |
| | | 55 | | /// <summary> |
| | | 56 | | /// Gets whether the mesh's exact-position-welded triangle topology is one |
| | | 57 | | /// connected, consistently wound, closed two-manifold surface. |
| | | 58 | | /// </summary> |
| | 177 | 59 | | public bool IsClosedSurface => GeometryMesh.IsClosedSurface; |
| | | 60 | | |
| | | 61 | | /// <summary> |
| | | 62 | | /// Determines how inertia is derived when this mesh collider is attached to a body with angular dynamics. |
| | | 63 | | /// </summary> |
| | | 64 | | public MeshInertiaPolicy InertiaPolicy { get; } |
| | | 65 | | |
| | | 66 | | /// <summary>Creates a convex mesh collider from authored vertices and triangle indices.</summary> |
| | | 67 | | public LSMeshCollider(Vector3d[] vertices, int[] triangles) |
| | 19 | 68 | | : this(vertices, triangles, MeshColliderMode.Convex, MeshInertiaPolicy.RequireClosedVolume) |
| | | 69 | | { |
| | 19 | 70 | | } |
| | | 71 | | |
| | | 72 | | /// <summary>Creates a mesh collider with an explicit collision mode.</summary> |
| | | 73 | | public LSMeshCollider(Vector3d[] vertices, int[] triangles, MeshColliderMode mode) |
| | 95 | 74 | | : this(vertices, triangles, mode, MeshInertiaPolicy.RequireClosedVolume) |
| | | 75 | | { |
| | 95 | 76 | | } |
| | | 77 | | |
| | | 78 | | /// <summary>Creates a mesh collider with explicit collision and inertia policies.</summary> |
| | 3990 | 79 | | public LSMeshCollider( |
| | 3990 | 80 | | Vector3d[] vertices, |
| | 3990 | 81 | | int[] triangles, |
| | 3990 | 82 | | MeshColliderMode mode, |
| | 3990 | 83 | | MeshInertiaPolicy inertiaPolicy) |
| | | 84 | | { |
| | 3990 | 85 | | ValidateInertiaPolicy(inertiaPolicy); |
| | 3990 | 86 | | InertiaPolicy = inertiaPolicy; |
| | 3990 | 87 | | GeometryMesh = new PhysicsMesh(vertices, triangles, mode, colliderOwned: true); |
| | 3990 | 88 | | _offset = GeometryMesh.LocalBounds.Center; |
| | | 89 | | // The inherited authoring summary may saturate; runtime vertices retain exact source differences. |
| | 3990 | 90 | | _size = GeometryMesh.LocalBounds.Max - GeometryMesh.LocalBounds.Min; |
| | 3990 | 91 | | _radius = _size.Magnitude * Fixed64.Half; |
| | 3990 | 92 | | } |
| | | 93 | | |
| | | 94 | | /// <summary>Creates a runtime convex mesh from an authored shape definition.</summary> |
| | | 95 | | public LSMeshCollider(ColliderShapeDefinition definition) |
| | 214 | 96 | | : this( |
| | 214 | 97 | | GetVertices(definition), |
| | 214 | 98 | | GetTriangles(definition), |
| | 214 | 99 | | MeshColliderMode.Convex, |
| | 214 | 100 | | GetInertiaPolicy(definition)) |
| | | 101 | | { |
| | 214 | 102 | | Material = definition.Material; |
| | 214 | 103 | | } |
| | | 104 | | |
| | | 105 | | /// <inheritdoc/> |
| | | 106 | | public override Fixed64 ScaledRadius |
| | | 107 | | { |
| | | 108 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 109 | | get |
| | | 110 | | { |
| | 94 | 111 | | EnsureDetachedMassGeometry(); |
| | 94 | 112 | | return GeometryMesh.ScaledLocalRadius; |
| | | 113 | | } |
| | | 114 | | } |
| | | 115 | | |
| | | 116 | | private protected override void PrepareShape(in ColliderShapeSnapshot snapshot) |
| | | 117 | | { |
| | 19703 | 118 | | GeometryMesh.PrepareTransformation( |
| | 19703 | 119 | | snapshot.Center, |
| | 19703 | 120 | | snapshot.Rotation, |
| | 19703 | 121 | | snapshot.OwnerScale, |
| | 19703 | 122 | | snapshot.PartScale, |
| | 19703 | 123 | | InertiaPolicy, |
| | 19703 | 124 | | new ScaledRigidFrame3d(snapshot.PoseOrigin, snapshot.PlacementRotation, |
| | 19703 | 125 | | snapshot.LocalOffset, snapshot.OwnerScale, snapshot.Rotation, |
| | 19703 | 126 | | GeometryMesh.LocalBounds.Center, snapshot.PartScale)); |
| | 19695 | 127 | | if (CompoundOwner == null |
| | 19695 | 128 | | && !CalculatePreparedLocalMassPoint().TryGetPoint(out _)) |
| | | 129 | | { |
| | 1 | 130 | | throw new InvalidOperationException( |
| | 1 | 131 | | "Prepared collider mass-property point is outside the Fixed64 coordinate domain."); |
| | | 132 | | } |
| | 19694 | 133 | | SetPreparedGeometryReach(GeometryMesh.PreparedScaledLocalRadius); |
| | 19694 | 134 | | SetPreparedBounds(GeometryMesh.PreparedBounds); |
| | 19694 | 135 | | } |
| | | 136 | | |
| | | 137 | | private protected override void PublishShape() |
| | | 138 | | { |
| | 19685 | 139 | | GeometryMesh.PublishPreparedTransformation(); |
| | 19685 | 140 | | StaticSurface?.Invalidate(geometryChanged: true, changed: this); |
| | 7 | 141 | | } |
| | | 142 | | |
| | | 143 | | internal override ExactMassWeight CalculateMassPropertyWeight() |
| | | 144 | | { |
| | 81 | 145 | | EnsureDetachedMassGeometry(); |
| | 81 | 146 | | if (InertiaPolicy == MeshInertiaPolicy.SurfaceApproximation) |
| | | 147 | | { |
| | 33 | 148 | | _ = GeometryMesh.SurfaceMassProperties; |
| | 33 | 149 | | return GeometryMesh.SurfaceMassWeight; |
| | | 150 | | } |
| | | 151 | | |
| | 48 | 152 | | if (GeometryMesh.TryGetClosedVolumeMassProperties(out MeshMassProperties properties, out _)) |
| | 39 | 153 | | return GeometryMesh.GetCompoundMassIntegrals(usePrepared: false, InertiaPolicy).Weight; |
| | | 154 | | |
| | 9 | 155 | | return ExactMassWeight.Zero; |
| | | 156 | | } |
| | | 157 | | |
| | | 158 | | internal override ExactMassWeight CalculatePreparedMassPropertyWeight() |
| | | 159 | | { |
| | 5650 | 160 | | if (InertiaPolicy == MeshInertiaPolicy.SurfaceApproximation) |
| | 268 | 161 | | return GeometryMesh.PreparedSurfaceMassWeight; |
| | | 162 | | |
| | 5382 | 163 | | if (GeometryMesh.TryGetPreparedClosedVolumeMassProperties( |
| | 5382 | 164 | | out MeshMassProperties properties, |
| | 5382 | 165 | | out _)) |
| | | 166 | | { |
| | 5352 | 167 | | return GeometryMesh.GetCompoundMassIntegrals(usePrepared: true, InertiaPolicy).Weight; |
| | | 168 | | } |
| | | 169 | | |
| | 30 | 170 | | return ExactMassWeight.Zero; |
| | | 171 | | } |
| | | 172 | | |
| | | 173 | | internal override bool SupportsMassProperties |
| | | 174 | | { |
| | | 175 | | get |
| | | 176 | | { |
| | 40 | 177 | | EnsureDetachedMassGeometry(); |
| | 37 | 178 | | return InertiaPolicy == MeshInertiaPolicy.SurfaceApproximation |
| | 37 | 179 | | || GeometryMesh.TryGetClosedVolumeMassProperties(out _, out _); |
| | | 180 | | } |
| | | 181 | | } |
| | | 182 | | |
| | | 183 | | internal override ExactMassPoint3D CalculateLocalMassPoint() |
| | | 184 | | { |
| | 7738 | 185 | | EnsureDetachedMassGeometry(); |
| | | 186 | | Vector3d meshCenterOfMass; |
| | 7738 | 187 | | if (InertiaPolicy == MeshInertiaPolicy.SurfaceApproximation) |
| | | 188 | | { |
| | 2664 | 189 | | meshCenterOfMass = |
| | 2664 | 190 | | GeometryMesh.SurfaceMassProperties.CenterOfMass; |
| | | 191 | | } |
| | 5074 | 192 | | else if (GeometryMesh.TryGetClosedVolumeMassProperties( |
| | 5074 | 193 | | out MeshMassProperties properties, |
| | 5074 | 194 | | out _)) |
| | | 195 | | { |
| | 5041 | 196 | | meshCenterOfMass = properties.CenterOfMass; |
| | | 197 | | } |
| | | 198 | | else |
| | | 199 | | { |
| | 33 | 200 | | meshCenterOfMass = Vector3d.Zero; |
| | | 201 | | } |
| | | 202 | | |
| | 7738 | 203 | | return TransformRelativeMassPropertyPointExact( |
| | 7738 | 204 | | meshCenterOfMass); |
| | | 205 | | } |
| | | 206 | | |
| | | 207 | | internal override ExactMassPoint3D CalculatePreparedLocalMassPoint() |
| | | 208 | | { |
| | | 209 | | Vector3d meshCenterOfMass; |
| | 19695 | 210 | | if (InertiaPolicy == MeshInertiaPolicy.SurfaceApproximation) |
| | | 211 | | { |
| | 7500 | 212 | | meshCenterOfMass = |
| | 7500 | 213 | | GeometryMesh.PreparedSurfaceMassProperties.CenterOfMass; |
| | | 214 | | } |
| | 12195 | 215 | | else if (GeometryMesh.TryGetPreparedClosedVolumeMassProperties( |
| | 12195 | 216 | | out MeshMassProperties properties, |
| | 12195 | 217 | | out _)) |
| | | 218 | | { |
| | 12100 | 219 | | meshCenterOfMass = properties.CenterOfMass; |
| | | 220 | | } |
| | | 221 | | else |
| | | 222 | | { |
| | 95 | 223 | | meshCenterOfMass = Vector3d.Zero; |
| | | 224 | | } |
| | | 225 | | |
| | 19695 | 226 | | return TransformPreparedRelativeMassPropertyPointExact( |
| | 19695 | 227 | | meshCenterOfMass); |
| | | 228 | | } |
| | | 229 | | |
| | | 230 | | /// <inheritdoc/> |
| | | 231 | | public override Fixed3x3 CalculateInertiaTensor(Fixed64 mass, Vector3d localCenterOfMassOffset) |
| | | 232 | | { |
| | 1731 | 233 | | if (mass <= Fixed64.Zero) return Fixed3x3.Zero; |
| | 1729 | 234 | | EnsureDetachedMassGeometry(); |
| | 1729 | 235 | | if (CompoundOwner == null) |
| | | 236 | | { |
| | 1725 | 237 | | GetCurrentShapeScales(out Vector3d ownerScale, out _); |
| | 1725 | 238 | | if (!_inertiaOriginValid || _inertiaOriginOffset != LocalOffset || _inertiaOriginScale != ownerScale) |
| | | 239 | | { |
| | 567 | 240 | | ExactMassPoint3D origin = TransformRelativeMassPropertyPointExact(Vector3d.Zero); |
| | 567 | 241 | | _inertiaOriginExact = origin.TryGetPoint(out _inertiaOrigin); |
| | 567 | 242 | | if (_inertiaOriginExact) |
| | | 243 | | { |
| | 566 | 244 | | ExactMassPoint3D projected = ExactMassPoint3D.FromPoint(_inertiaOrigin); |
| | 566 | 245 | | _inertiaOriginExact = WideArithmetic.SubtractSigned320(origin.XNumerator, projected.XNumerator).IsZe |
| | 566 | 246 | | && WideArithmetic.SubtractSigned320(origin.YNumerator, projected.YNumerator).IsZero |
| | 566 | 247 | | && WideArithmetic.SubtractSigned320(origin.ZNumerator, projected.ZNumerator).IsZero; |
| | | 248 | | } |
| | 567 | 249 | | _inertiaOriginOffset = LocalOffset; |
| | 567 | 250 | | _inertiaOriginScale = ownerScale; |
| | 567 | 251 | | _inertiaOriginValid = true; |
| | | 252 | | } |
| | 1725 | 253 | | if (_inertiaOriginExact && Vector3d.TrySubtract(localCenterOfMassOffset, _inertiaOrigin, out Vector3d refere |
| | 1704 | 254 | | return GeometryMesh.CalculateInertiaTensor(mass, InertiaPolicy, reference); |
| | | 255 | | } |
| | | 256 | | // Keep semantic offsets and first moments until the final mass-applied |
| | | 257 | | // conversion when a body-local reference cannot be mapped to a scalar. |
| | 25 | 258 | | if (InertiaPolicy == MeshInertiaPolicy.SurfaceApproximation) |
| | 19 | 259 | | _ = GeometryMesh.SurfaceMassProperties; |
| | 6 | 260 | | else if (!GeometryMesh.TryGetClosedVolumeMassProperties(out _, out MeshVolumeValidationResult result)) |
| | 1 | 261 | | throw new InvalidOperationException($"Mesh inertia requires a validated closed volume. Validation result: {r |
| | 24 | 262 | | MeshMassIntegrals integrals = GeometryMesh.GetCompoundMassIntegrals(false, InertiaPolicy); |
| | 24 | 263 | | CompoundInertiaTensor exact = default; |
| | 24 | 264 | | CompoundInertiaMath3D.AddMesh(ref exact, integrals, CompoundLocalRotation, |
| | 24 | 265 | | TransformRelativeMassPropertyPointExact(integrals.Reference), localCenterOfMassOffset); |
| | 24 | 266 | | return exact.MaterializeSaturated(mass, integrals.Weight); |
| | | 267 | | } |
| | | 268 | | |
| | | 269 | | internal override Fixed3x3 CalculateCenterOfMassInertiaTensor(Fixed64 mass) => |
| | 1 | 270 | | CalculateInertiaTensor(mass, CalculateLocalCenterOfMassOffset()); |
| | | 271 | | |
| | | 272 | | private void EnsureDetachedMassGeometry() |
| | | 273 | | { |
| | 3788398 | 274 | | if (HasCommittedShape) return; |
| | 1358 | 275 | | GetCurrentShapeScales(out Vector3d ownerScale, out Vector3d partScale); |
| | 2305 | 276 | | if (GeometryMesh.HasRuntimeGeometry && GeometryMesh.OwnerScale == ownerScale && GeometryMesh.PartScale == partSc |
| | | 277 | | // Authored part scale may be compensated by a later host scale; prepare |
| | | 278 | | // only when current properties are requested, never during construction. |
| | 411 | 279 | | GeometryMesh.PrepareTransformation(_offset, FixedQuaternion.Identity, ownerScale, partScale, null); |
| | 408 | 280 | | GeometryMesh.PublishPreparedTransformation(); |
| | 408 | 281 | | _bounds = GeometryMesh.Bounds; |
| | 408 | 282 | | } |
| | | 283 | | |
| | | 284 | | /// <inheritdoc/> |
| | | 285 | | public override Fixed64 GetFrontalArea(Vector3d direction) => |
| | 5 | 286 | | Mesh.GetFrontalArea(direction); |
| | | 287 | | |
| | | 288 | | /// <summary>Writes triangle indices near a world-space point into a caller-owned buffer.</summary> |
| | | 289 | | public void GetNearbyTriangles(Vector3d queryPoint, SwiftList<int> result) |
| | | 290 | | { |
| | 1 | 291 | | EnsureDetachedMassGeometry(); |
| | 1 | 292 | | FixedBoundVolume queryBounds = CreateQueryBounds(queryPoint, GetMeshQueryHalfExtent()); |
| | 1 | 293 | | GetTrianglesInBounds(queryBounds, result); |
| | 1 | 294 | | } |
| | | 295 | | |
| | | 296 | | /// <summary>Writes triangle indices overlapping world-space bounds into a caller-owned buffer.</summary> |
| | | 297 | | public void GetTrianglesInBounds(FixedBoundVolume queryBounds, SwiftList<int> result) |
| | | 298 | | { |
| | 151929 | 299 | | Mesh.GetTrianglesInWorldBounds(queryBounds, result); |
| | 151929 | 300 | | } |
| | | 301 | | |
| | | 302 | | /// <inheritdoc/> |
| | | 303 | | public override Vector3d ClosestPointOnSurface(Vector3d queryPoint) |
| | | 304 | | { |
| | 44 | 305 | | FixedPointAnchor closest = |
| | 44 | 306 | | GetClosestSurfaceAnchor(queryPoint, out _); |
| | 44 | 307 | | if (closest.TryGetPoint(out Vector3d point)) |
| | 42 | 308 | | return point; |
| | | 309 | | |
| | 2 | 310 | | throw new InvalidOperationException( |
| | 2 | 311 | | "The closest mesh point is outside the representable coordinate range."); |
| | | 312 | | } |
| | | 313 | | |
| | | 314 | | internal override FixedPointAnchor GetClosestSurfaceAnchor( |
| | | 315 | | Vector3d queryPoint, |
| | | 316 | | out Vector3d normal) |
| | | 317 | | { |
| | 1258 | 318 | | FindClosestPointAnchor( |
| | 1258 | 319 | | queryPoint, |
| | 1258 | 320 | | _triangleQueryBuffer, |
| | 1258 | 321 | | out FixedPointAnchor closest, |
| | 1258 | 322 | | out normal); |
| | 1258 | 323 | | return closest; |
| | | 324 | | } |
| | | 325 | | |
| | | 326 | | internal void FindClosestPointAnchor( |
| | | 327 | | Vector3d queryPoint, |
| | | 328 | | out FixedPointAnchor closest, |
| | | 329 | | out Vector3d normal) => |
| | 1665 | 330 | | FindClosestPointAnchor( |
| | 1665 | 331 | | queryPoint, |
| | 1665 | 332 | | _triangleQueryBuffer, |
| | 1665 | 333 | | out closest, |
| | 1665 | 334 | | out normal); |
| | | 335 | | |
| | | 336 | | internal void FindClosestPointAnchor( |
| | | 337 | | Vector3d queryPoint, |
| | | 338 | | SwiftList<int> triangleBuffer, |
| | | 339 | | out FixedPointAnchor closest, |
| | | 340 | | out Vector3d normal) |
| | | 341 | | { |
| | 5134 | 342 | | if (!Mesh.GeometryFrame.IsRigid || !Mesh.TryConvertWorldToRigidGeometry( |
| | 5134 | 343 | | queryPoint, |
| | 5134 | 344 | | out Vector3d localQueryPoint)) |
| | | 345 | | { |
| | 119 | 346 | | FindClosestPointWithoutLocalQuery( |
| | 119 | 347 | | queryPoint, |
| | 119 | 348 | | triangleBuffer, |
| | 119 | 349 | | out closest, |
| | 119 | 350 | | out normal); |
| | 119 | 351 | | return; |
| | | 352 | | } |
| | | 353 | | |
| | 5015 | 354 | | Vector3d localClosest = localQueryPoint; |
| | 5015 | 355 | | normal = Vector3d.Zero; |
| | 5015 | 356 | | int closestTriangleIndex = -1; |
| | 5015 | 357 | | _ = KeepClosestPointOnTriangle( |
| | 5015 | 358 | | 0, |
| | 5015 | 359 | | localQueryPoint, |
| | 5015 | 360 | | ref closestTriangleIndex, |
| | 5015 | 361 | | ref localClosest, |
| | 5015 | 362 | | ref normal); |
| | 5015 | 363 | | if (localClosest == localQueryPoint) |
| | | 364 | | { |
| | 2439 | 365 | | closest = Mesh.CreatePointAnchor(localClosest); |
| | 2439 | 366 | | normal = Mesh.Rotation.Rotate(normal).Normalized; |
| | 2439 | 367 | | return; |
| | | 368 | | } |
| | | 369 | | |
| | 2576 | 370 | | if (!TryCreateClosestPointSearchBounds( |
| | 2576 | 371 | | localQueryPoint, |
| | 2576 | 372 | | localClosest, |
| | 2576 | 373 | | out FixedBoundVolume queryBounds)) |
| | | 374 | | { |
| | 2 | 375 | | FindClosestPointAcrossAllTriangles( |
| | 2 | 376 | | localQueryPoint, |
| | 2 | 377 | | 1, |
| | 2 | 378 | | ref closestTriangleIndex, |
| | 2 | 379 | | ref localClosest, |
| | 2 | 380 | | ref normal); |
| | | 381 | | } |
| | | 382 | | else |
| | | 383 | | { |
| | 2574 | 384 | | Mesh.GetTrianglesInGeometryBounds(queryBounds, triangleBuffer); |
| | 24416 | 385 | | for (int i = 0; i < triangleBuffer.Count; i++) |
| | | 386 | | { |
| | 9634 | 387 | | _ = KeepClosestPointOnTriangle( |
| | 9634 | 388 | | triangleBuffer[i], |
| | 9634 | 389 | | localQueryPoint, |
| | 9634 | 390 | | ref closestTriangleIndex, |
| | 9634 | 391 | | ref localClosest, |
| | 9634 | 392 | | ref normal); |
| | | 393 | | } |
| | | 394 | | } |
| | | 395 | | |
| | 2576 | 396 | | closest = Mesh.CreatePointAnchor(localClosest); |
| | 2576 | 397 | | normal = Mesh.Rotation.Rotate(normal).Normalized; |
| | 2576 | 398 | | } |
| | | 399 | | |
| | | 400 | | private void FindClosestPointWithoutLocalQuery( |
| | | 401 | | Vector3d queryPoint, |
| | | 402 | | SwiftList<int> triangleBuffer, |
| | | 403 | | out FixedPointAnchor closest, |
| | | 404 | | out Vector3d normal) |
| | | 405 | | { |
| | 119 | 406 | | var queryAnchor = new FixedPointAnchor( |
| | 119 | 407 | | queryPoint, |
| | 119 | 408 | | FixedQuaternion.Identity, |
| | 119 | 409 | | Vector3d.Zero); |
| | 119 | 410 | | bool exactRanking = !Mesh.GeometryFrame.IsRigid; |
| | | 411 | | const int words = 36; |
| | 119 | 412 | | Span<ulong> values = stackalloc ulong[exactRanking ? 8 * words : 0]; |
| | 119 | 413 | | Span<int> signs = stackalloc int[exactRanking ? 8 : 0]; |
| | 119 | 414 | | ContactQuadratic bestNumerator = exactRanking ? ContactQuadratic.At(values, signs, 0, words) : default; |
| | 119 | 415 | | ContactQuadratic bestDenominator = exactRanking ? ContactQuadratic.At(values, signs, 1, words) : default; |
| | 119 | 416 | | ContactQuadratic candidateNumerator = exactRanking ? ContactQuadratic.At(values, signs, 2, words) : default; |
| | 119 | 417 | | ContactQuadratic candidateDenominator = exactRanking ? ContactQuadratic.At(values, signs, 3, words) : default; |
| | 119 | 418 | | closest = GetClosestPointAnchorOnTriangle( |
| | 119 | 419 | | 0, |
| | 119 | 420 | | queryAnchor, |
| | 119 | 421 | | bestNumerator, |
| | 119 | 422 | | bestDenominator, |
| | 119 | 423 | | out normal); |
| | | 424 | | // The first finite witness bounds every possible winner in physical |
| | | 425 | | // space. Inverse-map that outward box into the existing authored BVH. |
| | 119 | 426 | | FixedBoundVolume bounds = default; |
| | 119 | 427 | | bool bounded = closest.TryGetPoint(out Vector3d firstPoint) |
| | 119 | 428 | | && TryCreateClosestPointSearchBounds(queryPoint, firstPoint, out bounds); |
| | 232 | 429 | | if (bounded) Mesh.GetTrianglesInWorldBounds(bounds, triangleBuffer); |
| | 119 | 430 | | int bestTriangle = 0; |
| | 119 | 431 | | int count = bounded ? triangleBuffer.Count : Mesh.TriangleCount; |
| | 2790 | 432 | | for (int index = 0; index < count; index++) |
| | | 433 | | { |
| | 1276 | 434 | | int triangleIndex = bounded ? triangleBuffer[index] : index; |
| | 1276 | 435 | | if (triangleIndex == 0) continue; |
| | 1157 | 436 | | FixedPointAnchor candidate = GetClosestPointAnchorOnTriangle( |
| | 1157 | 437 | | triangleIndex, queryAnchor, candidateNumerator, candidateDenominator, out Vector3d candidateNormal); |
| | 1157 | 438 | | int comparison = exactRanking |
| | 1157 | 439 | | ? ContactQuadratic.CompareRatios(candidateNumerator, candidateDenominator, ReadOnlySpan<ulong>.Empty, |
| | 1157 | 440 | | bestNumerator, bestDenominator, ReadOnlySpan<ulong>.Empty) |
| | 1157 | 441 | | : queryAnchor.CompareSquaredDistance(candidate, closest); |
| | 1157 | 442 | | if (comparison > 0 || comparison == 0 && triangleIndex >= bestTriangle) continue; |
| | 107 | 443 | | if (exactRanking) |
| | | 444 | | { |
| | 106 | 445 | | candidateNumerator.CopyTo(bestNumerator); |
| | 106 | 446 | | candidateDenominator.CopyTo(bestDenominator); |
| | | 447 | | } |
| | 107 | 448 | | closest = candidate; |
| | 107 | 449 | | normal = candidateNormal; |
| | 107 | 450 | | bestTriangle = triangleIndex; |
| | | 451 | | } |
| | | 452 | | |
| | 119 | 453 | | } |
| | | 454 | | |
| | | 455 | | private FixedPointAnchor GetClosestPointAnchorOnTriangle( |
| | | 456 | | int triangleIndex, |
| | | 457 | | in FixedPointAnchor query, |
| | | 458 | | scoped ContactQuadratic squaredDistanceNumerator, |
| | | 459 | | scoped ContactQuadratic squaredDistanceDenominator, |
| | | 460 | | out Vector3d normal) |
| | | 461 | | { |
| | 1276 | 462 | | Mesh.GetGeometryTriangleVertices( |
| | 1276 | 463 | | triangleIndex, |
| | 1276 | 464 | | out Vector3d first, |
| | 1276 | 465 | | out Vector3d second, |
| | 1276 | 466 | | out Vector3d third); |
| | 1276 | 467 | | var triangle = new FixedTriangle(first, second, third); |
| | | 468 | | FixedPointAnchor closest; |
| | 1276 | 469 | | if (Mesh.GeometryFrame.IsRigid) |
| | 8 | 470 | | closest = triangle.GetClosestPointAnchor(Mesh.Origin, Mesh.Rotation, query); |
| | | 471 | | else |
| | | 472 | | { |
| | | 473 | | // This caller creates a scalar query with zero local displacement. |
| | | 474 | | // Its finite closest witness stays within admitted mesh reach. |
| | 1268 | 475 | | bool represented = WideTriangleRelations.TryGetClosestPointAnchor(triangle, Mesh.GeometryFrame, |
| | 1268 | 476 | | RationalPoint3d.FromScalarPoint(query.Origin), Mesh.Origin, |
| | 1268 | 477 | | squaredDistanceNumerator, squaredDistanceDenominator, out closest); |
| | | 478 | | System.Diagnostics.Debug.Assert(represented); |
| | | 479 | | } |
| | 1276 | 480 | | normal = OrientNormalTowardPoint( |
| | 1276 | 481 | | Mesh.GetFaceNormalWorld(triangleIndex), |
| | 1276 | 482 | | query, |
| | 1276 | 483 | | closest); |
| | 1276 | 484 | | return closest; |
| | | 485 | | } |
| | | 486 | | |
| | | 487 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 488 | | private Fixed64 GetMeshQueryHalfExtent() |
| | | 489 | | { |
| | 1 | 490 | | Vector3d extents = |
| | 1 | 491 | | ColliderCanonicalBounds.GetMaximumAbsoluteExtents( |
| | 1 | 492 | | this, |
| | 1 | 493 | | Mesh.Origin); |
| | 1 | 494 | | return FixedMath.Max( |
| | 1 | 495 | | FixedMath.Max(extents.X, extents.Y), |
| | 1 | 496 | | extents.Z); |
| | | 497 | | } |
| | | 498 | | |
| | | 499 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 500 | | private static FixedBoundVolume CreateQueryBounds(Vector3d center, Fixed64 halfExtent) |
| | | 501 | | { |
| | 1 | 502 | | Vector3d extents = Vector3d.One * halfExtent; |
| | 1 | 503 | | return new FixedBoundVolume(center - extents, center + extents); |
| | | 504 | | } |
| | | 505 | | |
| | | 506 | | private static void ValidateInertiaPolicy(MeshInertiaPolicy inertiaPolicy) |
| | | 507 | | { |
| | 3990 | 508 | | ChronicleGuard.ThrowIfArgument( |
| | 3990 | 509 | | inertiaPolicy != MeshInertiaPolicy.RequireClosedVolume && |
| | 3990 | 510 | | inertiaPolicy != MeshInertiaPolicy.SurfaceApproximation, |
| | 3990 | 511 | | nameof(inertiaPolicy), |
| | 3990 | 512 | | "Unsupported mesh inertia policy."); |
| | 3990 | 513 | | } |
| | | 514 | | |
| | | 515 | | private static Vector3d[] GetVertices(ColliderShapeDefinition definition) |
| | | 516 | | { |
| | 214 | 517 | | definition.EnsureKind(ColliderShapeDefinitionKind.ConvexMesh); |
| | 214 | 518 | | return definition.GetMeshVerticesForRuntime(); |
| | | 519 | | } |
| | | 520 | | |
| | | 521 | | private static int[] GetTriangles(ColliderShapeDefinition definition) |
| | | 522 | | { |
| | 214 | 523 | | definition.EnsureKind(ColliderShapeDefinitionKind.ConvexMesh); |
| | 214 | 524 | | return definition.GetMeshTrianglesForRuntime(); |
| | | 525 | | } |
| | | 526 | | |
| | | 527 | | private static MeshInertiaPolicy GetInertiaPolicy(ColliderShapeDefinition definition) |
| | | 528 | | { |
| | 214 | 529 | | definition.EnsureKind(ColliderShapeDefinitionKind.ConvexMesh); |
| | 214 | 530 | | return definition.MeshInertiaPolicy; |
| | | 531 | | } |
| | | 532 | | |
| | | 533 | | private static bool TryCreateClosestPointSearchBounds( |
| | | 534 | | Vector3d point, |
| | | 535 | | Vector3d closest, |
| | | 536 | | out FixedBoundVolume bounds) |
| | | 537 | | { |
| | 2694 | 538 | | if (!Vector3d.TrySubtract(point, closest, out Vector3d delta) |
| | 2694 | 539 | | || !delta.TryGetMagnitudeCeiling(out Fixed64 halfExtent) |
| | 2694 | 540 | | || !Fixed64.TryAdd( |
| | 2694 | 541 | | halfExtent, |
| | 2694 | 542 | | Fixed64.Epsilon, |
| | 2694 | 543 | | out halfExtent)) |
| | | 544 | | { |
| | 7 | 545 | | bounds = default; |
| | 7 | 546 | | return false; |
| | | 547 | | } |
| | | 548 | | |
| | 2687 | 549 | | Vector3d extents = Vector3d.One * halfExtent; |
| | 2687 | 550 | | Vector3d min = point - extents; |
| | 2687 | 551 | | Vector3d max = point + extents; |
| | 2687 | 552 | | bounds = new FixedBoundVolume(min, max); |
| | 2687 | 553 | | return true; |
| | | 554 | | } |
| | | 555 | | |
| | | 556 | | private void FindClosestPointAcrossAllTriangles( |
| | | 557 | | Vector3d point, |
| | | 558 | | int startTriangleIndex, |
| | | 559 | | ref int closestTriangleIndex, |
| | | 560 | | ref Vector3d closest, |
| | | 561 | | ref Vector3d normal) |
| | | 562 | | { |
| | 6 | 563 | | for (int i = startTriangleIndex; i < Mesh.TriangleCount; i++) |
| | | 564 | | { |
| | 1 | 565 | | _ = KeepClosestPointOnTriangle( |
| | 1 | 566 | | i, |
| | 1 | 567 | | point, |
| | 1 | 568 | | ref closestTriangleIndex, |
| | 1 | 569 | | ref closest, |
| | 1 | 570 | | ref normal); |
| | | 571 | | } |
| | 2 | 572 | | } |
| | | 573 | | |
| | | 574 | | private bool KeepClosestPointOnTriangle( |
| | | 575 | | int triangleIndex, |
| | | 576 | | Vector3d point, |
| | | 577 | | ref int closestTriangleIndex, |
| | | 578 | | ref Vector3d closest, |
| | | 579 | | ref Vector3d normal) |
| | | 580 | | { |
| | 14650 | 581 | | Mesh.GetGeometryTriangleVertices( |
| | 14650 | 582 | | triangleIndex, |
| | 14650 | 583 | | out Vector3d first, |
| | 14650 | 584 | | out Vector3d second, |
| | 14650 | 585 | | out Vector3d third); |
| | 14650 | 586 | | var triangle = new FixedTriangle(first, second, third); |
| | 14650 | 587 | | Vector3d faceNormal = triangle.Normal; |
| | 14650 | 588 | | Vector3d pointOnTriangle = triangle.ClosestPoint(point); |
| | 14650 | 589 | | if (closestTriangleIndex >= 0) |
| | | 590 | | { |
| | 9635 | 591 | | int distanceComparison = Vector3d.CompareDistanceSquared( |
| | 9635 | 592 | | point, |
| | 9635 | 593 | | pointOnTriangle, |
| | 9635 | 594 | | point, |
| | 9635 | 595 | | closest); |
| | 9635 | 596 | | if (distanceComparison > 0 |
| | 9635 | 597 | | || (distanceComparison == 0 && triangleIndex >= closestTriangleIndex)) |
| | | 598 | | { |
| | 8110 | 599 | | return false; |
| | | 600 | | } |
| | | 601 | | } |
| | | 602 | | |
| | 6540 | 603 | | closestTriangleIndex = triangleIndex; |
| | 6540 | 604 | | closest = pointOnTriangle; |
| | 6540 | 605 | | normal = OrientNormalTowardPoint(faceNormal, point - pointOnTriangle); |
| | 6540 | 606 | | return true; |
| | | 607 | | } |
| | | 608 | | |
| | | 609 | | /// <inheritdoc/> |
| | | 610 | | public override Vector3d GetNormalAtPoint(Vector3d point) |
| | | 611 | | { |
| | 657 | 612 | | _ = GetClosestSurfaceAnchor( |
| | 657 | 613 | | point, |
| | 657 | 614 | | out Vector3d normal); |
| | 657 | 615 | | return normal; |
| | | 616 | | } |
| | | 617 | | |
| | | 618 | | internal override bool TryGetPlanarSurfaceNormal(Vector3d point, out Vector3d normal) |
| | | 619 | | { |
| | 2211 | 620 | | FindClosestPointAnchor( |
| | 2211 | 621 | | point, |
| | 2211 | 622 | | _triangleQueryBuffer, |
| | 2211 | 623 | | out _, |
| | 2211 | 624 | | out normal); |
| | 2211 | 625 | | return true; |
| | | 626 | | } |
| | | 627 | | |
| | | 628 | | /// <inheritdoc/> |
| | | 629 | | public override bool ColliderOverlapsRay(RaycastSegmentWorker worker, ref SwiftList<Vector3d> outputIntersectionPoin |
| | | 630 | | { |
| | 1346 | 631 | | return worker.CheckMeshOverlaps(this, ref outputIntersectionPoints); |
| | | 632 | | } |
| | | 633 | | |
| | | 634 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 635 | | private static Vector3d OrientNormalTowardPoint(Vector3d normal, Vector3d targetDirection) |
| | | 636 | | { |
| | 6540 | 637 | | if (targetDirection.MagnitudeSquared <= Fixed64.Epsilon) |
| | 3088 | 638 | | return normal; |
| | | 639 | | |
| | 3452 | 640 | | return Vector3d.Dot(normal, targetDirection) < Fixed64.Zero ? -normal : normal; |
| | | 641 | | } |
| | | 642 | | |
| | | 643 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 644 | | private static Vector3d OrientNormalTowardPoint( |
| | | 645 | | Vector3d normal, |
| | | 646 | | in FixedPointAnchor target, |
| | | 647 | | in FixedPointAnchor surfacePoint) |
| | | 648 | | { |
| | 1276 | 649 | | if (target.ProjectNonNegativeOffsetFrom(surfacePoint, normal) |
| | 1276 | 650 | | > Fixed64.Zero) |
| | | 651 | | { |
| | 971 | 652 | | return normal; |
| | | 653 | | } |
| | | 654 | | |
| | 305 | 655 | | return surfacePoint.ProjectNonNegativeOffsetFrom(target, normal) |
| | 305 | 656 | | > Fixed64.Zero |
| | 305 | 657 | | ? -normal |
| | 305 | 658 | | : normal; |
| | | 659 | | } |
| | | 660 | | } |