| | | 1 | | //======================================================================= |
| | | 2 | | // LSCylinderCollider.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 finite runtime 3D cylinder whose local axis follows Y.</summary> |
| | | 17 | | public sealed class LSCylinderCollider : LSCollider |
| | | 18 | | { |
| | 1044 | 19 | | private Fixed64 _scaledRadius = Fixed64.Half; |
| | | 20 | | private Fixed64 _preparedRadius; |
| | | 21 | | private Fixed64 _preparedHeight; |
| | | 22 | | private Vector3d _preparedAxis; |
| | | 23 | | private Fixed64 _preparedArea; |
| | | 24 | | |
| | | 25 | | /// <summary>Creates a cylinder collider with default dimensions.</summary> |
| | 1604 | 26 | | public LSCylinderCollider() { } |
| | | 27 | | |
| | | 28 | | /// <summary>Creates a runtime cylinder from an authored shape definition.</summary> |
| | 242 | 29 | | public LSCylinderCollider(ColliderShapeDefinition definition) |
| | | 30 | | { |
| | 242 | 31 | | definition.EnsureKind(ColliderShapeDefinitionKind.Cylinder); |
| | 242 | 32 | | Material = definition.Material; |
| | 242 | 33 | | Radius = definition.Radius; |
| | 242 | 34 | | Size = definition.Size; |
| | 242 | 35 | | } |
| | | 36 | | |
| | | 37 | | /// <inheritdoc/> |
| | 106450 | 38 | | public override ColliderType Shape => ColliderType.Cylinder; |
| | | 39 | | /// <inheritdoc/> |
| | 2458 | 40 | | public override int Priority => ColliderSettings.GetPriority(Shape); |
| | | 41 | | |
| | | 42 | | /// <inheritdoc/> |
| | 26749 | 43 | | public override Fixed64 ScaledRadius => _scaledRadius; |
| | | 44 | | |
| | | 45 | | /// <summary> |
| | | 46 | | /// Gets the cylinder's full physical length along <see cref="WorldAxis"/>. |
| | | 47 | | /// </summary> |
| | | 48 | | public Fixed64 Height { get; private set; } |
| | | 49 | | |
| | | 50 | | /// <summary> |
| | | 51 | | /// Gets the derived normalized world-space cylinder axis. Exact geometry |
| | | 52 | | /// remains authoritative in the collider's rigid frame. |
| | | 53 | | /// </summary> |
| | | 54 | | public Vector3d WorldAxis { get; private set; } = Vector3d.Up; |
| | | 55 | | |
| | | 56 | | /// <inheritdoc/> |
| | | 57 | | protected override void OnRadiusChanged() |
| | | 58 | | { |
| | 324 | 59 | | Fixed64 diameter = _radius * 2; |
| | 324 | 60 | | _size = new Vector3d(diameter, _size.Y, diameter); |
| | 324 | 61 | | } |
| | | 62 | | |
| | | 63 | | /// <inheritdoc/> |
| | | 64 | | protected override Vector3d NormalizeSize(Vector3d value) => |
| | 937 | 65 | | new(_radius * 2, value.Y, _radius * 2); |
| | | 66 | | |
| | | 67 | | internal override ExactMassWeight CalculateMassPropertyWeight() |
| | | 68 | | { |
| | 44 | 69 | | GetMassPropertyGeometry(out Fixed64 radius, out Fixed64 height); |
| | 44 | 70 | | return ExactMassWeight.FromProduct( |
| | 44 | 71 | | Fixed64.Pi, |
| | 44 | 72 | | radius, |
| | 44 | 73 | | radius, |
| | 44 | 74 | | height); |
| | | 75 | | } |
| | | 76 | | |
| | | 77 | | private void GetMassPropertyGeometry(out Fixed64 radius, out Fixed64 height) |
| | | 78 | | { |
| | 831 | 79 | | if (HasCommittedShape) |
| | | 80 | | { |
| | 822 | 81 | | radius = _scaledRadius; |
| | 822 | 82 | | height = Height; |
| | 822 | 83 | | return; |
| | | 84 | | } |
| | | 85 | | |
| | 9 | 86 | | GetCurrentShapeScales(out Vector3d ownerScale, out Vector3d partScale); |
| | 9 | 87 | | radius = FixedMath.Max( |
| | 9 | 88 | | ColliderScalePolicy.ScalePositive(Radius, ownerScale.X, partScale.X), |
| | 9 | 89 | | ColliderScalePolicy.ScalePositive(Radius, ownerScale.Z, partScale.Z)); |
| | 9 | 90 | | height = ColliderScalePolicy.ScalePositive(Size.Y, ownerScale.Y, partScale.Y); |
| | 9 | 91 | | } |
| | | 92 | | |
| | | 93 | | internal override ExactMassWeight CalculatePreparedMassPropertyWeight() => |
| | 5332 | 94 | | ExactMassWeight.FromProduct( |
| | 5332 | 95 | | Fixed64.Pi, |
| | 5332 | 96 | | _preparedRadius, |
| | 5332 | 97 | | _preparedRadius, |
| | 5332 | 98 | | _preparedHeight); |
| | | 99 | | |
| | | 100 | | internal void GetInertiaGeometry(out Signed192 transverse, out Signed192 axial) |
| | | 101 | | { |
| | 787 | 102 | | GetMassPropertyGeometry(out Fixed64 radius, out Fixed64 height); |
| | 787 | 103 | | Fixed64 twelfth = Fixed64.FromFraction(1, 12); |
| | 787 | 104 | | transverse = WideArithmetic.AddSigned192( |
| | 787 | 105 | | ExactMassProperties.InertiaGeometryTerm(twelfth * (Fixed64)3, radius, radius), |
| | 787 | 106 | | ExactMassProperties.InertiaGeometryTerm(twelfth, height, height)); |
| | 787 | 107 | | axial = ExactMassProperties.InertiaGeometryTerm(Fixed64.Half, radius, radius); |
| | 787 | 108 | | } |
| | | 109 | | |
| | | 110 | | internal override Fixed3x3 CalculateCenterOfMassInertiaTensor(Fixed64 mass) |
| | | 111 | | { |
| | 772 | 112 | | GetInertiaGeometry(out Signed192 transverse, out Signed192 axial); |
| | 772 | 113 | | Fixed64 inertiaXZ = ExactMassProperties.InertiaMoment(mass, transverse); |
| | 772 | 114 | | Fixed64 inertiaY = ExactMassProperties.InertiaMoment(mass, axial); |
| | | 115 | | |
| | 772 | 116 | | Fixed3x3 tensor = new( |
| | 772 | 117 | | inertiaXZ, Fixed64.Zero, Fixed64.Zero, |
| | 772 | 118 | | Fixed64.Zero, inertiaY, Fixed64.Zero, |
| | 772 | 119 | | Fixed64.Zero, Fixed64.Zero, inertiaXZ |
| | 772 | 120 | | ); |
| | 772 | 121 | | return tensor; |
| | | 122 | | } |
| | | 123 | | |
| | | 124 | | /// <inheritdoc/> |
| | | 125 | | public override bool ColliderOverlapsRay(RaycastSegmentWorker worker, ref SwiftList<Vector3d> outputIntersectionPoin |
| | | 126 | | { |
| | 11 | 127 | | return worker.CheckCylinderOverlaps(this, ref outputIntersectionPoints); |
| | | 128 | | } |
| | | 129 | | |
| | | 130 | | /// <inheritdoc/> |
| | | 131 | | public override Vector3d ClosestPointOnSurface(Vector3d other) |
| | | 132 | | { |
| | 9 | 133 | | FixedPointAnchor surfaceAnchor = |
| | 9 | 134 | | GetClosestSurfaceAnchor(other, out _); |
| | 9 | 135 | | if (!surfaceAnchor.TryGetPoint(out Vector3d surfacePoint)) |
| | | 136 | | { |
| | 1 | 137 | | throw new InvalidOperationException( |
| | 1 | 138 | | "The closest cylinder surface point is outside the representable coordinate domain."); |
| | | 139 | | } |
| | | 140 | | |
| | 8 | 141 | | return surfacePoint; |
| | | 142 | | } |
| | | 143 | | |
| | | 144 | | /// <inheritdoc/> |
| | | 145 | | public override Vector3d GetNormalAtPoint(Vector3d point) |
| | | 146 | | { |
| | 696 | 147 | | _ = GetClosestSurfaceAnchor( |
| | 696 | 148 | | point, |
| | 696 | 149 | | out Vector3d outwardNormal); |
| | 696 | 150 | | return outwardNormal; |
| | | 151 | | } |
| | | 152 | | |
| | | 153 | | internal override FixedPointAnchor GetClosestSurfaceAnchor( |
| | | 154 | | Vector3d point, |
| | | 155 | | out Vector3d normal) => |
| | 1247 | 156 | | GetClosestSurfaceAnchor(point, out normal, out _); |
| | | 157 | | |
| | | 158 | | internal override FixedPointAnchor GetClosestSurfaceAnchor( |
| | | 159 | | Vector3d point, out Vector3d normal, out bool contained) => |
| | 1415 | 160 | | WideFiniteAxisIntersection |
| | 1415 | 161 | | .GetClosestCenteredFiniteCylinderSurfaceAnchor( |
| | 1415 | 162 | | point, |
| | 1415 | 163 | | Center, |
| | 1415 | 164 | | Rotation, |
| | 1415 | 165 | | Vector3d.Up, |
| | 1415 | 166 | | Height, |
| | 1415 | 167 | | ScaledRadius, |
| | 1415 | 168 | | Vector3d.Right, |
| | 1415 | 169 | | out normal, |
| | 1415 | 170 | | out contained); |
| | | 171 | | |
| | | 172 | | private protected override void PrepareShape(in ColliderShapeSnapshot snapshot) |
| | | 173 | | { |
| | 7661 | 174 | | Fixed64 radiusX = ColliderScalePolicy.ScalePositive( |
| | 7661 | 175 | | snapshot.Radius, |
| | 7661 | 176 | | snapshot.OwnerScale.X, |
| | 7661 | 177 | | snapshot.PartScale.X); |
| | 7661 | 178 | | Fixed64 radiusZ = ColliderScalePolicy.ScalePositive( |
| | 7661 | 179 | | snapshot.Radius, |
| | 7661 | 180 | | snapshot.OwnerScale.Z, |
| | 7661 | 181 | | snapshot.PartScale.Z); |
| | 7661 | 182 | | _preparedRadius = FixedMath.Max(radiusX, radiusZ); |
| | 7661 | 183 | | _preparedHeight = ColliderScalePolicy.ScalePositive( |
| | 7661 | 184 | | snapshot.Size.Y, |
| | 7661 | 185 | | snapshot.OwnerScale.Y, |
| | 7661 | 186 | | snapshot.PartScale.Y); |
| | 7661 | 187 | | SetPreparedGeometryReach(HasCommittedShape && _preparedRadius == _scaledRadius && _preparedHeight == Height |
| | 7661 | 188 | | ? CanonicalCenteredProxyRadius : ColliderGeometryPolicy.FiniteAxis(_preparedRadius, _preparedHeight)); |
| | 7661 | 189 | | _preparedAxis = (snapshot.Rotation * Vector3d.Up).Normalized; |
| | 7661 | 190 | | _preparedArea = Fixed64.Two * Fixed64.Pi * _preparedRadius |
| | 7661 | 191 | | * (_preparedHeight + _preparedRadius); |
| | 7661 | 192 | | SetPreparedBounds(FixedBoundBox.FromCenteredFiniteCylinderClippedToDomain( |
| | 7661 | 193 | | snapshot.Center, |
| | 7661 | 194 | | snapshot.Rotation, |
| | 7661 | 195 | | Vector3d.Up, |
| | 7661 | 196 | | _preparedHeight, |
| | 7661 | 197 | | _preparedRadius)); |
| | 7661 | 198 | | } |
| | | 199 | | |
| | | 200 | | private protected override void PublishShape() |
| | | 201 | | { |
| | 7655 | 202 | | _scaledRadius = _preparedRadius; |
| | 7655 | 203 | | Height = _preparedHeight; |
| | 7655 | 204 | | WorldAxis = _preparedAxis; |
| | 7655 | 205 | | Area = _preparedArea; |
| | 7655 | 206 | | } |
| | | 207 | | |
| | | 208 | | /// <inheritdoc/> |
| | | 209 | | public override Fixed64 GetFrontalArea(Vector3d direction) |
| | | 210 | | { |
| | 3 | 211 | | Fixed64 directionMagnitude = direction.Magnitude; |
| | 3 | 212 | | if (directionMagnitude <= Fixed64.Epsilon) |
| | 1 | 213 | | return Area; |
| | | 214 | | |
| | 2 | 215 | | Vector3d normalizedDirection = direction / directionMagnitude; |
| | 2 | 216 | | Fixed64 axial = Rotation.Inverse() |
| | 2 | 217 | | .Rotate(normalizedDirection).Y.Abs(); |
| | 2 | 218 | | Fixed64 radialFactorSqr = Fixed64.One - axial * axial; |
| | 2 | 219 | | Fixed64 radialFactor = radialFactorSqr <= Fixed64.Zero ? Fixed64.Zero : FixedMath.Sqrt(radialFactorSqr); |
| | | 220 | | |
| | 2 | 221 | | return axial * Fixed64.Pi * ScaledRadiusSqr |
| | 2 | 222 | | + radialFactor * 2 * ScaledRadius * Height; |
| | | 223 | | } |
| | | 224 | | |
| | | 225 | | } |