| | | 1 | | //======================================================================= |
| | | 2 | | // LSCapsuleCollider2D.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 System.Runtime.CompilerServices; |
| | | 10 | | using Chronicler; |
| | | 11 | | using FixedMathSharp; |
| | | 12 | | using FixedMathSharp.Geometry; |
| | | 13 | | |
| | | 14 | | namespace Gravitas.Colliders; |
| | | 15 | | |
| | | 16 | | /// <summary> |
| | | 17 | | /// Pure 2D capsule collider whose local center segment follows local 2D Y. |
| | | 18 | | /// </summary> |
| | | 19 | | public sealed class LSCapsuleCollider2D : LSCollider2D |
| | | 20 | | { |
| | | 21 | | // Retain the fixed coefficient's rounding before bringing the cap moment |
| | | 22 | | // to the rectangle's common divisor of twelve. |
| | 1 | 23 | | private static readonly Fixed64 CapCrossFactor = |
| | 1 | 24 | | ((Fixed64)4 / ((Fixed64)3 * Fixed64.Pi)) * (Fixed64)12; |
| | | 25 | | |
| | | 26 | | private Fixed64 _radius; |
| | | 27 | | private Fixed64 _height; |
| | | 28 | | private Fixed64 _scaledRadius; |
| | | 29 | | private Fixed64 _axisLength; |
| | | 30 | | private Fixed64 _preparedRadius; |
| | | 31 | | private Fixed64 _preparedAxisLength; |
| | | 32 | | private Vector2d _preparedAxis; |
| | | 33 | | |
| | | 34 | | /// <summary>Creates a pure 2D capsule with authored dimensions.</summary> |
| | 982 | 35 | | public LSCapsuleCollider2D(Fixed64 radius, Fixed64 height) |
| | | 36 | | { |
| | 982 | 37 | | ValidateDimensions(radius, height); |
| | 981 | 38 | | _radius = radius; |
| | 981 | 39 | | _height = height; |
| | 981 | 40 | | MarkShapeDirty(); |
| | 981 | 41 | | } |
| | | 42 | | |
| | | 43 | | /// <summary>Creates a runtime capsule from an authored shape definition.</summary> |
| | 94 | 44 | | public LSCapsuleCollider2D(ColliderShapeDefinition2D definition) |
| | | 45 | | { |
| | 94 | 46 | | definition.EnsureKind(ColliderShapeDefinition2DKind.Capsule); |
| | 94 | 47 | | Material = definition.Material; |
| | 94 | 48 | | Fixed64 radius = definition.Radius; |
| | 94 | 49 | | Fixed64 height = definition.Size.Y; |
| | 94 | 50 | | ValidateDimensions(radius, height); |
| | 94 | 51 | | _radius = radius; |
| | 94 | 52 | | _height = height; |
| | 94 | 53 | | MarkShapeDirty(); |
| | 94 | 54 | | } |
| | | 55 | | |
| | | 56 | | /// <inheritdoc/> |
| | 44940 | 57 | | public override ColliderType2D Shape => ColliderType2D.Capsule; |
| | | 58 | | |
| | | 59 | | /// <summary> |
| | | 60 | | /// Gets or sets the unscaled semicircle radius. |
| | | 61 | | /// </summary> |
| | | 62 | | public Fixed64 Radius |
| | | 63 | | { |
| | | 64 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 167 | 65 | | get => _radius; |
| | | 66 | | set |
| | | 67 | | { |
| | 7 | 68 | | ValidateDimensions(value, _height); |
| | 5 | 69 | | if (_radius == value) |
| | 3 | 70 | | return; |
| | | 71 | | |
| | 2 | 72 | | ValidateGeometryMutation(radius: value); |
| | 2 | 73 | | _radius = value; |
| | 2 | 74 | | MarkShapeDirty(); |
| | 2 | 75 | | } |
| | | 76 | | } |
| | | 77 | | |
| | | 78 | | /// <summary> |
| | | 79 | | /// Gets or sets the unscaled full end-to-end capsule height. |
| | | 80 | | /// </summary> |
| | | 81 | | public Fixed64 Height |
| | | 82 | | { |
| | | 83 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 109 | 84 | | get => _height; |
| | | 85 | | set |
| | | 86 | | { |
| | 8 | 87 | | ValidateDimensions(_radius, value); |
| | 7 | 88 | | if (_height == value) |
| | 3 | 89 | | return; |
| | | 90 | | |
| | 4 | 91 | | ValidateGeometryMutation(height: value); |
| | 3 | 92 | | _height = value; |
| | 3 | 93 | | MarkShapeDirty(); |
| | 3 | 94 | | } |
| | | 95 | | } |
| | | 96 | | |
| | | 97 | | /// <summary> |
| | | 98 | | /// Gets the radius after compound/local scaling. |
| | | 99 | | /// </summary> |
| | | 100 | | public Fixed64 ScaledRadius |
| | | 101 | | { |
| | | 102 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 103 | | get |
| | | 104 | | { |
| | 20341 | 105 | | GetMassPropertyGeometry( |
| | 20341 | 106 | | out Fixed64 radius, |
| | 20341 | 107 | | out _); |
| | 20341 | 108 | | return radius; |
| | | 109 | | } |
| | | 110 | | } |
| | | 111 | | |
| | | 112 | | /// <summary> |
| | | 113 | | /// Gets the derived normalized world-space direction of the capsule's |
| | | 114 | | /// conceptual center axis. Exact geometry remains authoritative in the |
| | | 115 | | /// collider's planar rigid frame. |
| | | 116 | | /// </summary> |
| | | 117 | | public Vector2d WorldAxis { get; private set; } = Vector2d.Forward; |
| | | 118 | | |
| | | 119 | | /// <summary> |
| | | 120 | | /// Gets the full length of the conceptual center axis. |
| | | 121 | | /// </summary> |
| | | 122 | | public Fixed64 AxisLength |
| | | 123 | | { |
| | | 124 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 125 | | get |
| | | 126 | | { |
| | 39802 | 127 | | if (HasCommittedShape) |
| | 39800 | 128 | | return _axisLength; |
| | | 129 | | |
| | 2 | 130 | | GetMassPropertyGeometry( |
| | 2 | 131 | | out _, |
| | 2 | 132 | | out Fixed64 axisLength); |
| | 2 | 133 | | return axisLength; |
| | | 134 | | } |
| | | 135 | | } |
| | | 136 | | |
| | | 137 | | /// <inheritdoc/> |
| | | 138 | | public override bool ContainsPoint(Vector2d point) |
| | 847 | 139 | | => FixedSegment2d.ContainsPointInCenteredCapsule( |
| | 847 | 140 | | point, |
| | 847 | 141 | | Center, |
| | 847 | 142 | | Rotation, |
| | 847 | 143 | | AxisLength, |
| | 847 | 144 | | ScaledRadius, |
| | 847 | 145 | | Fixed64.Zero, |
| | 847 | 146 | | strict: false); |
| | | 147 | | |
| | | 148 | | /// <inheritdoc/> |
| | | 149 | | public override Vector2d GetClosestPoint(Vector2d point) |
| | | 150 | | { |
| | 4 | 151 | | Vector2d direction = FixedSegment2d.GetDirectionFromCenteredAxis( |
| | 4 | 152 | | point, |
| | 4 | 153 | | Center, |
| | 4 | 154 | | Rotation, |
| | 4 | 155 | | AxisLength); |
| | 4 | 156 | | if (direction == Vector2d.Zero) |
| | 3 | 157 | | direction = Rotate(Vector2d.Right, Rotation).Normalized; |
| | 4 | 158 | | if (FixedSegment2d.TryGetSurfacePointOnCenteredCapsule( |
| | 4 | 159 | | point, |
| | 4 | 160 | | Center, |
| | 4 | 161 | | Rotation, |
| | 4 | 162 | | AxisLength, |
| | 4 | 163 | | ScaledRadius, |
| | 4 | 164 | | direction, |
| | 4 | 165 | | out Vector2d surfacePoint)) |
| | 2 | 166 | | return surfacePoint; |
| | | 167 | | |
| | 2 | 168 | | throw new InvalidOperationException("The closest capsule surface point is outside the Fixed64 coordinate domain. |
| | | 169 | | } |
| | | 170 | | |
| | | 171 | | internal Vector2d GetNormalFromCenteredAxis(Vector2d point) |
| | | 172 | | { |
| | 1130 | 173 | | Vector2d direction = FixedSegment2d.GetDirectionFromCenteredAxis( |
| | 1130 | 174 | | point, |
| | 1130 | 175 | | Center, |
| | 1130 | 176 | | Rotation, |
| | 1130 | 177 | | AxisLength); |
| | 1130 | 178 | | return direction != Vector2d.Zero |
| | 1130 | 179 | | ? direction |
| | 1130 | 180 | | : Rotate(Vector2d.Right, Rotation).Normalized; |
| | | 181 | | } |
| | | 182 | | |
| | | 183 | | internal bool TryGetSurfacePointFromCenteredAxis( |
| | | 184 | | Vector2d point, |
| | | 185 | | Vector2d normal, |
| | | 186 | | out Vector2d surfacePoint) => |
| | 2 | 187 | | TryGetSurfacePoint(point, normal, out surfacePoint); |
| | | 188 | | |
| | | 189 | | /// <inheritdoc/> |
| | | 190 | | public override Vector2d GetSupportPoint(Vector2d direction) |
| | | 191 | | { |
| | 263 | 192 | | if (TryGetSupportPoint(direction, out Vector2d support)) |
| | 262 | 193 | | return support; |
| | | 194 | | |
| | 1 | 195 | | throw new InvalidOperationException("The capsule support point is outside the Fixed64 coordinate domain."); |
| | | 196 | | } |
| | | 197 | | |
| | | 198 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 199 | | internal bool TryGetSupportPoint(Vector2d direction, out Vector2d support) |
| | | 200 | | { |
| | 263 | 201 | | Vector2d supportDirection = direction == Vector2d.Zero |
| | 263 | 202 | | ? Rotate(Vector2d.Right, Rotation) |
| | 263 | 203 | | : direction; |
| | 263 | 204 | | return FixedSegment2d.TryGetCenteredCapsuleSupport( |
| | 263 | 205 | | Center, |
| | 263 | 206 | | Rotation, |
| | 263 | 207 | | AxisLength, |
| | 263 | 208 | | ScaledRadius, |
| | 263 | 209 | | supportDirection, |
| | 263 | 210 | | out support); |
| | | 211 | | } |
| | | 212 | | |
| | | 213 | | private bool TryGetSurfacePoint( |
| | | 214 | | Vector2d point, |
| | | 215 | | Vector2d worldNormal, |
| | | 216 | | out Vector2d surfacePoint) => |
| | 2 | 217 | | FixedSegment2d.TryGetSurfacePointOnCenteredCapsule( |
| | 2 | 218 | | point, |
| | 2 | 219 | | Center, |
| | 2 | 220 | | Rotation, |
| | 2 | 221 | | AxisLength, |
| | 2 | 222 | | ScaledRadius, |
| | 2 | 223 | | worldNormal, |
| | 2 | 224 | | out surfacePoint); |
| | | 225 | | |
| | | 226 | | internal override ExactMassWeight CalculateAreaForMassProperties() |
| | | 227 | | { |
| | 65 | 228 | | GetMassPropertyGeometry( |
| | 65 | 229 | | out Fixed64 radius, |
| | 65 | 230 | | out Fixed64 cylinderLength); |
| | 65 | 231 | | return GetRectangleWeight(radius, cylinderLength) |
| | 65 | 232 | | .Add(GetCircleWeight(radius)); |
| | | 233 | | } |
| | | 234 | | |
| | | 235 | | internal override ExactMassWeight CalculatePreparedAreaForMassProperties() => |
| | 364 | 236 | | GetRectangleWeight(_preparedRadius, _preparedAxisLength) |
| | 364 | 237 | | .Add(GetCircleWeight(_preparedRadius)); |
| | | 238 | | |
| | | 239 | | internal override Fixed64 CalculateCenterOfMassMoment(Fixed64 mass) |
| | | 240 | | { |
| | 4577 | 241 | | GetMassPropertyGeometry( |
| | 4577 | 242 | | out Fixed64 radius, |
| | 4577 | 243 | | out Fixed64 cylinderLength); |
| | 4577 | 244 | | return CalculateCenteredMoment(mass, radius, cylinderLength); |
| | | 245 | | } |
| | | 246 | | |
| | | 247 | | private protected override void PrepareShape(in ColliderShapeSnapshot2D snapshot) |
| | | 248 | | { |
| | 5657 | 249 | | GetScaledGeometry(_radius, _height, snapshot.OwnerScale, snapshot.PartScale, |
| | 5657 | 250 | | out _preparedRadius, out _preparedAxisLength); |
| | 5655 | 251 | | SetPreparedGeometryReach(ColliderGeometryPolicy.Capsule(_preparedRadius, _preparedAxisLength)); |
| | 5655 | 252 | | _preparedAxis = Rotate(Vector2d.Forward, snapshot.Rotation).Normalized; |
| | 5655 | 253 | | SetPreparedBounds(FixedBoundArea.FromCenteredRotatedCapsuleClippedToDomain( |
| | 5655 | 254 | | snapshot.Center, |
| | 5655 | 255 | | snapshot.Rotation, |
| | 5655 | 256 | | _preparedAxisLength, |
| | 5655 | 257 | | _preparedRadius)); |
| | 5655 | 258 | | } |
| | | 259 | | |
| | | 260 | | internal static void GetScaledGeometry(Fixed64 authoredRadius, Fixed64 authoredHeight, |
| | | 261 | | Vector2d ownerScale, Vector2d partScale, out Fixed64 radius, out Fixed64 axisLength) |
| | | 262 | | { |
| | 5668 | 263 | | Fixed64 radiusX = ColliderScalePolicy.ScalePositive( |
| | 5668 | 264 | | authoredRadius, |
| | 5668 | 265 | | ownerScale.X, |
| | 5668 | 266 | | partScale.X); |
| | 5668 | 267 | | Fixed64 radiusY = ColliderScalePolicy.ScalePositive( |
| | 5668 | 268 | | authoredRadius, |
| | 5668 | 269 | | ownerScale.Y, |
| | 5668 | 270 | | partScale.Y); |
| | 5668 | 271 | | radius = FixedMath.Max(radiusX, radiusY); |
| | 5668 | 272 | | ChronicleGuard.ThrowIfArgument( |
| | 5668 | 273 | | !HasValidScaledDimensions( |
| | 5668 | 274 | | authoredRadius, |
| | 5668 | 275 | | authoredHeight, |
| | 5668 | 276 | | ownerScale, |
| | 5668 | 277 | | partScale), |
| | 5668 | 278 | | nameof(authoredHeight), |
| | 5668 | 279 | | "Scaled 2D capsule height must be at least the capsule diameter."); |
| | 5666 | 280 | | ChronicleGuard.ThrowIfArgument( |
| | 5666 | 281 | | !Fixed64.TryMultiplySubtractClamped( |
| | 5666 | 282 | | authoredHeight, |
| | 5666 | 283 | | ownerScale.Y, |
| | 5666 | 284 | | partScale.Y, |
| | 5666 | 285 | | Fixed64.One, |
| | 5666 | 286 | | authoredRadius, |
| | 5666 | 287 | | Fixed64.Two, |
| | 5666 | 288 | | ownerScale.X, |
| | 5666 | 289 | | partScale.X, |
| | 5666 | 290 | | out Fixed64 axisLengthX) |
| | 5666 | 291 | | | !Fixed64.TryMultiplySubtractClamped( |
| | 5666 | 292 | | authoredHeight, |
| | 5666 | 293 | | ownerScale.Y, |
| | 5666 | 294 | | partScale.Y, |
| | 5666 | 295 | | Fixed64.One, |
| | 5666 | 296 | | authoredRadius, |
| | 5666 | 297 | | Fixed64.Two, |
| | 5666 | 298 | | ownerScale.Y, |
| | 5666 | 299 | | partScale.Y, |
| | 5666 | 300 | | out Fixed64 axisLengthY), |
| | 5666 | 301 | | nameof(authoredHeight), |
| | 5666 | 302 | | "Scaled 2D capsule center-axis length must be representable."); |
| | 5666 | 303 | | axisLength = FixedMath.Min(axisLengthX, axisLengthY); |
| | 5666 | 304 | | } |
| | | 305 | | |
| | | 306 | | private protected override void PublishShape() |
| | | 307 | | { |
| | 5655 | 308 | | _scaledRadius = _preparedRadius; |
| | 5655 | 309 | | _axisLength = _preparedAxisLength; |
| | 5655 | 310 | | WorldAxis = _preparedAxis; |
| | 5655 | 311 | | } |
| | | 312 | | |
| | | 313 | | internal void CopyPreparedReconfiguration(LSCapsuleCollider2D candidate) |
| | | 314 | | { |
| | 22 | 315 | | _radius = candidate._radius; |
| | 22 | 316 | | _height = candidate._height; |
| | 22 | 317 | | _preparedRadius = candidate._preparedRadius; |
| | 22 | 318 | | _preparedAxisLength = candidate._preparedAxisLength; |
| | 22 | 319 | | _preparedAxis = candidate._preparedAxis; |
| | 22 | 320 | | } |
| | | 321 | | |
| | | 322 | | /// <inheritdoc/> |
| | | 323 | | protected override void RecordShapeData(IChronicler chronicler) |
| | | 324 | | { |
| | 4 | 325 | | Fixed64 radius = _radius; |
| | 4 | 326 | | Fixed64 height = _height; |
| | 4 | 327 | | RecordValues.Look(chronicler, ref radius, "Radius", Fixed64.Half); |
| | 4 | 328 | | RecordValues.Look(chronicler, ref height, "Height", Fixed64.One); |
| | 4 | 329 | | if (chronicler.Mode == SerializationMode.Loading) |
| | | 330 | | { |
| | 2 | 331 | | ValidateDimensions(radius, height); |
| | 2 | 332 | | _radius = radius; |
| | 2 | 333 | | _height = height; |
| | | 334 | | } |
| | 4 | 335 | | } |
| | | 336 | | |
| | | 337 | | internal void GetMassPropertyGeometry( |
| | | 338 | | out Fixed64 radius, |
| | | 339 | | out Fixed64 axisLength) |
| | | 340 | | { |
| | 25006 | 341 | | if (HasCommittedShape) |
| | | 342 | | { |
| | 23912 | 343 | | radius = _scaledRadius; |
| | 23912 | 344 | | axisLength = _axisLength; |
| | 23912 | 345 | | return; |
| | | 346 | | } |
| | | 347 | | |
| | 1094 | 348 | | GetCurrentScaleFactors( |
| | 1094 | 349 | | out Vector2d ownerScale, |
| | 1094 | 350 | | out Vector2d partScale); |
| | 1094 | 351 | | Fixed64 radiusX = ColliderScalePolicy.Scale( |
| | 1094 | 352 | | _radius, |
| | 1094 | 353 | | ownerScale.X, |
| | 1094 | 354 | | partScale.X); |
| | 1094 | 355 | | Fixed64 radiusY = ColliderScalePolicy.Scale( |
| | 1094 | 356 | | _radius, |
| | 1094 | 357 | | ownerScale.Y, |
| | 1094 | 358 | | partScale.Y); |
| | 1094 | 359 | | radius = FixedMath.Max(radiusX, radiusY); |
| | 1094 | 360 | | ChronicleGuard.ThrowIfTrue( |
| | 1094 | 361 | | !HasValidScaledDimensions( |
| | 1094 | 362 | | _radius, |
| | 1094 | 363 | | _height, |
| | 1094 | 364 | | ownerScale, |
| | 1094 | 365 | | partScale), |
| | 1094 | 366 | | nameof(CalculateAreaForMassProperties), |
| | 1094 | 367 | | "Scaled 2D capsule height must be at least the capsule diameter."); |
| | 1094 | 368 | | bool representable = Fixed64.TryMultiplySubtractClamped( |
| | 1094 | 369 | | _height, |
| | 1094 | 370 | | ownerScale.Y, |
| | 1094 | 371 | | partScale.Y, |
| | 1094 | 372 | | Fixed64.One, |
| | 1094 | 373 | | _radius, |
| | 1094 | 374 | | Fixed64.Two, |
| | 1094 | 375 | | ownerScale.X, |
| | 1094 | 376 | | partScale.X, |
| | 1094 | 377 | | out Fixed64 axisLengthX) |
| | 1094 | 378 | | & Fixed64.TryMultiplySubtractClamped( |
| | 1094 | 379 | | _height, |
| | 1094 | 380 | | ownerScale.Y, |
| | 1094 | 381 | | partScale.Y, |
| | 1094 | 382 | | Fixed64.One, |
| | 1094 | 383 | | _radius, |
| | 1094 | 384 | | Fixed64.Two, |
| | 1094 | 385 | | ownerScale.Y, |
| | 1094 | 386 | | partScale.Y, |
| | 1094 | 387 | | out Fixed64 axisLengthY); |
| | 1094 | 388 | | ChronicleGuard.ThrowIfTrue( |
| | 1094 | 389 | | !representable, |
| | 1094 | 390 | | nameof(CalculateAreaForMassProperties), |
| | 1094 | 391 | | "Scaled 2D capsule center-axis length must be representable."); |
| | 1094 | 392 | | axisLength = FixedMath.Min(axisLengthX, axisLengthY); |
| | 1094 | 393 | | } |
| | | 394 | | |
| | | 395 | | private static Fixed64 CalculateCenteredMoment(Fixed64 mass, Fixed64 radius, Fixed64 cylinderLength) |
| | | 396 | | { |
| | 4577 | 397 | | if (cylinderLength <= Fixed64.Epsilon) |
| | 1335 | 398 | | return ExactMassProperties.InertiaMoment(mass, |
| | 1335 | 399 | | ExactMassProperties.InertiaGeometryTerm(Fixed64.Half, radius, radius)); |
| | | 400 | | |
| | 3242 | 401 | | if (radius == Fixed64.Zero) |
| | 2 | 402 | | return Fixed64.TryMultiplyDivide(mass, cylinderLength, cylinderLength, (Fixed64)12, out Fixed64 rodMoment) |
| | 2 | 403 | | ? rodMoment : Fixed64.MaxValue; |
| | | 404 | | |
| | | 405 | | // Cancelling the common radius leaves exact linear area weights. |
| | 3240 | 406 | | Signed192 rectangleWeight = Fixed64.GetExactRawProduct(Fixed64.Two, cylinderLength); |
| | 3240 | 407 | | Signed192 capWeight = Fixed64.GetExactRawProduct(Fixed64.Pi, radius); |
| | 3240 | 408 | | GetCenteredMomentGeometry(radius, cylinderLength, |
| | 3240 | 409 | | out Signed192 rectangleGeometry, out Signed192 capGeometry); |
| | 3240 | 410 | | return ExactMassProperties.InertiaWeightedMoment( |
| | 3240 | 411 | | mass, rectangleGeometry, rectangleWeight, capGeometry, capWeight, divisor: 12); |
| | | 412 | | } |
| | | 413 | | |
| | | 414 | | /// <summary>Gets exact Q96 rectangle and paired-cap geometry with common divisor twelve.</summary> |
| | | 415 | | internal static void GetCenteredMomentGeometry(Fixed64 radius, Fixed64 cylinderLength, |
| | | 416 | | out Signed192 rectangleGeometry, out Signed192 capGeometry) |
| | | 417 | | { |
| | 3253 | 418 | | rectangleGeometry = WideArithmetic.AddSigned192( |
| | 3253 | 419 | | ExactMassProperties.InertiaGeometryTerm((Fixed64)4, radius, radius), |
| | 3253 | 420 | | ExactMassProperties.InertiaGeometryTerm(Fixed64.One, cylinderLength, cylinderLength)); |
| | | 421 | | |
| | | 422 | | // Combining both caps cancels their centroid-square terms and retains |
| | | 423 | | // odd raw mass: Ic = mc*(r^2/2 + L^2/4 + 4Lr/(3*pi)). |
| | 3253 | 424 | | capGeometry = WideArithmetic.AddSigned192( |
| | 3253 | 425 | | ExactMassProperties.InertiaGeometryTerm((Fixed64)6, radius, radius), |
| | 3253 | 426 | | WideArithmetic.AddSigned192( |
| | 3253 | 427 | | ExactMassProperties.InertiaGeometryTerm((Fixed64)3, cylinderLength, cylinderLength), |
| | 3253 | 428 | | ExactMassProperties.InertiaGeometryTerm(CapCrossFactor, cylinderLength, radius))); |
| | 3253 | 429 | | } |
| | | 430 | | |
| | | 431 | | private static ExactMassWeight GetRectangleWeight( |
| | | 432 | | Fixed64 radius, |
| | | 433 | | Fixed64 cylinderLength) => |
| | 429 | 434 | | ExactMassWeight.FromProduct( |
| | 429 | 435 | | Fixed64.Two, |
| | 429 | 436 | | radius, |
| | 429 | 437 | | cylinderLength); |
| | | 438 | | |
| | | 439 | | private static ExactMassWeight GetCircleWeight(Fixed64 radius) => |
| | 429 | 440 | | ExactMassWeight.FromProduct( |
| | 429 | 441 | | Fixed64.Pi, |
| | 429 | 442 | | radius, |
| | 429 | 443 | | radius); |
| | | 444 | | |
| | | 445 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 446 | | private static void ValidateDimensions(Fixed64 radius, Fixed64 height) |
| | | 447 | | { |
| | 1093 | 448 | | ChronicleGuard.ThrowIfArgument(radius <= Fixed64.Zero, nameof(radius), "2D capsule radius must be greater than z |
| | 1092 | 449 | | ChronicleGuard.ThrowIfArgument( |
| | 1092 | 450 | | Fixed64.CompareProducts( |
| | 1092 | 451 | | height, |
| | 1092 | 452 | | Fixed64.One, |
| | 1092 | 453 | | radius, |
| | 1092 | 454 | | Fixed64.Two) < 0, |
| | 1092 | 455 | | nameof(height), |
| | 1092 | 456 | | "2D capsule height must be at least the capsule diameter."); |
| | 1089 | 457 | | } |
| | | 458 | | |
| | | 459 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 460 | | private static bool HasValidScaledDimensions( |
| | | 461 | | Fixed64 radius, |
| | | 462 | | Fixed64 height, |
| | | 463 | | Vector2d ownerScale, |
| | | 464 | | Vector2d partScale) => |
| | 6762 | 465 | | Fixed64.CompareProducts( |
| | 6762 | 466 | | height, |
| | 6762 | 467 | | ownerScale.Y, |
| | 6762 | 468 | | partScale.Y, |
| | 6762 | 469 | | Fixed64.One, |
| | 6762 | 470 | | radius, |
| | 6762 | 471 | | Fixed64.Two, |
| | 6762 | 472 | | ownerScale.X, |
| | 6762 | 473 | | partScale.X) >= 0 |
| | 6762 | 474 | | & Fixed64.CompareProducts( |
| | 6762 | 475 | | height, |
| | 6762 | 476 | | ownerScale.Y, |
| | 6762 | 477 | | partScale.Y, |
| | 6762 | 478 | | Fixed64.One, |
| | 6762 | 479 | | radius, |
| | 6762 | 480 | | Fixed64.Two, |
| | 6762 | 481 | | ownerScale.Y, |
| | 6762 | 482 | | partScale.Y) >= 0; |
| | | 483 | | } |