| | | 1 | | //======================================================================= |
| | | 2 | | // LSConeCollider.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 Gravitas.Queries; |
| | | 11 | | using SwiftCollections; |
| | | 12 | | using System; |
| | | 13 | | |
| | | 14 | | namespace Gravitas.Colliders; |
| | | 15 | | |
| | | 16 | | /// <summary> |
| | | 17 | | /// Represents a finite circular cone whose local Y axis runs from base plane |
| | | 18 | | /// to apex and whose local origin is the cone's bounding center. |
| | | 19 | | /// </summary> |
| | | 20 | | public sealed class LSConeCollider : LSCollider |
| | | 21 | | { |
| | 524 | 22 | | private Fixed64 _scaledRadius = Fixed64.Half; |
| | | 23 | | private Fixed64 _preparedRadius; |
| | | 24 | | private Fixed64 _preparedHeight; |
| | | 25 | | private Vector3d _preparedAxis; |
| | | 26 | | private Fixed64 _preparedVolume; |
| | | 27 | | |
| | | 28 | | /// <summary>Creates a cone collider with default dimensions.</summary> |
| | 982 | 29 | | public LSConeCollider() { } |
| | | 30 | | |
| | | 31 | | /// <summary>Creates a runtime cone from an authored shape definition.</summary> |
| | 33 | 32 | | public LSConeCollider(ColliderShapeDefinition definition) |
| | | 33 | | { |
| | 33 | 34 | | definition.EnsureKind(ColliderShapeDefinitionKind.Cone); |
| | 33 | 35 | | Material = definition.Material; |
| | 33 | 36 | | Radius = definition.Radius; |
| | 33 | 37 | | Size = definition.Size; |
| | 33 | 38 | | } |
| | | 39 | | |
| | | 40 | | /// <inheritdoc/> |
| | 606 | 41 | | public override ColliderType Shape => ColliderType.Cone; |
| | | 42 | | |
| | | 43 | | /// <inheritdoc/> |
| | 67 | 44 | | public override int Priority => ColliderSettings.GetPriority(Shape); |
| | | 45 | | |
| | | 46 | | /// <inheritdoc/> |
| | 5555 | 47 | | public override Fixed64 ScaledRadius => _scaledRadius; |
| | | 48 | | |
| | | 49 | | /// <summary> |
| | | 50 | | /// Gets the derived normalized world-space base-to-apex axis. Exact |
| | | 51 | | /// geometry remains authoritative in the collider's rigid frame. |
| | | 52 | | /// </summary> |
| | | 53 | | public Vector3d WorldAxis { get; private set; } = Vector3d.Up; |
| | | 54 | | |
| | | 55 | | /// <summary> |
| | | 56 | | /// Gets the full physical distance from the cone's base plane to its apex. |
| | | 57 | | /// </summary> |
| | | 58 | | public Fixed64 Height { get; private set; } |
| | | 59 | | |
| | | 60 | | /// <summary> |
| | | 61 | | /// Gets the solid cone volume used for mass weighting and diagnostics. |
| | | 62 | | /// </summary> |
| | | 63 | | public Fixed64 Volume { get; private set; } |
| | | 64 | | |
| | | 65 | | /// <inheritdoc/> |
| | | 66 | | protected override void OnRadiusChanged() |
| | | 67 | | { |
| | 22 | 68 | | Fixed64 diameter = _radius * 2; |
| | 22 | 69 | | _size = new Vector3d(diameter, _size.Y, diameter); |
| | 22 | 70 | | } |
| | | 71 | | |
| | | 72 | | /// <inheritdoc/> |
| | | 73 | | protected override Vector3d NormalizeSize(Vector3d value) => |
| | 493 | 74 | | new(_radius * 2, value.Y, _radius * 2); |
| | | 75 | | |
| | | 76 | | private protected override void PrepareShape(in ColliderShapeSnapshot snapshot) |
| | | 77 | | { |
| | 534 | 78 | | Fixed64 radiusX = ColliderScalePolicy.ScalePositive( |
| | 534 | 79 | | snapshot.Radius, |
| | 534 | 80 | | snapshot.OwnerScale.X, |
| | 534 | 81 | | snapshot.PartScale.X); |
| | 534 | 82 | | Fixed64 radiusZ = ColliderScalePolicy.ScalePositive( |
| | 534 | 83 | | snapshot.Radius, |
| | 534 | 84 | | snapshot.OwnerScale.Z, |
| | 534 | 85 | | snapshot.PartScale.Z); |
| | 534 | 86 | | _preparedRadius = FixedMath.Max(radiusX, radiusZ); |
| | 534 | 87 | | _preparedHeight = ColliderScalePolicy.ScalePositive( |
| | 534 | 88 | | snapshot.Size.Y, |
| | 534 | 89 | | snapshot.OwnerScale.Y, |
| | 534 | 90 | | snapshot.PartScale.Y); |
| | 534 | 91 | | _preparedAxis = (snapshot.Rotation * Vector3d.Up).Normalized; |
| | 534 | 92 | | _preparedVolume = Fixed64.Pi * _preparedRadius * _preparedRadius |
| | 534 | 93 | | * _preparedHeight / (Fixed64)3; |
| | 534 | 94 | | if (CompoundOwner == null |
| | 534 | 95 | | && !CalculatePreparedLocalMassPoint().TryGetPoint(out _)) |
| | | 96 | | { |
| | 1 | 97 | | throw new InvalidOperationException( |
| | 1 | 98 | | "Prepared collider mass-property point is outside the Fixed64 coordinate domain."); |
| | | 99 | | } |
| | 533 | 100 | | SetPreparedBounds(FixedBoundBox.FromCenteredFiniteConeClippedToDomain( |
| | 533 | 101 | | snapshot.Center, |
| | 533 | 102 | | snapshot.Rotation, |
| | 533 | 103 | | Vector3d.Up, |
| | 533 | 104 | | _preparedHeight, |
| | 533 | 105 | | _preparedRadius)); |
| | 533 | 106 | | } |
| | | 107 | | |
| | | 108 | | private protected override void PublishShape() |
| | | 109 | | { |
| | 530 | 110 | | _scaledRadius = _preparedRadius; |
| | 530 | 111 | | Height = _preparedHeight; |
| | 530 | 112 | | WorldAxis = _preparedAxis; |
| | 530 | 113 | | Volume = _preparedVolume; |
| | 530 | 114 | | Area = _preparedVolume; |
| | 530 | 115 | | } |
| | | 116 | | |
| | | 117 | | internal override ExactMassWeight CalculateMassPropertyWeight() |
| | | 118 | | { |
| | 60 | 119 | | Fixed64 radius = HasCommittedShape |
| | 60 | 120 | | ? ScaledRadius |
| | 60 | 121 | | : GetCurrentScaledRadius(); |
| | 60 | 122 | | Fixed64 height = HasCommittedShape |
| | 60 | 123 | | ? Height |
| | 60 | 124 | | : GetCurrentHeight(); |
| | 60 | 125 | | return ExactMassWeight.FromProduct( |
| | 60 | 126 | | Fixed64.Pi / (Fixed64)3, |
| | 60 | 127 | | radius, |
| | 60 | 128 | | radius, |
| | 60 | 129 | | height); |
| | | 130 | | } |
| | | 131 | | |
| | | 132 | | internal override ExactMassWeight CalculatePreparedMassPropertyWeight() => |
| | 73 | 133 | | ExactMassWeight.FromProduct( |
| | 73 | 134 | | Fixed64.Pi / (Fixed64)3, |
| | 73 | 135 | | _preparedRadius, |
| | 73 | 136 | | _preparedRadius, |
| | 73 | 137 | | _preparedHeight); |
| | | 138 | | |
| | | 139 | | internal override ExactMassPoint3D CalculateLocalMassPoint() => |
| | 1015 | 140 | | TransformRelativeMassPropertyPointExact( |
| | 1015 | 141 | | GetLocalCenterOfMass( |
| | 1015 | 142 | | HasCommittedShape |
| | 1015 | 143 | | ? Height |
| | 1015 | 144 | | : GetCurrentHeight())); |
| | | 145 | | |
| | | 146 | | internal override ExactMassPoint3D CalculatePreparedLocalMassPoint() => |
| | 1040 | 147 | | TransformPreparedRelativeMassPropertyPointExact( |
| | 1040 | 148 | | GetLocalCenterOfMass(_preparedHeight)); |
| | | 149 | | |
| | | 150 | | private Fixed64 GetCurrentHeight() |
| | | 151 | | { |
| | 2 | 152 | | GetCurrentShapeScales( |
| | 2 | 153 | | out Vector3d ownerScale, |
| | 2 | 154 | | out Vector3d partScale); |
| | 2 | 155 | | return ColliderScalePolicy.ScalePositive( |
| | 2 | 156 | | Size.Y, |
| | 2 | 157 | | ownerScale.Y, |
| | 2 | 158 | | partScale.Y); |
| | | 159 | | } |
| | | 160 | | |
| | | 161 | | private static Vector3d GetLocalCenterOfMass(Fixed64 height) => |
| | 2055 | 162 | | new( |
| | 2055 | 163 | | Fixed64.Zero, |
| | 2055 | 164 | | -height * Fixed64.FromFraction(1, 4), |
| | 2055 | 165 | | Fixed64.Zero); |
| | | 166 | | |
| | | 167 | | internal override Fixed3x3 CalculateCenterOfMassInertiaTensor(Fixed64 mass) |
| | | 168 | | { |
| | 484 | 169 | | Fixed64 radiusSqr = ScaledRadiusSqr; |
| | 484 | 170 | | Fixed64 heightSqr = Height * Height; |
| | 484 | 171 | | Fixed64 inertiaXZ = mass |
| | 484 | 172 | | * ((Fixed64.FromFraction(3, 20) * radiusSqr) |
| | 484 | 173 | | + (Fixed64.FromFraction(3, 80) * heightSqr)); |
| | 484 | 174 | | Fixed64 inertiaY = Fixed64.FromFraction(3, 10) * mass * radiusSqr; |
| | | 175 | | |
| | 484 | 176 | | Fixed3x3 tensor = new( |
| | 484 | 177 | | inertiaXZ, Fixed64.Zero, Fixed64.Zero, |
| | 484 | 178 | | Fixed64.Zero, inertiaY, Fixed64.Zero, |
| | 484 | 179 | | Fixed64.Zero, Fixed64.Zero, inertiaXZ); |
| | 484 | 180 | | return tensor; |
| | | 181 | | } |
| | | 182 | | |
| | | 183 | | /// <inheritdoc/> |
| | | 184 | | public override Fixed64 GetFrontalArea(Vector3d direction) |
| | | 185 | | { |
| | 4 | 186 | | Fixed64 directionMagnitude = direction.Magnitude; |
| | 4 | 187 | | if (directionMagnitude <= Fixed64.Epsilon) |
| | 1 | 188 | | return Area; |
| | | 189 | | |
| | 3 | 190 | | Vector3d normalizedDirection = direction / directionMagnitude; |
| | 3 | 191 | | Fixed64 axial = Rotation.Inverse() |
| | 3 | 192 | | .Rotate(normalizedDirection).Y.Abs(); |
| | 3 | 193 | | Fixed64 radialFactorSqr = Fixed64.One - axial * axial; |
| | 3 | 194 | | Fixed64 radialFactor = radialFactorSqr <= Fixed64.Zero ? Fixed64.Zero : FixedMath.Sqrt(radialFactorSqr); |
| | | 195 | | |
| | 3 | 196 | | Fixed64 baseArea = Fixed64.Pi * ScaledRadiusSqr; |
| | 3 | 197 | | Fixed64 triangularProfile = ScaledRadius * Height; |
| | 3 | 198 | | return axial * baseArea + radialFactor * triangularProfile; |
| | | 199 | | } |
| | | 200 | | |
| | | 201 | | /// <inheritdoc/> |
| | | 202 | | public override Vector3d ClosestPointOnSurface(Vector3d other) |
| | | 203 | | { |
| | 85 | 204 | | FixedPointAnchor surfaceAnchor = |
| | 85 | 205 | | GetClosestSurfaceAnchor(other, out _); |
| | 85 | 206 | | if (!surfaceAnchor.TryGetPoint(out Vector3d surfacePoint)) |
| | | 207 | | { |
| | 1 | 208 | | throw new InvalidOperationException( |
| | 1 | 209 | | "The closest cone surface point is outside the representable coordinate domain."); |
| | | 210 | | } |
| | | 211 | | |
| | 84 | 212 | | return surfacePoint; |
| | | 213 | | } |
| | | 214 | | |
| | | 215 | | /// <inheritdoc/> |
| | | 216 | | public override Vector3d GetNormalAtPoint(Vector3d point) |
| | | 217 | | { |
| | 489 | 218 | | _ = GetClosestSurfaceAnchor( |
| | 489 | 219 | | point, |
| | 489 | 220 | | out Vector3d outwardNormal); |
| | 489 | 221 | | return outwardNormal; |
| | | 222 | | } |
| | | 223 | | |
| | | 224 | | internal override FixedPointAnchor GetClosestSurfaceAnchor( |
| | | 225 | | Vector3d point, |
| | | 226 | | out Vector3d normal) => |
| | 787 | 227 | | WideFiniteAxisIntersection |
| | 787 | 228 | | .GetClosestCenteredFiniteConeSurfaceAnchor( |
| | 787 | 229 | | point, |
| | 787 | 230 | | Center, |
| | 787 | 231 | | Rotation, |
| | 787 | 232 | | Vector3d.Up, |
| | 787 | 233 | | Height, |
| | 787 | 234 | | ScaledRadius, |
| | 787 | 235 | | Vector3d.Right, |
| | 787 | 236 | | out normal, |
| | 787 | 237 | | out _); |
| | | 238 | | |
| | | 239 | | /// <inheritdoc/> |
| | | 240 | | public override bool ColliderOverlapsRay(RaycastSegmentWorker worker, ref SwiftList<Vector3d> outputIntersectionPoin |
| | 477 | 241 | | worker.CheckConeOverlaps(this, ref outputIntersectionPoints); |
| | | 242 | | |
| | | 243 | | internal bool ContainsWorldPoint( |
| | | 244 | | Vector3d point, |
| | | 245 | | Fixed64 tolerance = default) => |
| | 6 | 246 | | FixedSegment.TryGetClosestCenteredFiniteConeSurfaceAnchor( |
| | 6 | 247 | | point, |
| | 6 | 248 | | Center, |
| | 6 | 249 | | Rotation, |
| | 6 | 250 | | Vector3d.Up, |
| | 6 | 251 | | Height, |
| | 6 | 252 | | ScaledRadius, |
| | 6 | 253 | | Vector3d.Right, |
| | 6 | 254 | | out _, |
| | 6 | 255 | | out _, |
| | 6 | 256 | | out Fixed64 signedDistance) |
| | 6 | 257 | | && signedDistance <= tolerance; |
| | | 258 | | } |