| | | 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 FixedMathSharp; |
| | | 9 | | using FixedMathSharp.Geometry; |
| | | 10 | | using Gravitas.Queries; |
| | | 11 | | using SwiftCollections; |
| | | 12 | | using System; |
| | | 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 | | { |
| | 567 | 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> |
| | 1084 | 26 | | public LSCylinderCollider() { } |
| | | 27 | | |
| | | 28 | | /// <summary>Creates a runtime cylinder from an authored shape definition.</summary> |
| | 25 | 29 | | public LSCylinderCollider(ColliderShapeDefinition definition) |
| | | 30 | | { |
| | 25 | 31 | | definition.EnsureKind(ColliderShapeDefinitionKind.Cylinder); |
| | 25 | 32 | | Material = definition.Material; |
| | 25 | 33 | | Radius = definition.Radius; |
| | 25 | 34 | | Size = definition.Size; |
| | 25 | 35 | | } |
| | | 36 | | |
| | | 37 | | /// <inheritdoc/> |
| | 673 | 38 | | public override ColliderType Shape => ColliderType.Cylinder; |
| | | 39 | | /// <inheritdoc/> |
| | 96 | 40 | | public override int Priority => ColliderSettings.GetPriority(Shape); |
| | | 41 | | |
| | | 42 | | /// <inheritdoc/> |
| | 8533 | 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 | | { |
| | 19 | 59 | | Fixed64 diameter = _radius * 2; |
| | 19 | 60 | | _size = new Vector3d(diameter, _size.Y, diameter); |
| | 19 | 61 | | } |
| | | 62 | | |
| | | 63 | | /// <inheritdoc/> |
| | | 64 | | protected override Vector3d NormalizeSize(Vector3d value) => |
| | 489 | 65 | | new(_radius * 2, value.Y, _radius * 2); |
| | | 66 | | |
| | | 67 | | internal override ExactMassWeight CalculateMassPropertyWeight() => |
| | 45 | 68 | | ExactMassWeight.FromProduct( |
| | 45 | 69 | | Fixed64.Pi, |
| | 45 | 70 | | ScaledRadius, |
| | 45 | 71 | | ScaledRadius, |
| | 45 | 72 | | Height); |
| | | 73 | | |
| | | 74 | | internal override ExactMassWeight CalculatePreparedMassPropertyWeight() => |
| | 60 | 75 | | ExactMassWeight.FromProduct( |
| | 60 | 76 | | Fixed64.Pi, |
| | 60 | 77 | | _preparedRadius, |
| | 60 | 78 | | _preparedRadius, |
| | 60 | 79 | | _preparedHeight); |
| | | 80 | | |
| | | 81 | | internal override Fixed3x3 CalculateCenterOfMassInertiaTensor(Fixed64 mass) |
| | | 82 | | { |
| | 503 | 83 | | Fixed64 radiusSqr = ScaledRadiusSqr; |
| | 503 | 84 | | Fixed64 heightSqr = Height * Height; |
| | 503 | 85 | | Fixed64 inertiaXZ = Fixed64.FromFraction(1, 12) * mass * ((3 * radiusSqr) + heightSqr); |
| | 503 | 86 | | Fixed64 inertiaY = Fixed64.Half * mass * radiusSqr; |
| | | 87 | | |
| | 503 | 88 | | Fixed3x3 tensor = new( |
| | 503 | 89 | | inertiaXZ, Fixed64.Zero, Fixed64.Zero, |
| | 503 | 90 | | Fixed64.Zero, inertiaY, Fixed64.Zero, |
| | 503 | 91 | | Fixed64.Zero, Fixed64.Zero, inertiaXZ |
| | 503 | 92 | | ); |
| | 503 | 93 | | return tensor; |
| | | 94 | | } |
| | | 95 | | |
| | | 96 | | /// <inheritdoc/> |
| | | 97 | | public override bool ColliderOverlapsRay(RaycastSegmentWorker worker, ref SwiftList<Vector3d> outputIntersectionPoin |
| | | 98 | | { |
| | 10 | 99 | | return worker.CheckCylinderOverlaps(this, ref outputIntersectionPoints); |
| | | 100 | | } |
| | | 101 | | |
| | | 102 | | /// <inheritdoc/> |
| | | 103 | | public override Vector3d ClosestPointOnSurface(Vector3d other) |
| | | 104 | | { |
| | 86 | 105 | | FixedPointAnchor surfaceAnchor = |
| | 86 | 106 | | GetClosestSurfaceAnchor(other, out _); |
| | 86 | 107 | | if (!surfaceAnchor.TryGetPoint(out Vector3d surfacePoint)) |
| | | 108 | | { |
| | 1 | 109 | | throw new InvalidOperationException( |
| | 1 | 110 | | "The closest cylinder surface point is outside the representable coordinate domain."); |
| | | 111 | | } |
| | | 112 | | |
| | 85 | 113 | | return surfacePoint; |
| | | 114 | | } |
| | | 115 | | |
| | | 116 | | /// <inheritdoc/> |
| | | 117 | | public override Vector3d GetNormalAtPoint(Vector3d point) |
| | | 118 | | { |
| | 29 | 119 | | _ = GetClosestSurfaceAnchor( |
| | 29 | 120 | | point, |
| | 29 | 121 | | out Vector3d outwardNormal); |
| | 29 | 122 | | return outwardNormal; |
| | | 123 | | } |
| | | 124 | | |
| | | 125 | | internal override FixedPointAnchor GetClosestSurfaceAnchor( |
| | | 126 | | Vector3d point, |
| | | 127 | | out Vector3d normal) => |
| | 329 | 128 | | WideFiniteAxisIntersection |
| | 329 | 129 | | .GetClosestCenteredFiniteCylinderSurfaceAnchor( |
| | 329 | 130 | | point, |
| | 329 | 131 | | Center, |
| | 329 | 132 | | Rotation, |
| | 329 | 133 | | Vector3d.Up, |
| | 329 | 134 | | Height, |
| | 329 | 135 | | ScaledRadius, |
| | 329 | 136 | | Vector3d.Right, |
| | 329 | 137 | | out normal, |
| | 329 | 138 | | out _); |
| | | 139 | | |
| | | 140 | | private protected override void PrepareShape(in ColliderShapeSnapshot snapshot) |
| | | 141 | | { |
| | 578 | 142 | | Fixed64 radiusX = ColliderScalePolicy.ScalePositive( |
| | 578 | 143 | | snapshot.Radius, |
| | 578 | 144 | | snapshot.OwnerScale.X, |
| | 578 | 145 | | snapshot.PartScale.X); |
| | 578 | 146 | | Fixed64 radiusZ = ColliderScalePolicy.ScalePositive( |
| | 578 | 147 | | snapshot.Radius, |
| | 578 | 148 | | snapshot.OwnerScale.Z, |
| | 578 | 149 | | snapshot.PartScale.Z); |
| | 578 | 150 | | _preparedRadius = FixedMath.Max(radiusX, radiusZ); |
| | 578 | 151 | | _preparedHeight = ColliderScalePolicy.ScalePositive( |
| | 578 | 152 | | snapshot.Size.Y, |
| | 578 | 153 | | snapshot.OwnerScale.Y, |
| | 578 | 154 | | snapshot.PartScale.Y); |
| | 578 | 155 | | _preparedAxis = (snapshot.Rotation * Vector3d.Up).Normalized; |
| | 578 | 156 | | _preparedArea = Fixed64.Two * Fixed64.Pi * _preparedRadius |
| | 578 | 157 | | * (_preparedHeight + _preparedRadius); |
| | 578 | 158 | | SetPreparedBounds(FixedBoundBox.FromCenteredFiniteCylinderClippedToDomain( |
| | 578 | 159 | | snapshot.Center, |
| | 578 | 160 | | snapshot.Rotation, |
| | 578 | 161 | | Vector3d.Up, |
| | 578 | 162 | | _preparedHeight, |
| | 578 | 163 | | _preparedRadius)); |
| | 578 | 164 | | } |
| | | 165 | | |
| | | 166 | | private protected override void PublishShape() |
| | | 167 | | { |
| | 578 | 168 | | _scaledRadius = _preparedRadius; |
| | 578 | 169 | | Height = _preparedHeight; |
| | 578 | 170 | | WorldAxis = _preparedAxis; |
| | 578 | 171 | | Area = _preparedArea; |
| | 578 | 172 | | } |
| | | 173 | | |
| | | 174 | | /// <inheritdoc/> |
| | | 175 | | public override Fixed64 GetFrontalArea(Vector3d direction) |
| | | 176 | | { |
| | 4 | 177 | | Fixed64 directionMagnitude = direction.Magnitude; |
| | 4 | 178 | | if (directionMagnitude <= Fixed64.Epsilon) |
| | 1 | 179 | | return Area; |
| | | 180 | | |
| | 3 | 181 | | Vector3d normalizedDirection = direction / directionMagnitude; |
| | 3 | 182 | | Fixed64 axial = Rotation.Inverse() |
| | 3 | 183 | | .Rotate(normalizedDirection).Y.Abs(); |
| | 3 | 184 | | Fixed64 radialFactorSqr = Fixed64.One - axial * axial; |
| | 3 | 185 | | Fixed64 radialFactor = radialFactorSqr <= Fixed64.Zero ? Fixed64.Zero : FixedMath.Sqrt(radialFactorSqr); |
| | | 186 | | |
| | 3 | 187 | | return axial * Fixed64.Pi * ScaledRadiusSqr |
| | 3 | 188 | | + radialFactor * 2 * ScaledRadius * Height; |
| | | 189 | | } |
| | | 190 | | |
| | | 191 | | } |