| | | 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 System; |
| | | 9 | | using FixedMathSharp; |
| | | 10 | | using FixedMathSharp.Geometry; |
| | | 11 | | using Gravitas.Queries; |
| | | 12 | | using SwiftCollections; |
| | | 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 | | { |
| | 981 | 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> |
| | 1744 | 29 | | public LSConeCollider() { } |
| | | 30 | | |
| | | 31 | | /// <summary>Creates a runtime cone from an authored shape definition.</summary> |
| | 109 | 32 | | public LSConeCollider(ColliderShapeDefinition definition) |
| | | 33 | | { |
| | 109 | 34 | | definition.EnsureKind(ColliderShapeDefinitionKind.Cone); |
| | 109 | 35 | | Material = definition.Material; |
| | 109 | 36 | | Radius = definition.Radius; |
| | 109 | 37 | | Size = definition.Size; |
| | 109 | 38 | | } |
| | | 39 | | |
| | | 40 | | /// <inheritdoc/> |
| | 466631 | 41 | | public override ColliderType Shape => ColliderType.Cone; |
| | | 42 | | |
| | | 43 | | /// <inheritdoc/> |
| | 3523 | 44 | | public override int Priority => ColliderSettings.GetPriority(Shape); |
| | | 45 | | |
| | | 46 | | /// <inheritdoc/> |
| | 55483 | 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, rounded once after final division and |
| | | 62 | | /// saturated to <see cref="Fixed64.MaxValue"/> when unrepresentable. |
| | | 63 | | /// Compound mass weighting retains its separate unnarrowed measure. |
| | | 64 | | /// </summary> |
| | | 65 | | public Fixed64 Volume { get; private set; } |
| | | 66 | | |
| | | 67 | | /// <inheritdoc/> |
| | | 68 | | protected override void OnRadiusChanged() |
| | | 69 | | { |
| | 194 | 70 | | Fixed64 diameter = _radius * 2; |
| | 194 | 71 | | _size = new Vector3d(diameter, _size.Y, diameter); |
| | 194 | 72 | | } |
| | | 73 | | |
| | | 74 | | /// <inheritdoc/> |
| | | 75 | | protected override Vector3d NormalizeSize(Vector3d value) => |
| | 829 | 76 | | new(_radius * 2, value.Y, _radius * 2); |
| | | 77 | | |
| | | 78 | | private protected override void PrepareShape(in ColliderShapeSnapshot snapshot) |
| | | 79 | | { |
| | 18596 | 80 | | Fixed64 radiusX = ColliderScalePolicy.ScalePositive( |
| | 18596 | 81 | | snapshot.Radius, |
| | 18596 | 82 | | snapshot.OwnerScale.X, |
| | 18596 | 83 | | snapshot.PartScale.X); |
| | 18596 | 84 | | Fixed64 radiusZ = ColliderScalePolicy.ScalePositive( |
| | 18596 | 85 | | snapshot.Radius, |
| | 18596 | 86 | | snapshot.OwnerScale.Z, |
| | 18596 | 87 | | snapshot.PartScale.Z); |
| | 18596 | 88 | | Fixed64 radius = FixedMath.Max(radiusX, radiusZ); |
| | 18596 | 89 | | Fixed64 height = ColliderScalePolicy.ScalePositive( |
| | 18596 | 90 | | snapshot.Size.Y, |
| | 18596 | 91 | | snapshot.OwnerScale.Y, |
| | 18596 | 92 | | snapshot.PartScale.Y); |
| | | 93 | | Fixed64 volume; |
| | 18596 | 94 | | if (HasCommittedShape && radius == ScaledRadius && height == Height) |
| | | 95 | | { |
| | 17610 | 96 | | volume = Volume; |
| | | 97 | | } |
| | 986 | 98 | | else if (!Fixed64.TryMultiplyDivideBySum( |
| | 986 | 99 | | Fixed64.Pi, radius, radius, height, |
| | 986 | 100 | | (Fixed64)3, Fixed64.Zero, Fixed64.Zero, Fixed64.Zero, |
| | 986 | 101 | | out volume)) |
| | | 102 | | { |
| | | 103 | | // Saturate only the final diagnostic measure; valid geometry and |
| | | 104 | | // exact compound mass weights need not fit a scalar volume. |
| | 9 | 105 | | volume = Fixed64.MaxValue; |
| | | 106 | | } |
| | 18596 | 107 | | _preparedRadius = radius; |
| | 18596 | 108 | | _preparedHeight = height; |
| | 18596 | 109 | | SetPreparedGeometryReach(HasCommittedShape && radius == _scaledRadius && height == Height |
| | 18596 | 110 | | ? CanonicalCenteredProxyRadius : ColliderGeometryPolicy.FiniteAxis(radius, height)); |
| | 18596 | 111 | | _preparedVolume = volume; |
| | 18596 | 112 | | _preparedAxis = HasCommittedShape && snapshot.Rotation == CanonicalRotation |
| | 18596 | 113 | | ? WorldAxis |
| | 18596 | 114 | | : (snapshot.Rotation * Vector3d.Up).Normalized; |
| | | 115 | | // The complete admitted body-relative reach also bounds this primitive's |
| | | 116 | | // centroid; bodyless attachments retain their independent scalar check. |
| | 18596 | 117 | | if (CompoundOwner == null |
| | 18596 | 118 | | && Body == null |
| | 18596 | 119 | | && !CalculatePreparedLocalMassPoint().TryGetPoint(out _)) |
| | | 120 | | { |
| | 2 | 121 | | throw new InvalidOperationException( |
| | 2 | 122 | | "Prepared collider mass-property point is outside the Fixed64 coordinate domain."); |
| | | 123 | | } |
| | 18594 | 124 | | SetPreparedBounds(FixedBoundBox.FromCenteredFiniteConeClippedToDomain( |
| | 18594 | 125 | | snapshot.Center, |
| | 18594 | 126 | | snapshot.Rotation, |
| | 18594 | 127 | | Vector3d.Up, |
| | 18594 | 128 | | _preparedHeight, |
| | 18594 | 129 | | _preparedRadius)); |
| | 18594 | 130 | | } |
| | | 131 | | |
| | | 132 | | private protected override void PublishShape() |
| | | 133 | | { |
| | 18589 | 134 | | _scaledRadius = _preparedRadius; |
| | 18589 | 135 | | Height = _preparedHeight; |
| | 18589 | 136 | | WorldAxis = _preparedAxis; |
| | 18589 | 137 | | Volume = _preparedVolume; |
| | 18589 | 138 | | Area = _preparedVolume; |
| | 18589 | 139 | | } |
| | | 140 | | |
| | | 141 | | internal override ExactMassWeight CalculateMassPropertyWeight() |
| | | 142 | | { |
| | 54 | 143 | | GetMassPropertyGeometry(out Fixed64 radius, out Fixed64 height); |
| | 54 | 144 | | return ExactMassWeight.FromProduct( |
| | 54 | 145 | | Fixed64.Pi / (Fixed64)3, |
| | 54 | 146 | | radius, |
| | 54 | 147 | | radius, |
| | 54 | 148 | | height); |
| | | 149 | | } |
| | | 150 | | |
| | | 151 | | private void GetMassPropertyGeometry(out Fixed64 radius, out Fixed64 height) |
| | | 152 | | { |
| | 909 | 153 | | if (HasCommittedShape) |
| | | 154 | | { |
| | 899 | 155 | | radius = _scaledRadius; |
| | 899 | 156 | | height = Height; |
| | 899 | 157 | | return; |
| | | 158 | | } |
| | | 159 | | |
| | 10 | 160 | | GetCurrentShapeScales(out Vector3d ownerScale, out Vector3d partScale); |
| | 10 | 161 | | radius = FixedMath.Max( |
| | 10 | 162 | | ColliderScalePolicy.ScalePositive(Radius, ownerScale.X, partScale.X), |
| | 10 | 163 | | ColliderScalePolicy.ScalePositive(Radius, ownerScale.Z, partScale.Z)); |
| | 10 | 164 | | height = ColliderScalePolicy.ScalePositive(Size.Y, ownerScale.Y, partScale.Y); |
| | 10 | 165 | | } |
| | | 166 | | |
| | | 167 | | internal override ExactMassWeight CalculatePreparedMassPropertyWeight() => |
| | 5343 | 168 | | ExactMassWeight.FromProduct( |
| | 5343 | 169 | | Fixed64.Pi / (Fixed64)3, |
| | 5343 | 170 | | _preparedRadius, |
| | 5343 | 171 | | _preparedRadius, |
| | 5343 | 172 | | _preparedHeight); |
| | | 173 | | |
| | | 174 | | internal override ExactMassPoint3D CalculateLocalMassPoint() => |
| | 8894 | 175 | | TransformRelativeMassPropertyPointExact( |
| | 8894 | 176 | | GetLocalCenterOfMass( |
| | 8894 | 177 | | HasCommittedShape |
| | 8894 | 178 | | ? Height |
| | 8894 | 179 | | : GetCurrentHeight())); |
| | | 180 | | |
| | | 181 | | internal override ExactMassPoint3D CalculatePreparedLocalMassPoint() => |
| | 3661 | 182 | | TransformPreparedRelativeMassPropertyPointExact( |
| | 3661 | 183 | | GetLocalCenterOfMass(_preparedHeight)); |
| | | 184 | | |
| | | 185 | | private Fixed64 GetCurrentHeight() |
| | | 186 | | { |
| | 9 | 187 | | GetCurrentShapeScales( |
| | 9 | 188 | | out Vector3d ownerScale, |
| | 9 | 189 | | out Vector3d partScale); |
| | 9 | 190 | | return ColliderScalePolicy.ScalePositive( |
| | 9 | 191 | | Size.Y, |
| | 9 | 192 | | ownerScale.Y, |
| | 9 | 193 | | partScale.Y); |
| | | 194 | | } |
| | | 195 | | |
| | | 196 | | private static Vector3d GetLocalCenterOfMass(Fixed64 height) => |
| | 12555 | 197 | | new( |
| | 12555 | 198 | | Fixed64.Zero, |
| | 12555 | 199 | | -height * Fixed64.FromFraction(1, 4), |
| | 12555 | 200 | | Fixed64.Zero); |
| | | 201 | | |
| | | 202 | | internal void GetInertiaGeometry(out Signed192 transverse, out Signed192 axial) |
| | | 203 | | { |
| | 855 | 204 | | GetMassPropertyGeometry(out Fixed64 radius, out Fixed64 height); |
| | 855 | 205 | | transverse = WideArithmetic.AddSigned192( |
| | 855 | 206 | | ExactMassProperties.InertiaGeometryTerm(Fixed64.FromFraction(3, 20), radius, radius), |
| | 855 | 207 | | ExactMassProperties.InertiaGeometryTerm(Fixed64.FromFraction(3, 80), height, height)); |
| | 855 | 208 | | axial = ExactMassProperties.InertiaGeometryTerm(Fixed64.FromFraction(3, 10), radius, radius); |
| | 855 | 209 | | } |
| | | 210 | | |
| | | 211 | | internal override Fixed3x3 CalculateCenterOfMassInertiaTensor(Fixed64 mass) |
| | | 212 | | { |
| | 837 | 213 | | GetInertiaGeometry(out Signed192 transverse, out Signed192 axial); |
| | 837 | 214 | | Fixed64 inertiaXZ = ExactMassProperties.InertiaMoment(mass, transverse); |
| | 837 | 215 | | Fixed64 inertiaY = ExactMassProperties.InertiaMoment(mass, axial); |
| | | 216 | | |
| | 837 | 217 | | Fixed3x3 tensor = new( |
| | 837 | 218 | | inertiaXZ, Fixed64.Zero, Fixed64.Zero, |
| | 837 | 219 | | Fixed64.Zero, inertiaY, Fixed64.Zero, |
| | 837 | 220 | | Fixed64.Zero, Fixed64.Zero, inertiaXZ); |
| | 837 | 221 | | return tensor; |
| | | 222 | | } |
| | | 223 | | |
| | | 224 | | /// <inheritdoc/> |
| | | 225 | | public override Fixed64 GetFrontalArea(Vector3d direction) |
| | | 226 | | { |
| | 3 | 227 | | Fixed64 directionMagnitude = direction.Magnitude; |
| | 3 | 228 | | if (directionMagnitude <= Fixed64.Epsilon) |
| | 1 | 229 | | return Area; |
| | | 230 | | |
| | 2 | 231 | | Vector3d normalizedDirection = direction / directionMagnitude; |
| | 2 | 232 | | Fixed64 axial = Rotation.Inverse() |
| | 2 | 233 | | .Rotate(normalizedDirection).Y.Abs(); |
| | 2 | 234 | | Fixed64 radialFactorSqr = Fixed64.One - axial * axial; |
| | 2 | 235 | | Fixed64 radialFactor = radialFactorSqr <= Fixed64.Zero ? Fixed64.Zero : FixedMath.Sqrt(radialFactorSqr); |
| | | 236 | | |
| | 2 | 237 | | Fixed64 baseArea = Fixed64.Pi * ScaledRadiusSqr; |
| | 2 | 238 | | Fixed64 triangularProfile = ScaledRadius * Height; |
| | 2 | 239 | | return axial * baseArea + radialFactor * triangularProfile; |
| | | 240 | | } |
| | | 241 | | |
| | | 242 | | /// <inheritdoc/> |
| | | 243 | | public override Vector3d ClosestPointOnSurface(Vector3d other) |
| | | 244 | | { |
| | 10 | 245 | | FixedPointAnchor surfaceAnchor = |
| | 10 | 246 | | GetClosestSurfaceAnchor(other, out _); |
| | 10 | 247 | | if (!surfaceAnchor.TryGetPoint(out Vector3d surfacePoint)) |
| | | 248 | | { |
| | 1 | 249 | | throw new InvalidOperationException( |
| | 1 | 250 | | "The closest cone surface point is outside the representable coordinate domain."); |
| | | 251 | | } |
| | | 252 | | |
| | 9 | 253 | | return surfacePoint; |
| | | 254 | | } |
| | | 255 | | |
| | | 256 | | /// <inheritdoc/> |
| | | 257 | | public override Vector3d GetNormalAtPoint(Vector3d point) |
| | | 258 | | { |
| | 1298 | 259 | | _ = GetClosestSurfaceAnchor( |
| | 1298 | 260 | | point, |
| | 1298 | 261 | | out Vector3d outwardNormal); |
| | 1298 | 262 | | return outwardNormal; |
| | | 263 | | } |
| | | 264 | | |
| | | 265 | | internal override FixedPointAnchor GetClosestSurfaceAnchor( |
| | | 266 | | Vector3d point, |
| | | 267 | | out Vector3d normal) => |
| | 1849 | 268 | | GetClosestSurfaceAnchor(point, out normal, out _); |
| | | 269 | | |
| | | 270 | | internal override FixedPointAnchor GetClosestSurfaceAnchor( |
| | | 271 | | Vector3d point, out Vector3d normal, out bool contained) => |
| | 2012 | 272 | | WideFiniteAxisIntersection |
| | 2012 | 273 | | .GetClosestCenteredFiniteConeSurfaceAnchor( |
| | 2012 | 274 | | point, |
| | 2012 | 275 | | Center, |
| | 2012 | 276 | | Rotation, |
| | 2012 | 277 | | Vector3d.Up, |
| | 2012 | 278 | | Height, |
| | 2012 | 279 | | ScaledRadius, |
| | 2012 | 280 | | Vector3d.Right, |
| | 2012 | 281 | | out normal, |
| | 2012 | 282 | | out contained); |
| | | 283 | | |
| | | 284 | | /// <inheritdoc/> |
| | | 285 | | public override bool ColliderOverlapsRay(RaycastSegmentWorker worker, ref SwiftList<Vector3d> outputIntersectionPoin |
| | 622 | 286 | | worker.CheckConeOverlaps(this, ref outputIntersectionPoints); |
| | | 287 | | |
| | | 288 | | internal bool ContainsWorldPoint( |
| | | 289 | | Vector3d point, |
| | | 290 | | Fixed64 tolerance = default) => |
| | 6 | 291 | | FixedSegment.TryGetClosestCenteredFiniteConeSurfaceAnchor( |
| | 6 | 292 | | point, |
| | 6 | 293 | | Center, |
| | 6 | 294 | | Rotation, |
| | 6 | 295 | | Vector3d.Up, |
| | 6 | 296 | | Height, |
| | 6 | 297 | | ScaledRadius, |
| | 6 | 298 | | Vector3d.Right, |
| | 6 | 299 | | out _, |
| | 6 | 300 | | out _, |
| | 6 | 301 | | out Fixed64 signedDistance) |
| | 6 | 302 | | && signedDistance <= tolerance; |
| | | 303 | | } |