| | | 1 | | //======================================================================= |
| | | 2 | | // LSSphereCollider.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 FixedMathSharp; |
| | | 10 | | using FixedMathSharp.Geometry; |
| | | 11 | | using Gravitas.Queries; |
| | | 12 | | using SwiftCollections; |
| | | 13 | | |
| | | 14 | | namespace Gravitas.Colliders; |
| | | 15 | | |
| | | 16 | | /// <summary>Represents a runtime 3D sphere collider.</summary> |
| | | 17 | | public sealed class LSSphereCollider : LSCollider |
| | | 18 | | { |
| | 4713 | 19 | | private Fixed64 _scaledRadius = Fixed64.Half; |
| | | 20 | | private Fixed64 _preparedRadius; |
| | | 21 | | private Fixed64 _preparedArea; |
| | | 22 | | |
| | | 23 | | /// <summary>Creates a sphere collider with default dimensions.</summary> |
| | 8536 | 24 | | public LSSphereCollider() { } |
| | | 25 | | |
| | | 26 | | /// <summary>Creates a runtime sphere from an authored shape definition.</summary> |
| | 445 | 27 | | public LSSphereCollider(ColliderShapeDefinition definition) |
| | | 28 | | { |
| | 445 | 29 | | definition.EnsureKind(ColliderShapeDefinitionKind.Sphere); |
| | 444 | 30 | | Material = definition.Material; |
| | 444 | 31 | | Radius = definition.Radius; |
| | 444 | 32 | | } |
| | | 33 | | |
| | | 34 | | /// <inheritdoc/> |
| | 1199744 | 35 | | public override ColliderType Shape => ColliderType.Sphere; |
| | | 36 | | |
| | | 37 | | /// <inheritdoc/> |
| | 14885 | 38 | | public override int Priority => ColliderSettings.GetPriority(Shape); |
| | | 39 | | |
| | | 40 | | /// <inheritdoc/> |
| | | 41 | | protected override void OnRadiusChanged() |
| | | 42 | | { |
| | 873 | 43 | | Fixed64 diameter = _radius * 2; |
| | 873 | 44 | | _size = new Vector3d(diameter, diameter, diameter); |
| | 873 | 45 | | } |
| | | 46 | | |
| | | 47 | | /// <inheritdoc/> |
| | 129706 | 48 | | public override Fixed64 ScaledRadius => _scaledRadius; |
| | | 49 | | |
| | | 50 | | private protected override void PrepareShape(in ColliderShapeSnapshot snapshot) |
| | | 51 | | { |
| | 61156 | 52 | | Fixed64 radiusX = ColliderScalePolicy.ScalePositive( |
| | 61156 | 53 | | snapshot.Radius, |
| | 61156 | 54 | | snapshot.OwnerScale.X, |
| | 61156 | 55 | | snapshot.PartScale.X); |
| | 61154 | 56 | | Fixed64 radiusY = ColliderScalePolicy.ScalePositive( |
| | 61154 | 57 | | snapshot.Radius, |
| | 61154 | 58 | | snapshot.OwnerScale.Y, |
| | 61154 | 59 | | snapshot.PartScale.Y); |
| | 61154 | 60 | | Fixed64 radiusZ = ColliderScalePolicy.ScalePositive( |
| | 61154 | 61 | | snapshot.Radius, |
| | 61154 | 62 | | snapshot.OwnerScale.Z, |
| | 61154 | 63 | | snapshot.PartScale.Z); |
| | 61154 | 64 | | _preparedRadius = FixedMath.Max(radiusX, FixedMath.Max(radiusY, radiusZ)); |
| | 61154 | 65 | | SetPreparedGeometryReach(_preparedRadius); |
| | 61154 | 66 | | _preparedArea = Fixed64.Pi * _preparedRadius * _preparedRadius; |
| | 61154 | 67 | | SetPreparedBounds(FixedBoundBox.FromCenterAndScopeClippedToDomain( |
| | 61154 | 68 | | snapshot.Center, |
| | 61154 | 69 | | Vector3d.One * _preparedRadius)); |
| | 61154 | 70 | | } |
| | | 71 | | |
| | | 72 | | private protected override void PublishShape() |
| | | 73 | | { |
| | 60482 | 74 | | _scaledRadius = _preparedRadius; |
| | 60482 | 75 | | Area = _preparedArea; |
| | 60482 | 76 | | } |
| | | 77 | | |
| | | 78 | | internal override ExactMassWeight CalculateMassPropertyWeight() |
| | | 79 | | { |
| | 360 | 80 | | Fixed64 radius = GetMassPropertyRadius(); |
| | 359 | 81 | | return ExactMassWeight.FromProduct( |
| | 359 | 82 | | Fixed64.FromFraction(4, 3) * Fixed64.Pi, |
| | 359 | 83 | | radius, |
| | 359 | 84 | | radius, |
| | 359 | 85 | | radius); |
| | | 86 | | } |
| | | 87 | | |
| | | 88 | | internal Fixed64 GetMassPropertyRadius() => |
| | 18043 | 89 | | HasCommittedShape ? _scaledRadius : GetCurrentScaledRadius(); |
| | | 90 | | |
| | | 91 | | internal override ExactMassWeight CalculatePreparedMassPropertyWeight() => |
| | 10226 | 92 | | ExactMassWeight.FromProduct( |
| | 10226 | 93 | | Fixed64.FromFraction(4, 3) * Fixed64.Pi, |
| | 10226 | 94 | | _preparedRadius, |
| | 10226 | 95 | | _preparedRadius, |
| | 10226 | 96 | | _preparedRadius); |
| | | 97 | | |
| | | 98 | | internal override Fixed3x3 CalculateCenterOfMassInertiaTensor(Fixed64 mass) |
| | | 99 | | { |
| | | 100 | | // For a solid sphere, the inertia tensor is (2/5)*m*r^2 for the diagonal elements |
| | 17549 | 101 | | Fixed64 radius = GetMassPropertyRadius(); |
| | 17549 | 102 | | Fixed64 diagonalElement = ExactMassProperties.InertiaMoment( |
| | 17549 | 103 | | mass, ExactMassProperties.InertiaGeometryTerm(Fixed64.FromFraction(2, 5), radius, radius)); |
| | | 104 | | |
| | 17549 | 105 | | Fixed3x3 tensor = new( |
| | 17549 | 106 | | diagonalElement, Fixed64.Zero, Fixed64.Zero, |
| | 17549 | 107 | | Fixed64.Zero, diagonalElement, Fixed64.Zero, |
| | 17549 | 108 | | Fixed64.Zero, Fixed64.Zero, diagonalElement |
| | 17549 | 109 | | ); |
| | 17549 | 110 | | return tensor; |
| | | 111 | | } |
| | | 112 | | |
| | | 113 | | /// <inheritdoc/> |
| | | 114 | | public override Vector3d ClosestPointOnSurface(Vector3d other) |
| | | 115 | | { |
| | 113 | 116 | | FixedPointAnchor anchor = |
| | 113 | 117 | | GetClosestSurfaceAnchor(other, out _); |
| | 113 | 118 | | if (anchor.TryGetPoint(out Vector3d point)) |
| | 112 | 119 | | return point; |
| | | 120 | | |
| | 1 | 121 | | throw new InvalidOperationException( |
| | 1 | 122 | | "The closest sphere surface point is outside the representable coordinate domain."); |
| | | 123 | | } |
| | | 124 | | |
| | | 125 | | /// <inheritdoc/> |
| | | 126 | | public override Vector3d GetNormalAtPoint(Vector3d point) |
| | | 127 | | { |
| | 1738 | 128 | | if (point == Center) |
| | 5 | 129 | | return Vector3d.Zero; |
| | | 130 | | |
| | 1733 | 131 | | _ = GetClosestSurfaceAnchor( |
| | 1733 | 132 | | point, |
| | 1733 | 133 | | out Vector3d normal); |
| | 1733 | 134 | | return normal; |
| | | 135 | | } |
| | | 136 | | |
| | | 137 | | internal override FixedPointAnchor GetClosestSurfaceAnchor( |
| | | 138 | | Vector3d point, |
| | | 139 | | out Vector3d normal) => |
| | 10219 | 140 | | GetClosestSurfaceAnchor(point, out normal, out _); |
| | | 141 | | |
| | | 142 | | internal override FixedPointAnchor GetClosestSurfaceAnchor( |
| | | 143 | | Vector3d point, out Vector3d normal, out bool contained) => |
| | 10751 | 144 | | WideFiniteAxisIntersection |
| | 10751 | 145 | | .GetClosestCenteredCapsuleSurfaceAnchor( |
| | 10751 | 146 | | point, |
| | 10751 | 147 | | Center, |
| | 10751 | 148 | | FixedQuaternion.Identity, |
| | 10751 | 149 | | Vector3d.Up, |
| | 10751 | 150 | | Fixed64.Zero, |
| | 10751 | 151 | | ScaledRadius, |
| | 10751 | 152 | | Vector3d.Right, |
| | 10751 | 153 | | out normal, |
| | 10751 | 154 | | out contained); |
| | | 155 | | |
| | | 156 | | /// <inheritdoc/> |
| | | 157 | | public override bool ColliderOverlapsRay(RaycastSegmentWorker worker, ref SwiftList<Vector3d> outputIntersectionPoin |
| | 1386 | 158 | | worker.CheckSphereOverlaps(this, ref outputIntersectionPoints); |
| | | 159 | | } |