< Summary

Information
Class: Gravitas.SolidBody2D
Assembly: Gravitas
File(s): File 1: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/2D/Bodies/SolidBody2D.CenterOfMass.cs
File 2: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/2D/Bodies/SolidBody2D.ColliderReconfiguration.cs
File 3: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/2D/Bodies/SolidBody2D.ContinuousCollision.cs
File 4: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/2D/Bodies/SolidBody2D.ContinuousCollision.Dynamic.cs
File 5: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/2D/Bodies/SolidBody2D.ContinuousCollision.Helpers.cs
File 6: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/2D/Bodies/SolidBody2D.ContinuousCollision.Hits.cs
File 7: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/2D/Bodies/SolidBody2D.ContinuousCollision.Kinematic.cs
File 8: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/2D/Bodies/SolidBody2D.ContinuousCollision.Rotational.cs
File 9: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/2D/Bodies/SolidBody2D.ContinuousCollision.Rotational.Mixed.cs
File 10: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/2D/Bodies/SolidBody2D.ContinuousCollision.Rotational.Response.cs
File 11: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/2D/Bodies/SolidBody2D.cs
File 12: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/2D/Bodies/SolidBody2D.Grounding.cs
File 13: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/2D/Bodies/SolidBody2D.Motion.cs
File 14: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/2D/Bodies/SolidBody2D.ReplayHash.cs
File 15: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/2D/Bodies/SolidBody2D.Serialization.cs
Line coverage
100%
Covered lines: 4346
Uncovered lines: 0
Coverable lines: 4346
Total lines: 7190
Line coverage: 100%
Branch coverage
100%
Covered branches: 1528
Total branches: 1528
Branch coverage: 100%
Method coverage

Feature is only available for sponsors

Upgrade to PRO version

Metrics

MethodBranch coverage Crap Score Cyclomatic complexity Line coverage
File 1: get_LocalCenterOfMassOffset()100%11100%
File 1: set_LocalCenterOfMassOffset(...)100%66100%
File 1: get_WorldCenterOfMass()100%11100%
File 1: TryGetWorldCenterOfMass(...)100%11100%
File 1: TryGetOffsetFromCenterOfMass(...)100%11100%
File 1: GetCenterOfMassAnchor()100%11100%
File 1: ResetCenterOfMassFromCollider()100%11100%
File 2: TryReconfigureCollider(...)100%3434100%
File 2: CaptureSynchronizedStatePublication(...)100%22100%
File 3: get_ContinuousCollisionFrameStart()100%11100%
File 3: get_ContinuousCollisionFrameDisplacement()100%11100%
File 3: get_ContinuousCollisionFrameRotation()100%11100%
File 3: get_ContinuousCollisionFrameEnd()100%11100%
File 3: get_ContinuousCollisionFrameTargetRotation()100%11100%
File 3: get_ContinuousCollisionTrajectoryCount()100%11100%
File 3: GetContinuousCollisionTrajectorySegment(...)100%11100%
File 3: get_HasContinuousCollisionMotion()100%66100%
File 3: ShouldOwnContinuousCollisionMovingPair(...)100%1010100%
File 3: get_HasContinuousCollisionRotationalMotion()100%44100%
File 3: SampleContinuousCollisionPosition(...)100%22100%
File 3: SampleContinuousCollisionRotation(...)100%22100%
File 3: SampleContinuousCollisionLinearVelocity(...)100%22100%
File 3: SampleContinuousCollisionAngularVelocity(...)100%22100%
File 3: ResolveLegacyContinuousCollisionFrameStart()100%11100%
File 3: ResolveLegacyContinuousCollisionFrameDisplacement()100%11100%
File 3: EnsureContinuousCollisionFramePrepared(...)100%1212100%
File 3: PrepareMovableContinuousCollisionMotion()100%66100%
File 3: SetContinuousCollisionFrameSegment(...)100%11100%
File 3: AppendContinuousCollisionFrameSegment(...)100%11100%
File 3: AppendContinuousCollisionFrameSegment(...)100%44100%
File 3: ValidateContinuousCollisionPosition(...)100%44100%
File 3: CanAppendContinuousCollisionFrameSegment(...)100%44100%
File 3: TryResolveContinuousCollisionMotionBound(...)100%88100%
File 3: ResolveContinuousCollisionTrajectoryBounds(...)100%22100%
File 3: RemoveSupersededContinuousCollisionFrameSegments(...)100%88100%
File 3: ResolveContinuousCollisionSegment(...)100%11100%
File 3: GetContinuousCollisionTrajectorySegmentIndex(...)100%44100%
File 3: GetContinuousCollisionTrajectorySegmentRange(...)100%22100%
File 3: InvalidateContinuousCollisionFrame()100%11100%
File 4: ApplyContinuousCollisionHandoff(...)100%22100%
File 4: ApplyContinuousCollisionHandoff(...)100%22100%
File 4: ApplyContinuousCollisionHandoffState(...)100%22100%
File 4: ApplyContinuousCollisionHandoffStateReserved(...)100%1212100%
File 4: ValidateContinuousCollisionHandoffPosition(...)100%88100%
File 4: HasRemainingHandoffMotion(...)100%44100%
File 4: CanApplyContinuousCollisionHandoffState(...)100%22100%
File 4: TryConsumeContinuousCollisionHandoff(...)100%2626100%
File 4: DiscardContinuousCollisionHandoff()100%11100%
File 4: TryResolveContinuousCollision(...)100%11100%
File 4: TryResolveContinuousCollision(...)100%4444100%
File 4: TryApplyContinuousCollisionDynamicResponse(...)100%66100%
File 4: TryApplyContinuousCollisionDynamicResponse(...)100%2222100%
File 4: TryApplyContinuousCollisionMixed3DResponse(...)100%1818100%
File 4: ApplyContinuousCollisionSourceResponse(...)100%11100%
File 4: UpdateContinuousCollisionFrameTrajectory(...)100%11100%
File 4: ResolveContinuousCollisionFrameFraction(...)100%11100%
File 4: ResolveContinuousCollisionRestitution(...)100%22100%
File 4: ResolveContinuousCollisionRestitution(...)100%22100%
File 4: RemoveClosingContinuousCollisionVelocity(...)100%44100%
File 5: ShouldUseContinuousCollision(...)100%22100%
File 5: ResolveContinuousCollisionMode()100%1010100%
File 5: ResolveContinuousCollisionProxyRadius()100%11100%
File 5: ResolveMixedContinuousCollisionProxyRadius()100%11100%
File 5: IsValidContinuousCollisionHit(...)100%11100%
File 5: IsClosingContinuousCollisionHit(...)100%11100%
File 5: IsValidContinuousCollisionTarget(...)100%88100%
File 5: IsValidMixedContinuousCollisionHit(...)100%88100%
File 6: TryGetFirstContinuousCollisionHit(...)100%2222100%
File 6: TryGetFirstValidContinuousCollisionHit(...)100%2424100%
File 6: AdmitStaticSurfaceContinuousCollisionHit(...)100%88100%
File 6: TryGetFirstValidMixedContinuousCollisionHit(...)100%1212100%
File 6: UsesMixedStaticManifold(...)100%66100%
File 6: TryBuildStaticMixedContinuousCollisionManifold(...)100%11100%
File 6: TryApplyStaticMixedContinuousCollisionResponse(...)100%66100%
File 6: TryGetFirstDynamicContinuousCollisionHit(...)100%88100%
File 6: TryGetDynamicRelativeContinuousCollisionHit(...)100%2424100%
File 6: TranslateContactAnchor(...)100%11100%
File 6: TryGetFirstDynamicMixedContinuousCollisionHit(...)100%88100%
File 6: TryGetDynamicMixed3DContinuousCollisionHit(...)100%2828100%
File 6: IsEligibleDynamicContinuousCollisionTarget(...)100%88100%
File 6: IsEligibleDynamicMixed3DTarget(...)100%88100%
File 7: TryResolveKinematicContinuousCollision(...)100%1818100%
File 7: TryGetFirstKinematicStaticContinuousCollisionHit(...)100%1010100%
File 7: TryApplyKinematicDynamicContinuousCollisionPushes(...)100%88100%
File 7: GatherKinematicDynamic2DContinuousCollisionHits(...)100%88100%
File 7: GatherKinematicDynamicMixed3DContinuousCollisionHits(...)100%1212100%
File 7: ApplyKinematicContinuousCollisionHandoff(...)100%44100%
File 7: ApplyKinematicContinuousCollisionHandoff(...)100%66100%
File 8: get_HasRotationInvariantCollider()100%22100%
File 8: HasNearbyRotationalContinuousCollisionTarget(...)100%1010100%
File 8: IsRotatingContinuousCollisionTarget(...)100%66100%
File 8: IsRotatingContinuousCollisionTarget(...)100%44100%
File 8: ShouldUseRotationalContinuousCollisionArbiter(...)100%2424100%
File 8: TryResolveRotationalContinuousCollision(...)100%11100%
File 8: TryResolveRotationalContinuousCollision(...)100%8686100%
File 8: TryResolveKinematicRotationalContinuousCollision(...)100%11100%
File 8: GatherRotationalContinuousCollisionCandidates(...)100%1010100%
File 8: TryFindEarliestRotationalContinuousCollision(...)100%2222100%
File 8: TryFindEarliestRotationalContinuousCollisionAgainstTarget(...)100%2828100%
File 8: CanExcludeInvariantTangentialContact(...)100%1414100%
File 8: IsRotationalIntervalSeparated(...)100%22100%
File 8: TryGetCirclePairSeparationGap(...)100%44100%
File 8: TryGetCircleSeparationGap(...)100%1212100%
File 8: SampleRotationalContinuousPairPose(...)100%22100%
File 8: IsMovingRotationalContinuousCollisionTarget(...)100%1414100%
File 9: .ctor(...)100%11100%
File 9: ResolvedMixedSurfaceFamilies(...)100%66100%
File 9: SetMixedContinuousCollisionSamplePose(...)100%11100%
File 9: ResolveMixedContinuousCollisionTrajectoryBounds(...)100%11100%
File 9: .ctor(...)100%11100%
File 9: TryFindEarliestMixedRotationalContinuousCollision(...)100%2626100%
File 9: GatherMixedRotationalContinuousCollisionCandidates(...)100%22100%
File 9: ConsiderMixedRotationalContinuousCollisionCandidate(...)100%1414100%
File 9: TryFindEarliestMixedRotationalContinuousCollisionAgainstTarget(...)100%6262100%
File 9: SampleMixedRotationalContinuousPairPose(...)100%22100%
File 9: TryGetExactSphereCircleTranslationalContact(...)100%2626100%
File 9: BuildMixedRotationalContinuousContact(...)100%1414100%
File 9: IsValidMixedRotationalContinuousCollisionTarget(...)100%1212100%
File 9: IsMovingMixedRotationalContinuousCollisionTarget(...)100%1010100%
File 9: TryApplyMixedTranslationalManifoldResponse(...)100%66100%
File 9: TryApplyMixedRotationalContinuousCollisionResponse(...)100%5252100%
File 10: TryApplyRotationalContinuousCollisionResponse(...)100%4646100%
File 10: ResolveRotationalFrameFraction(...)100%11100%
File 10: StopRotationalContinuousCollision(...)100%11100%
File 11: .ctor(...)100%11100%
File 11: get_FreezeAxes()100%11100%
File 11: set_FreezeAxes(...)100%44100%
File 11: get_IsPositionFullyFrozen()100%11100%
File 11: get_MotionType()100%11100%
File 11: get_IsDynamic()100%11100%
File 11: get_IsStatic()100%11100%
File 11: get_IsKinematic()100%11100%
File 11: SetMotionType(...)100%22100%
File 11: PrepareMotionTypeTransition(...)100%44100%
File 11: CommitMotionTypeTransition(...)100%11100%
File 11: ApplyLoadedMotionType(...)100%44100%
File 11: PreflightLoadedMotionType(...)100%22100%
File 11: ThrowIfRuntimeRegistrationMissing()100%22100%
File 11: ReconcileMotionTypeRegistration(...)100%11100%
File 11: ClearMotionForMotionTypeTransition()100%11100%
File 11: get_ContinuousCollisionMode()100%11100%
File 11: set_ContinuousCollisionMode(...)100%11100%
File 11: get_IsRotationFullyFrozen()100%11100%
File 11: get_Mass()100%11100%
File 11: set_Mass(...)100%44100%
File 11: InvalidateMassDependentSolverState()100%11100%
File 11: RefreshPartitionAwakeState()100%11100%
File 11: get_InverseMass()100%22100%
File 11: get_CanTranslate()100%88100%
File 11: get_CanRotate()100%88100%
File 11: get_HasSolverMobility()100%22100%
File 11: get_EffectiveInverseMass()100%22100%
File 11: get_EffectiveInverseMomentOfInertia()100%22100%
File 11: ProjectLinearMotion(...)100%88100%
File 11: ProjectLinearEndpoint(...)100%44100%
File 11: GetConstrainedInverseMass(...)100%88100%
File 11: get_MomentOfInertia()100%11100%
File 11: get_InverseMomentOfInertia()100%11100%
File 11: get_AngularVelocity()100%11100%
File 11: get_AngularAcceleration()100%11100%
File 11: get_AngularSpeed()100%11100%
File 11: get_Position()100%11100%
File 11: get_Rotation()100%11100%
File 11: get_LinearVelocity()100%11100%
File 11: get_LinearSpeed()100%11100%
File 11: get_Gravity()100%11100%
File 11: set_Gravity(...)100%44100%
File 11: get_GravityScale()100%11100%
File 11: set_GravityScale(...)100%11100%
File 11: get_SleepEnabled()100%11100%
File 11: set_SleepEnabled(...)100%44100%
File 11: get_SleepFrameThreshold()100%11100%
File 11: set_SleepFrameThreshold(...)100%11100%
File 11: get_SleepLinearSpeedThreshold()100%11100%
File 11: set_SleepLinearSpeedThreshold(...)100%11100%
File 11: get_SleepAngularSpeedThreshold()100%11100%
File 11: set_SleepAngularSpeedThreshold(...)100%11100%
File 11: get_IsSleeping()100%11100%
File 11: get_IsAwakeForCollision()100%22100%
File 11: Initialize(...)100%44100%
File 11: GetWorldPoint(...)100%11100%
File 11: TryGetWorldPoint(...)100%22100%
File 11: GetLocalPoint(...)100%11100%
File 11: TryGetLocalPoint(...)100%22100%
File 11: ResetPosition(...)100%44100%
File 11: Sleep()100%22100%
File 11: Wake()100%22100%
File 11: WakeFromCollision()100%22100%
File 11: LateSimulate()100%11100%
File 11: LateSimulate(...)100%2020100%
File 11: OnVisualize()100%22100%
File 11: UpdateKinematicPositionAndRotation(...)100%1212100%
File 11: CanonicalizeRotation(...)100%11100%
File 11: ReadHostRotation(...)100%66100%
File 11: GetHostYaw(...)100%22100%
File 11: SetHostWorldPose(...)100%11100%
File 11: PublishAuthoritativePose()100%11100%
File 11: Deactivate()100%22100%
File 12: .cctor()100%11100%
File 12: .ctor(...)100%11100%
File 12: get_GroundingMode()100%11100%
File 12: get_GroundProbeMode()100%11100%
File 12: set_GroundProbeMode(...)100%22100%
File 12: get_UseGravityDerivedGroundUpDirection()100%11100%
File 12: set_UseGravityDerivedGroundUpDirection(...)100%44100%
File 12: get_GroundUpDirection()100%11100%
File 12: set_GroundUpDirection(...)100%44100%
File 12: get_GroundProbeRadius()100%11100%
File 12: set_GroundProbeRadius(...)100%22100%
File 12: get_GroundedDistanceRay()100%11100%
File 12: set_GroundedDistanceRay(...)100%22100%
File 12: get_GroundMinNormalDot()100%11100%
File 12: set_GroundMinNormalDot(...)100%22100%
File 12: get_IsGrounded()100%11100%
File 12: set_IsGrounded(...)100%44100%
File 12: get_WasGrounded()100%11100%
File 12: get_GroundNormal()100%11100%
File 12: get_HasGroundPoint()100%11100%
File 12: get_GroundPoint()100%22100%
File 12: TryGetGroundPoint(...)100%22100%
File 12: get_LastGroundedPosition()100%11100%
File 12: UseManualGrounding(...)100%22100%
File 12: UseAutomaticGrounding(...)100%44100%
File 12: SetManualGrounding(...)100%11100%
File 12: ClearManualGrounding()100%11100%
File 12: CheckGround()100%22100%
File 12: BeginAutomaticGroundingRefresh()100%22100%
File 12: TryAcceptContactGroundCandidate(...)100%1818100%
File 12: CompleteAutomaticGroundingRefresh()100%1010100%
File 12: CheckGroundForSimulation()100%11100%
File 12: RefreshAutomaticGroundSupportBeforeMotion()100%88100%
File 12: HasCurrentGroundCollider()100%88100%
File 12: WakeForContactChange()100%44100%
File 12: RemoveIntoGroundComponent(...)100%1212100%
File 12: get_CanUseAutomaticGrounding()100%44100%
File 12: CheckGround(...)100%2424100%
File 12: TryFindGroundHit(...)100%44100%
File 12: IsValidGroundHit(...)100%44100%
File 12: IsValidGroundCollider(...)100%1010100%
File 12: ResolveGroundProbeMode()100%44100%
File 12: ResolveGroundProbeRadius()100%1212100%
File 12: ValidateGroundMinNormalDot(...)100%22100%
File 12: IsValidGroundNormal(...)100%22100%
File 12: InvalidateAutomaticGroundSupport()100%22100%
File 12: ResolveGroundUpDirection()100%44100%
File 12: CaptureGroundedTransitionState()100%11100%
File 12: CaptureGroundedStepState()100%22100%
File 12: CompleteGroundedStepState()100%11100%
File 12: ClearGrounding()100%44100%
File 12: ClearGroundingForAutomaticRefresh()100%22100%
File 12: SetGroundingState(...)100%1616100%
File 12: ResetGroundingForInitialize(...)100%11100%
File 12: RevalidateAutomaticGroundingWrite(...)100%1818100%
File 12: ClearGroundContactCandidate()100%11100%
File 12: CompareGroundContactCandidate(...)100%1616100%
File 13: AddForce(...)100%22100%
File 13: AddTorque(...)100%22100%
File 13: AddLinearImpulse(...)100%22100%
File 13: AddAngularImpulse(...)100%22100%
File 13: SetPosition(...)100%22100%
File 13: SetRotation(...)100%22100%
File 13: RefreshStaticColliderAfterExplicitPoseChange()100%66100%
File 13: PreflightStaticPoseChange()100%44100%
File 13: RefreshPartitionMobility()100%66100%
File 13: ApplyCollisionLinearVelocityDelta(...)100%44100%
File 13: ApplyCollisionAngularVelocityDelta(...)100%44100%
File 13: CanApplyCollisionVelocityDeltas(...)100%11100%
File 13: PrepareCollisionPositionCorrection(...)100%22100%
File 13: ApplyCollisionPositionState(...)100%22100%
File 13: PublishCollisionPositionState()100%44100%
File 13: AddPositionDisplacement(...)100%22100%
File 13: get_CanSleep()100%22100%
File 13: ApplyFreezeConstraintsToMotion()100%22100%
File 13: UpdateSleepState()100%22100%
File 13: get_CanCountTowardSleep()100%88100%
File 13: get_ReadyForAutomaticSleep()100%22100%
File 13: AdvanceSleepCounter()100%66100%
File 13: RefreshLinearSpeed()100%22100%
File 13: RefreshAngularSpeed()100%22100%
File 13: RefreshMassPropertiesFromColliderShape()100%22100%
File 13: RefreshMomentOfInertia(...)100%44100%
File 14: ContributeReplayHash(...)100%88100%
File 14: WriteContinuousCollisionTrajectory(...)100%22100%
File 15: RecordData(...)100%2222100%
File 15: ApplyLoadedState()100%11100%
File 15: ApplyLoadedFreezeConstraintsToMotion()100%22100%

File(s)

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/2D/Bodies/SolidBody2D.CenterOfMass.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody2D.CenterOfMass.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
 8using System;
 9using System.Runtime.CompilerServices;
 10using FixedMathSharp;
 11using Gravitas.CollisionHandling;
 12
 13namespace Gravitas;
 14
 15/// <content>Maintains the planar center-of-mass offset and resolves it in world space.</content>
 16public sealed partial class SolidBody2D
 17{
 18    /// <summary>
 19    /// Gets or sets the authoritative body-local center-of-mass offset in the X/Z simulation plane.
 20    /// </summary>
 21    public Vector2d LocalCenterOfMassOffset
 22    {
 684923        get => _localCenterOfMassOffset;
 24        set
 25        {
 2626            if (_localCenterOfMassOffset == value && _centerOfMassOffsetExplicit)
 127                return;
 28
 2529            _localCenterOfMassOffset = value;
 2530            _centerOfMassOffsetExplicit = true;
 2531            if (!Active)
 232                return;
 33
 2334            Wake();
 2335            RefreshMassPropertiesFromColliderShape();
 2336        }
 37    }
 38
 39    /// <summary>
 40    /// Gets the authoritative world-space center of mass in the X/Z simulation plane.
 41    /// </summary>
 42    /// <exception cref="InvalidOperationException">
 43    /// The center lies outside the representable coordinate domain. Use
 44    /// <see cref="TryGetWorldCenterOfMass(out Vector2d)"/> when that is possible.
 45    /// </exception>
 46    public Vector2d WorldCenterOfMass
 47    {
 48        get
 49        {
 25650            bool succeeded = TryGetWorldCenterOfMass(out Vector2d center);
 25651            ChronicleGuard.ThrowIfTrue(
 25652                !succeeded,
 25653                nameof(WorldCenterOfMass),
 25654                "The world center of mass is outside the representable coordinate domain.");
 25555            return center;
 56        }
 57    }
 58
 59    /// <summary>
 60    /// Attempts to materialize the authoritative world-space center of mass
 61    /// without saturation.
 62    /// </summary>
 63    public bool TryGetWorldCenterOfMass(out Vector2d center)
 25964        => Vector2d.TryTransformPoint(
 25965            _position,
 25966            _localCenterOfMassOffset,
 25967            _rotation,
 25968            out center);
 69
 70    internal bool TryGetOffsetFromCenterOfMass(
 71        ContactAnchor2D anchor,
 72        out Vector2d offset)
 345773        => anchor.TryGetOffsetFrom(GetCenterOfMassAnchor(), out offset);
 74
 75    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 76    internal ContactAnchor2D GetCenterOfMassAnchor() =>
 2090977        new(
 2090978            _position,
 2090979            _rotation,
 2090980            _localCenterOfMassOffset);
 81
 82    /// <summary>
 83    /// Clears an explicit center-of-mass override and derives the offset from the bound collider again.
 84    /// </summary>
 85    public void ResetCenterOfMassFromCollider()
 86    {
 187        _centerOfMassOffsetExplicit = false;
 188        RefreshMassPropertiesFromColliderShape();
 189    }
 90}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/2D/Bodies/SolidBody2D.ColliderReconfiguration.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody2D.ColliderReconfiguration.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
 8using System;
 9using FixedMathSharp;
 10using Gravitas.Colliders;
 11using Gravitas.CollisionHandling;
 12using SwiftCollections;
 13
 14namespace Gravitas;
 15
 16/// <content>Owns transactional native 2D collider replacement.</content>
 17public sealed partial class SolidBody2D
 18{
 19    /// <summary>
 20    /// Attempts to publish replacement native 2D geometry and a body-root pose
 21    /// without replacing body, collider, or host identity.
 22    /// </summary>
 23    /// <remarks>
 24    /// Supports circle-to-circle and capsule-to-capsule replacement only in
 25    /// <see cref="PhysicsRuntimeMode.TwoD"/>. The definition contributes geometry;
 26    /// material, filters, hierarchy, events, rotation, motion, and host world Y
 27    /// are retained. Exact touching is allowed; any positive penetration,
 28    /// even below one raw unit, against an eligible registered collider rejects
 29    /// the change, including outside grid coverage. Triggers do not block it.
 30    /// This synchronous operation may scan the registry and allocate. It makes
 31    /// no work, timing, or allocation-failure recovery guarantee.
 32    /// The publisher runs once after physical publication or confirmation of
 33    /// unchanged state, and before pair separation. Rejection never invokes it.
 34    /// Notification failures do not roll back accepted state; pair retirement
 35    /// continues and failures are returned separately in stable order.
 36    /// </remarks>
 37    /// <param name="definition">The complete replacement geometry in the existing family.</param>
 38    /// <param name="localOffset">The unscaled collider offset from the body root.</param>
 39    /// <param name="position">The authoritative X/Z body-root position.</param>
 40    /// <param name="blocker">The first eligible penetrating collider in stable service order.</param>
 41    /// <param name="notificationException">Post-commit publisher or separation failures, aggregated when multiple.</par
 42    /// <param name="publishSynchronizedState">Optional once-only publication of already prepared dependent state.</para
 43    /// <returns>Whether the change was applied, already matched, or was blocked.</returns>
 44    /// <exception cref="ArgumentException">The definition, family, or candidate geometry is invalid.</exception>
 45    /// <exception cref="InvalidOperationException">The runtime mode, registration, fixed-step boundary, or host pose is
 46    public ColliderReconfigurationStatus TryReconfigureCollider(
 47        ColliderShapeDefinition2D definition, Vector2d localOffset, Vector2d position,
 48        out LSCollider2D? blocker, out Exception? notificationException,
 49        Action? publishSynchronizedState = null)
 50    {
 9451        blocker = null;
 9452        notificationException = null;
 9453        definition.EnsureDefined();
 9354        ThrowIfRuntimeRegistrationMissing();
 9155        Context.ThrowIfFixedStepMutationNotAllowed();
 9056        ChronicleGuard.ThrowIfTrue(
 9057            Context.Settings.RuntimeMode != PhysicsRuntimeMode.TwoD,
 9058            nameof(Context.Settings.RuntimeMode),
 9059            "Native 2D collider reconfiguration requires pure TwoD runtime mode.");
 8760        ColliderType2D expectedShape = definition.Kind switch
 8761        {
 4962            ColliderShapeDefinition2DKind.Circle => ColliderType2D.Circle,
 3763            ColliderShapeDefinition2DKind.Capsule => ColliderType2D.Capsule,
 164            _ => ColliderType2D.None
 8765        };
 8766        ChronicleGuard.ThrowIfArgument(
 8767            expectedShape == ColliderType2D.None || Collider.Shape != expectedShape,
 8768            nameof(definition),
 8769            "2D collider reconfiguration requires the existing circle or capsule family.");
 70
 8571        FixedTransform transform = Agent.Transform;
 8572        Vector3d worldPosition = new(position.X, transform.WorldPosition.Y, position.Y);
 8573        FixedTransform? parent = transform.Parent;
 8574        ChronicleGuard.ThrowIfTrue(
 8575            parent != null && (!parent.TryInverseTransformPoint(worldPosition, out Vector3d localPosition)
 8576                || !parent.TryTransformPoint(localPosition, out Vector3d roundTrip)
 8577                || !roundTrip.FuzzyEqualAbsolute(worldPosition, Fixed64.Epsilon)),
 8578            nameof(position),
 8579            "The host transform cannot represent the requested 2D body-root position.");
 80
 8481        LSCollider2D candidate = definition.CreateRuntimeCollider();
 8482        candidate.PrepareDetachedBodyReconfigurationCandidate(this, localOffset, position);
 8383        if (_position == position && Collider.MatchesCommittedReconfigurationCandidate(candidate))
 84        {
 185            SwiftList<Exception>? unchangedFailures = null;
 186            CaptureSynchronizedStatePublication(publishSynchronizedState, ref unchangedFailures);
 187            notificationException = CollisionNotificationExceptions.ToException(unchangedFailures);
 188            return ColliderReconfigurationStatus.Unchanged;
 89        }
 90
 91        // The grid-backed query index excludes unpartitioned colliders. A rare
 92        // explicit posture transaction must not omit those registered blockers.
 8293        int count = Context.Physics2D.ColliderCount;
 44094        for (int i = 0; i < count; i++)
 95        {
 16396            LSCollider2D other = Context.Physics2D.GetColliderByServiceIndex(i);
 16397            if (ReferenceEquals(other, Collider) || other.IsTrigger
 16398                || !Context.Physics2D.RequireCollisionPair(Collider, other)
 16399                || !CollisionDetection2D.DoesPostureCandidatePenetrate(candidate, other))
 100                continue;
 101
 25102            blocker = other;
 25103            return ColliderReconfigurationStatus.Blocked;
 104        }
 105
 57106        Vector2d centerOfMass = _centerOfMassOffsetExplicit
 57107            ? _localCenterOfMassOffset
 57108            : candidate.CalculateLocalCenterOfMassOffset();
 57109        Fixed64 moment = _mass > Fixed64.Zero
 57110            ? candidate.CalculateMomentOfInertia(_mass, centerOfMass)
 57111            : Fixed64.Zero;
 57112        Fixed64 inverseMoment = moment > Fixed64.Zero ? Fixed64.One / moment : Fixed64.Zero;
 113
 114        // All geometry and mass work has succeeded on detached state. There are
 115        // no host callbacks until every authoritative view has been published.
 57116        ChronicleGuard.ThrowIfTrue(!transform.TrySetWorldPosition(worldPosition), nameof(position),
 57117            "The host transform cannot represent the requested 2D body-root position.");
 57118        _position = position;
 57119        Collider.PublishBodyReconfigurationCandidate(candidate);
 57120        _localCenterOfMassOffset = centerOfMass;
 57121        _momentOfInertia = moment;
 57122        _inverseMomentOfInertia = inverseMoment;
 57123        Context.Constraints2D.ClearSolverCachesForBody(this);
 57124        Context.Physics2D.InvalidateContinuousCollisionStateForBodyMutation(this, DynamicId);
 57125        InvalidateContinuousCollisionFrame();
 57126        Context.Collisions2D.RefreshColliderPartitionAfterShapeChange(Collider);
 57127        Wake();
 128
 57129        var registration = new ColliderLifetimeToken2D(Collider);
 57130        SwiftList<Exception>? failures = null;
 57131        CaptureSynchronizedStatePublication(publishSynchronizedState, ref failures);
 57132        if (registration.IsCurrentLifetime)
 55133            Context.Physics2D.InvalidatePairsForColliderReconfiguration(Collider, ref failures);
 57134        notificationException = CollisionNotificationExceptions.ToException(failures);
 57135        return ColliderReconfigurationStatus.Applied;
 136    }
 137
 138    private static void CaptureSynchronizedStatePublication(
 139        Action? publisher, ref SwiftList<Exception>? failures)
 140    {
 58141        if (publisher == null)
 19142            return;
 143        try
 144        {
 39145            publisher();
 37146        }
 2147        catch (Exception exception)
 148        {
 2149            CollisionNotificationExceptions.Capture(ref failures, exception);
 2150        }
 39151    }
 152}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/2D/Bodies/SolidBody2D.ContinuousCollision.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody2D.ContinuousCollision.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
 8using System.Runtime.CompilerServices;
 9using FixedMathSharp;
 10using Gravitas.Colliders;
 11using Gravitas.CollisionHandling;
 12
 13namespace Gravitas;
 14
 15/// <content>Tracks planar motion segments and samples the body trajectory for continuous collision detection.</content>
 16public sealed partial class SolidBody2D
 17{
 18    private LSCollider? _continuousCollisionHandoffIgnoredCollider3D;
 19    private LSCollider2D? _continuousCollisionHandoffIgnoredCollider2D;
 20
 21    internal Vector2d ContinuousCollisionFrameStart
 22    {
 23        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 1564924        get => ResolveLegacyContinuousCollisionFrameStart();
 25    }
 26
 27    internal Vector2d ContinuousCollisionFrameDisplacement
 28    {
 29        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 430        get => ResolveLegacyContinuousCollisionFrameDisplacement();
 31    }
 32
 33    internal Fixed64 ContinuousCollisionFrameRotation
 34    {
 35        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 236        get => _continuousCollisionTrajectory[0].StartRotation;
 37    }
 38
 39    internal Vector2d ContinuousCollisionFrameEnd
 40    {
 41        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 1613542        get => _continuousCollisionTrajectory[_continuousCollisionTrajectory.Count - 1].EndPosition;
 43    }
 44
 45    internal Fixed64 ContinuousCollisionFrameTargetRotation
 46    {
 47        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 48        get
 49        {
 48150            ContinuousCollisionMotionSegment2D segment =
 48151                _continuousCollisionTrajectory[_continuousCollisionTrajectory.Count - 1];
 48152            return CanonicalizeRotation(segment.StartRotation + segment.AngularDelta);
 53        }
 54    }
 55
 56    internal int ContinuousCollisionTrajectoryCount =>
 78857        _continuousCollisionTrajectory.Count;
 58
 59    internal ContinuousCollisionMotionSegment2D GetContinuousCollisionTrajectorySegment(
 60        int index) =>
 62961        _continuousCollisionTrajectory[index];
 62
 63    internal bool HasContinuousCollisionMotion
 64    {
 65        get
 66        {
 93867            for (int i = 0; i < _continuousCollisionTrajectory.Count; i++)
 68            {
 44469                ContinuousCollisionMotionSegment2D segment =
 44470                    _continuousCollisionTrajectory[i];
 44471                if (segment.Displacement.MagnitudeSquared > Fixed64.Epsilon
 44472                    || segment.AngularDistance > Fixed64.Epsilon)
 73                {
 41974                    return true;
 75                }
 76            }
 77
 2578            return false;
 79        }
 80    }
 81
 82    internal bool ShouldOwnContinuousCollisionMovingPair(bool otherHasRotationalMotion)
 83    {
 160884        if (!IsKinematic || !ShouldUseContinuousCollision(out ContinuousCollisionMode mode))
 147085            return false;
 86
 13887        EnsureContinuousCollisionFramePrepared(Context.LateSimulateToken);
 13888        if (!HasContinuousCollisionMotion)
 189            return false;
 13790        if (mode == ContinuousCollisionMode.Continuous)
 13491            return true;
 92
 393        Fixed64 proxyRadius = ResolveContinuousCollisionProxyRadius();
 394        Vector2d displacement = ContinuousCollisionFrameDisplacement;
 95
 396        Fixed64 angularDistance = Fixed64.Zero;
 1297        for (int i = 0; i < _continuousCollisionTrajectory.Count; i++)
 398            angularDistance += _continuousCollisionTrajectory[i].AngularDistance;
 99
 3100        bool requiresTargetSampling = angularDistance <= Fixed64.Epsilon
 3101            && otherHasRotationalMotion;
 3102        return requiresTargetSampling
 3103            | angularDistance * proxyRadius > proxyRadius
 3104            | !ContinuousCollisionMath.IsWithinProxyRadius(
 3105                displacement,
 3106                displacement.MagnitudeSquared,
 3107                proxyRadius);
 108    }
 109
 110    internal bool HasContinuousCollisionRotationalMotion
 111    {
 112        get
 113        {
 7022114            for (int i = 0; i < _continuousCollisionTrajectory.Count; i++)
 115            {
 2102116                if (_continuousCollisionTrajectory[i].AngularDistance > Fixed64.Epsilon)
 258117                    return true;
 118            }
 119
 1409120            return false;
 121        }
 122    }
 123
 124    internal Vector2d SampleContinuousCollisionPosition(Fixed64 frameFraction) =>
 6350125        _continuousCollisionTrajectory.Count == 0
 6350126            ? _position
 6350127            : ResolveContinuousCollisionSegment(frameFraction).SamplePosition(frameFraction);
 128
 129    internal Fixed64 SampleContinuousCollisionRotation(Fixed64 frameFraction) =>
 7317130        _continuousCollisionTrajectory.Count == 0
 7317131            ? _rotation
 7317132            : CanonicalizeRotation(
 7317133                ResolveContinuousCollisionSegment(frameFraction).SampleRotation(frameFraction));
 134
 135    internal Vector2d SampleContinuousCollisionLinearVelocity(Fixed64 frameFraction)
 136    {
 403137        if (_continuousCollisionTrajectory.Count == 0)
 49138            return ProjectLinearMotion(_linearVelocity);
 139
 354140        ContinuousCollisionMotionSegment2D segment =
 354141            ResolveContinuousCollisionSegment(frameFraction);
 354142        Fixed64 duration = (segment.EndFraction - segment.StartFraction) * Context.DeltaTime;
 354143        return ProjectLinearMotion(segment.Displacement / duration);
 144    }
 145
 146    internal Fixed64 SampleContinuousCollisionAngularVelocity(Fixed64 frameFraction) =>
 390147        _continuousCollisionTrajectory.Count == 0
 390148            ? _angularVelocity
 390149            : ResolveContinuousCollisionSegment(frameFraction).AngularVelocity;
 150
 151    private Vector2d ResolveLegacyContinuousCollisionFrameStart()
 152    {
 15649153        ContinuousCollisionMotionSegment2D segment =
 15649154            _continuousCollisionTrajectory[_continuousCollisionTrajectory.Count - 1];
 15649155        Vector2d frameDisplacement = ResolveLegacyContinuousCollisionFrameDisplacement();
 15649156        return segment.StartPosition - frameDisplacement * segment.StartFraction;
 157    }
 158
 159    private Vector2d ResolveLegacyContinuousCollisionFrameDisplacement()
 160    {
 15653161        ContinuousCollisionMotionSegment2D segment =
 15653162            _continuousCollisionTrajectory[_continuousCollisionTrajectory.Count - 1];
 15653163        Fixed64 remainingFraction = Fixed64.One - segment.StartFraction;
 15653164        return segment.Displacement / remainingFraction;
 165    }
 166
 167    internal bool EnsureContinuousCollisionFramePrepared(long token, bool refreshGroundSupport = false)
 168    {
 32020169        if (_continuousCollisionFrameToken == token)
 16347170            return !refreshGroundSupport || RefreshAutomaticGroundSupportBeforeMotion();
 171
 172        // Publish pending authoring before inertia integration and CCD indexing;
 173        // rotational samples must not introduce a different geometry epoch.
 15673174        Collider.Rebuild(geometryOnly: true);
 175
 176        // Only the body and service owners opt into grounding callbacks;
 177        // target sampling remains a callback-free trajectory operation.
 15673178        if (refreshGroundSupport && !RefreshAutomaticGroundSupportBeforeMotion())
 4179            return false;
 180
 15669181        Vector2d startPosition = _position;
 15669182        Fixed64 startRotation = _rotation;
 183        Vector2d displacement;
 184        Vector2d endPosition;
 185        Fixed64 angularDelta;
 186        Fixed64 angularVelocity;
 15669187        if (IsKinematic)
 188        {
 520189            Vector2d targetPosition = ProjectLinearEndpoint(
 520190                startPosition,
 520191                Agent.Transform.WorldPositionXZ);
 520192            endPosition = targetPosition;
 520193            displacement = ShouldUseContinuousCollision(out _)
 520194                ? ContinuousCollisionSweepRange.ValidateEndpoint(
 520195                    startPosition,
 520196                    targetPosition,
 520197                    out _)
 520198                : targetPosition - startPosition;
 199
 519200            Fixed64 targetRotation = ReadHostRotation(Agent.Transform);
 519201            angularDelta = CanonicalizeRotation(targetRotation - startRotation);
 519202            angularVelocity = angularDelta / Context.DeltaTime;
 203        }
 204        else
 205        {
 15149206            PrepareMovableContinuousCollisionMotion();
 15149207            displacement = ProjectLinearMotion(_linearVelocity) * Context.DeltaTime;
 15149208            endPosition = AddPositionDisplacement(startPosition, displacement);
 15147209            angularVelocity = _angularVelocity;
 15147210            angularDelta = angularVelocity * Context.DeltaTime;
 211        }
 212
 15666213        _continuousCollisionFrameToken = token;
 15666214        SetContinuousCollisionFrameSegment(new ContinuousCollisionMotionSegment2D(
 15666215            Fixed64.Zero,
 15666216            Fixed64.One,
 15666217            startPosition,
 15666218            endPosition,
 15666219            displacement,
 15666220            startRotation,
 15666221            angularDelta,
 15666222            angularVelocity));
 15666223        return true;
 224    }
 225
 226    private void PrepareMovableContinuousCollisionMotion()
 227    {
 15149228        if (_isSleeping)
 1306229            return;
 230
 13843231        Fixed64 deltaTime = Context.DeltaTime;
 13843232        if (CanTranslate)
 233        {
 13812234            _linearAccelerationStore = RemoveIntoGroundComponent(
 13812235                _deltaAcceleration + Gravity * _gravityScale);
 13812236            _deltaAcceleration = Vector2d.Zero;
 13812237            _linearVelocity += ProjectLinearMotion(_linearAccelerationStore * deltaTime);
 13812238            _linearVelocity = RemoveIntoGroundComponent(_linearVelocity);
 13812239            _linearAccelerationStore = Vector2d.Zero;
 13812240            RefreshLinearSpeed();
 241        }
 242        else
 243        {
 31244            _linearAccelerationStore = Vector2d.Zero;
 31245            _deltaAcceleration = Vector2d.Zero;
 246        }
 247
 13843248        if (CanRotate)
 249        {
 5860250            _angularAccelerationStore = _deltaAngularAcceleration;
 5860251            _deltaAngularAcceleration = Fixed64.Zero;
 5860252            _angularVelocity += _angularAccelerationStore * deltaTime;
 5860253            RefreshAngularSpeed();
 254        }
 255        else
 256        {
 7983257            _angularAccelerationStore = Fixed64.Zero;
 7983258            _deltaAngularAcceleration = Fixed64.Zero;
 259        }
 7983260    }
 261
 262    private void SetContinuousCollisionFrameSegment(ContinuousCollisionMotionSegment2D segment)
 263    {
 15666264        _continuousCollisionTrajectory.FastClear();
 15666265        _continuousCollisionTrajectory.Add(segment);
 15666266    }
 267
 268    private void AppendContinuousCollisionFrameSegment(
 269        Vector2d positionAtElapsedTime,
 270        Vector2d velocity,
 271        Fixed64 elapsedTime)
 272    {
 441273        Fixed64 startFraction = FixedMath.Clamp01(elapsedTime / Context.DeltaTime);
 441274        Fixed64 rotationAtElapsedTime = SampleContinuousCollisionRotation(startFraction);
 441275        AppendContinuousCollisionFrameSegment(
 441276            positionAtElapsedTime,
 441277            rotationAtElapsedTime,
 441278            velocity,
 441279            _angularVelocity,
 441280            elapsedTime);
 441281    }
 282
 283    private void AppendContinuousCollisionFrameSegment(
 284        Vector2d positionAtElapsedTime,
 285        Fixed64 rotationAtElapsedTime,
 286        Vector2d velocity,
 287        Fixed64 angularVelocity,
 288        Fixed64 elapsedTime)
 289    {
 782290        Fixed64 deltaTime = Context.DeltaTime;
 782291        Fixed64 startFraction = FixedMath.Clamp01(elapsedTime / deltaTime);
 782292        Fixed64 remainingTime = FixedMath.Max(Fixed64.Zero, deltaTime - elapsedTime);
 782293        Vector2d displacement = ProjectLinearMotion(velocity) * remainingTime;
 782294        Vector2d endPosition = AddPositionDisplacement(positionAtElapsedTime, displacement);
 782295        _continuousCollisionFrameToken = Context.LateSimulateToken;
 782296        RemoveSupersededContinuousCollisionFrameSegments(
 782297            startFraction,
 782298            positionAtElapsedTime);
 782299        if (startFraction >= Fixed64.One && _continuousCollisionTrajectory.Count > 0)
 4300            return;
 301
 778302        _continuousCollisionTrajectory.Add(new ContinuousCollisionMotionSegment2D(
 778303            startFraction,
 778304            Fixed64.One,
 778305            positionAtElapsedTime,
 778306            endPosition,
 778307            displacement,
 778308            rotationAtElapsedTime,
 778309            angularVelocity * remainingTime,
 778310            angularVelocity));
 778311    }
 312
 313    // Step durations are at most one; keep the existing product rounding before checking the final coordinate.
 314    internal void ValidateContinuousCollisionPosition(Vector2d position, Vector2d velocity, Fixed64 remainingTime)
 315    {
 293316        velocity = ProjectLinearMotion(velocity);
 293317        if (velocity != _linearVelocity && velocity.Magnitude <= Fixed64.Epsilon)
 102318            velocity = Vector2d.Zero;
 293319        _ = AddPositionDisplacement(position, velocity * remainingTime);
 292320    }
 321
 322    internal bool CanAppendContinuousCollisionFrameSegment(Fixed64 elapsedTime)
 323    {
 1047324        Fixed64 startFraction = FixedMath.Clamp01(elapsedTime / Context.DeltaTime);
 1047325        int retainedCount = _continuousCollisionTrajectory.Count;
 1500326        while (retainedCount > 0
 1500327            && _continuousCollisionTrajectory[retainedCount - 1].StartFraction >= startFraction)
 328        {
 453329            retainedCount--;
 330        }
 331
 1047332        return retainedCount <= Context.Settings.ContinuousCollisionMaxToiIterations;
 333    }
 334
 335    internal bool TryResolveContinuousCollisionMotionBound(
 336        Fixed64 lowerFraction,
 337        Fixed64 upperFraction,
 338        Fixed64 pivotRadius,
 339        out Fixed64 motionBound)
 340    {
 5910341        motionBound = Fixed64.Zero;
 23748342        for (int i = 0; i < _continuousCollisionTrajectory.Count; i++)
 343        {
 5965344            ContinuousCollisionMotionSegment2D segment = _continuousCollisionTrajectory[i];
 5965345            Fixed64 lower = FixedMath.Max(lowerFraction, segment.StartFraction);
 5965346            Fixed64 upper = FixedMath.Min(upperFraction, segment.EndFraction);
 5965347            if (upper <= lower)
 348                continue;
 349
 5914350            if (!segment.TryResolveMotionBound(lower, upper, pivotRadius, out Fixed64 segmentBound)
 5914351                || !Fixed64.TryAdd(motionBound, segmentBound, out motionBound))
 352            {
 1353                motionBound = default;
 1354                return false;
 355            }
 356        }
 357
 5909358        return true;
 359    }
 360
 361    internal DynamicCcdPlanarBounds ResolveContinuousCollisionTrajectoryBounds(Fixed64 radius)
 362    {
 7094363        ContinuousCollisionMotionSegment2D first = _continuousCollisionTrajectory[0];
 7094364        Fixed64 minX = FixedMath.Min(first.StartPosition.X, first.EndPosition.X);
 7094365        Fixed64 minZ = FixedMath.Min(first.StartPosition.Y, first.EndPosition.Y);
 7094366        Fixed64 maxX = FixedMath.Max(first.StartPosition.X, first.EndPosition.X);
 7094367        Fixed64 maxZ = FixedMath.Max(first.StartPosition.Y, first.EndPosition.Y);
 15456368        for (int i = 1; i < _continuousCollisionTrajectory.Count; i++)
 369        {
 634370            ContinuousCollisionMotionSegment2D segment = _continuousCollisionTrajectory[i];
 634371            minX = FixedMath.Min(minX, FixedMath.Min(segment.StartPosition.X, segment.EndPosition.X));
 634372            minZ = FixedMath.Min(minZ, FixedMath.Min(segment.StartPosition.Y, segment.EndPosition.Y));
 634373            maxX = FixedMath.Max(maxX, FixedMath.Max(segment.StartPosition.X, segment.EndPosition.X));
 634374            maxZ = FixedMath.Max(maxZ, FixedMath.Max(segment.StartPosition.Y, segment.EndPosition.Y));
 375        }
 376
 7094377        return new DynamicCcdPlanarBounds(
 7094378            minX - radius,
 7094379            minZ - radius,
 7094380            maxX + radius,
 7094381            maxZ + radius);
 382    }
 383
 384    private void RemoveSupersededContinuousCollisionFrameSegments(
 385        Fixed64 startFraction,
 386        Vector2d positionAtElapsedTime)
 387    {
 1093388        while (_continuousCollisionTrajectory.Count > 0
 1093389            && _continuousCollisionTrajectory[_continuousCollisionTrajectory.Count - 1]
 1093390                .StartFraction >= startFraction)
 391        {
 311392            _continuousCollisionTrajectory.RemoveAt(
 311393                _continuousCollisionTrajectory.Count - 1);
 394        }
 395
 782396        if (_continuousCollisionTrajectory.Count == 0)
 295397            return;
 398
 487399        int predecessorIndex = _continuousCollisionTrajectory.Count - 1;
 487400        ContinuousCollisionMotionSegment2D predecessor =
 487401            _continuousCollisionTrajectory[predecessorIndex];
 487402        if (predecessor.EndFraction <= startFraction)
 25403            return;
 404
 462405        Fixed64 localFraction = (startFraction - predecessor.StartFraction)
 462406            / (predecessor.EndFraction - predecessor.StartFraction);
 462407        _continuousCollisionTrajectory[predecessorIndex] =
 462408            new ContinuousCollisionMotionSegment2D(
 462409                predecessor.StartFraction,
 462410                startFraction,
 462411                predecessor.StartPosition,
 462412                positionAtElapsedTime,
 462413                positionAtElapsedTime - predecessor.StartPosition,
 462414                predecessor.StartRotation,
 462415                predecessor.AngularDelta * localFraction,
 462416                predecessor.AngularVelocity);
 462417    }
 418
 419    private ContinuousCollisionMotionSegment2D ResolveContinuousCollisionSegment(
 420        Fixed64 frameFraction) =>
 14277421        _continuousCollisionTrajectory[
 14277422            GetContinuousCollisionTrajectorySegmentIndex(frameFraction)];
 423
 424    internal int GetContinuousCollisionTrajectorySegmentIndex(
 425        Fixed64 frameFraction)
 426    {
 29238427        for (int i = _continuousCollisionTrajectory.Count - 1; i > 0; i--)
 428        {
 554429            ContinuousCollisionMotionSegment2D segment =
 554430                _continuousCollisionTrajectory[i];
 554431            if (frameFraction >= segment.StartFraction)
 263432                return i;
 433        }
 434
 14065435        return 0;
 436    }
 437
 438    internal void GetContinuousCollisionTrajectorySegmentRange(
 439        Fixed64 startFraction,
 440        out int startIndex,
 441        out int endExclusive)
 442    {
 368443        startIndex = startFraction <= Fixed64.Zero
 368444            ? 0
 368445            : GetContinuousCollisionTrajectorySegmentIndex(startFraction);
 368446        endExclusive = _continuousCollisionTrajectory.Count;
 368447    }
 448
 449    private void InvalidateContinuousCollisionFrame()
 450    {
 8501451        _continuousCollisionFrameToken = -1;
 8501452        _continuousCollisionTrajectory.FastClear();
 8501453    }
 454}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/2D/Bodies/SolidBody2D.ContinuousCollision.Dynamic.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody2D.ContinuousCollision.Dynamic.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
 8using System.Runtime.CompilerServices;
 9using FixedMathSharp;
 10using FixedMathSharp.Geometry;
 11using Gravitas.Colliders;
 12using Gravitas.CollisionHandling;
 13using Gravitas.Materials;
 14
 15namespace Gravitas;
 16
 17/// <content>Resolves dynamic planar sweeps and applies same-plane and mixed collision handoffs.</content>
 18public sealed partial class SolidBody2D
 19{
 20    internal bool ApplyContinuousCollisionHandoff(
 21        Vector2d positionAtImpact,
 22        Vector2d velocityDelta,
 23        Fixed64 remainingTime,
 24        LSCollider? ignoredCollider3D = null,
 25        LSCollider2D? ignoredCollider2D = null)
 26    {
 5327        Fixed64 elapsedTime = Context.DeltaTime - remainingTime;
 5328        Fixed64 frameFraction = ResolveContinuousCollisionFrameFraction(elapsedTime);
 5329        Fixed64 rotationAtImpact = _continuousCollisionTrajectory.Count == 0
 5330            ? _rotation
 5331            : SampleContinuousCollisionRotation(frameFraction);
 5332        return ApplyContinuousCollisionHandoff(
 5333            positionAtImpact,
 5334            rotationAtImpact,
 5335            velocityDelta,
 5336            Fixed64.Zero,
 5337            remainingTime,
 5338            ignoredCollider3D,
 5339            ignoredCollider2D);
 40    }
 41
 42    internal bool ApplyContinuousCollisionHandoff(
 43        Vector2d positionAtImpact,
 44        Fixed64 rotationAtImpact,
 45        Vector2d linearVelocityDelta,
 46        Fixed64 angularVelocityDelta,
 47        Fixed64 remainingTime,
 48        LSCollider? ignoredCollider3D = null,
 49        LSCollider2D? ignoredCollider2D = null)
 50    {
 5651        Vector2d projectedLinearDelta = ProjectLinearMotion(linearVelocityDelta);
 5652        bool linearVelocityResolved = Vector2d.TryAdd(
 5653            _linearVelocity,
 5654            projectedLinearDelta,
 5655            out Vector2d postLinearVelocity);
 5656        bool angularVelocityResolved = Fixed64.TryAdd(
 5657            _angularVelocity,
 5658            angularVelocityDelta,
 5659            out Fixed64 postAngularVelocity);
 5660        if (!(linearVelocityResolved & angularVelocityResolved))
 61        {
 162            return false;
 63        }
 64
 5565        return ApplyContinuousCollisionHandoffState(
 5566            positionAtImpact,
 5567            rotationAtImpact,
 5568            postLinearVelocity,
 5569            postAngularVelocity,
 5570            remainingTime,
 5571            ignoredCollider3D,
 5572            ignoredCollider2D);
 73    }
 74
 75    internal bool ApplyContinuousCollisionHandoffState(
 76        Vector2d positionAtImpact,
 77        Fixed64 rotationAtImpact,
 78        Vector2d postLinearVelocity,
 79        Fixed64 postAngularVelocity,
 80        Fixed64 remainingTime,
 81        LSCollider? ignoredCollider3D = null,
 82        LSCollider2D? ignoredCollider2D = null)
 83    {
 11584        if (!CanApplyContinuousCollisionHandoffState(
 11585                positionAtImpact,
 11586                rotationAtImpact,
 11587                remainingTime,
 11588                out Vector2d resolvedPosition))
 89        {
 390            return false;
 91        }
 92
 11293        ValidateContinuousCollisionHandoffPosition(resolvedPosition, postLinearVelocity, postAngularVelocity, remainingT
 11094        _ = Context.Physics2D.TryReserveContinuousCollisionCandidateRefresh(this);
 95
 11096        ApplyContinuousCollisionHandoffStateReserved(
 11097            resolvedPosition,
 11098            rotationAtImpact,
 11099            postLinearVelocity,
 110100            postAngularVelocity,
 110101            remainingTime,
 110102            ignoredCollider3D,
 110103            ignoredCollider2D);
 110104        return true;
 105    }
 106
 107    internal void ApplyContinuousCollisionHandoffStateReserved(
 108        Vector2d resolvedPosition,
 109        Fixed64 rotationAtImpact,
 110        Vector2d postLinearVelocity,
 111        Fixed64 postAngularVelocity,
 112        Fixed64 remainingTime,
 113        LSCollider? ignoredCollider3D = null,
 114        LSCollider2D? ignoredCollider2D = null)
 115    {
 221116        _position = resolvedPosition;
 221117        _rotation = CanonicalizeRotation(rotationAtImpact);
 221118        Collider.PublishPreparedBodyPose();
 221119        postLinearVelocity = ProjectLinearMotion(postLinearVelocity);
 221120        postAngularVelocity = CanRotate ? postAngularVelocity : Fixed64.Zero;
 221121        if (_linearVelocity != postLinearVelocity || _angularVelocity != postAngularVelocity)
 196122            WakeFromCollision();
 221123        _linearVelocity = postLinearVelocity;
 221124        _angularVelocity = postAngularVelocity;
 221125        RefreshLinearSpeed();
 221126        RefreshAngularSpeed();
 221127        bool hasRemainingMotion = HasRemainingHandoffMotion(_linearVelocity, _angularVelocity, remainingTime);
 221128        AppendContinuousCollisionFrameSegment(
 221129            resolvedPosition,
 221130            _rotation,
 221131            hasRemainingMotion ? _linearVelocity : Vector2d.Zero,
 221132            hasRemainingMotion ? _angularVelocity : Fixed64.Zero,
 221133            Context.DeltaTime - remainingTime);
 221134        Context.Physics2D.RefreshContinuousCollisionCandidate(this);
 221135        if (!hasRemainingMotion)
 136        {
 31137            DiscardContinuousCollisionHandoff();
 31138            return;
 139        }
 140
 190141        _continuousCollisionHandoffToken = Context.LateSimulateToken;
 190142        _continuousCollisionHandoffRemainingTime = remainingTime;
 190143        _continuousCollisionHandoffIgnoredCollider3D = ignoredCollider3D;
 190144        _continuousCollisionHandoffIgnoredCollider2D = ignoredCollider2D;
 190145        _continuousCollisionHandoffPending = true;
 190146        Context.Physics2D.QueueContinuousCollisionHandoff(this);
 190147    }
 148
 149    internal void ValidateContinuousCollisionHandoffPosition(
 150        Vector2d position, Vector2d linearVelocity, Fixed64 angularVelocity, Fixed64 remainingTime)
 151    {
 227152        linearVelocity = ProjectLinearMotion(linearVelocity);
 227153        if (linearVelocity.Magnitude <= Fixed64.Epsilon)
 29154            linearVelocity = Vector2d.Zero;
 227155        angularVelocity = CanRotate && angularVelocity.Abs() > Fixed64.Epsilon ? angularVelocity : Fixed64.Zero;
 227156        if (HasRemainingHandoffMotion(linearVelocity, angularVelocity, remainingTime))
 196157            _ = AddPositionDisplacement(position, linearVelocity * remainingTime);
 221158    }
 159
 160    private static bool HasRemainingHandoffMotion(Vector2d linearVelocity, Fixed64 angularVelocity, Fixed64 remainingTim
 448161        remainingTime > Fixed64.Epsilon
 448162        && (linearVelocity.MagnitudeSquared > Fixed64.Epsilon || angularVelocity.Abs() > Fixed64.Epsilon);
 163
 164    internal bool CanApplyContinuousCollisionHandoffState(
 165        Vector2d positionAtImpact,
 166        Fixed64 rotationAtImpact,
 167        Fixed64 remainingTime,
 168        out Vector2d resolvedPosition)
 169    {
 237170        Fixed64 elapsedTime = Context.DeltaTime - remainingTime;
 237171        bool hasMobility = CanTranslate | CanRotate;
 237172        bool hasTrajectoryCapacity = CanAppendContinuousCollisionFrameSegment(elapsedTime);
 237173        bool positionDeltaResolved = Vector2d.TrySubtract(
 237174            positionAtImpact,
 237175            _position,
 237176            out Vector2d positionDelta);
 237177        bool positionResolved = Vector2d.TryAdd(
 237178            _position,
 237179            ProjectLinearMotion(positionDelta),
 237180            out resolvedPosition);
 237181        if (!(hasMobility
 237182                & hasTrajectoryCapacity
 237183                & positionDeltaResolved
 237184                & positionResolved))
 185        {
 8186            return false;
 187        }
 188
 229189        return Collider.TryPrepareBodyPose(
 229190            resolvedPosition,
 229191            CanonicalizeRotation(rotationAtImpact));
 192    }
 193
 194    internal bool TryConsumeContinuousCollisionHandoff(bool updateSleepState, bool updateColliderState)
 195    {
 15530196        if (!_continuousCollisionHandoffPending || _continuousCollisionHandoffToken != Context.LateSimulateToken)
 15451197            return false;
 198
 79199        Fixed64 remainingTime = _continuousCollisionHandoffRemainingTime;
 79200        _continuousCollisionHandoffPending = false;
 79201        _continuousCollisionHandoffRemainingTime = Fixed64.Zero;
 79202        bool hasLinearMotion = CanTranslate
 79203            && _linearVelocity.MagnitudeSquared > Fixed64.Epsilon;
 79204        bool hasAngularMotion = CanRotate
 79205            && _angularVelocity.Abs() > Fixed64.Epsilon;
 79206        if (!hasLinearMotion && !hasAngularMotion)
 207        {
 2208            _continuousCollisionHandoffIgnoredCollider3D = null;
 2209            _continuousCollisionHandoffIgnoredCollider2D = null;
 2210            if (updateColliderState)
 1211                Collider.Rebuild();
 2212            return true;
 213        }
 214
 77215        Vector2d startPosition = _position;
 77216        Vector2d proposedPosition = hasLinearMotion
 77217            ? AddPositionDisplacement(startPosition, ProjectLinearMotion(_linearVelocity) * remainingTime)
 77218            : startPosition;
 77219        Fixed64 startRotation = _rotation;
 77220        Fixed64 proposedRotation = hasAngularMotion
 77221            ? startRotation + _angularVelocity * remainingTime
 77222            : startRotation;
 77223        Fixed64 elapsedTime = FixedMath.Max(Fixed64.Zero, Context.DeltaTime - remainingTime);
 224        try
 225        {
 77226            if (ShouldUseRotationalContinuousCollisionArbiter(
 77227                    startPosition,
 77228                    proposedPosition,
 77229                    startRotation,
 77230                    proposedRotation,
 77231                    forceContinuous: true))
 232            {
 3233                TryResolveRotationalContinuousCollision(
 3234                    startPosition,
 3235                    ref proposedPosition,
 3236                    startRotation,
 3237                    ref proposedRotation,
 3238                    remainingTime,
 3239                    elapsedTime,
 3240                    forceContinuous: true);
 241            }
 74242            else if (hasLinearMotion)
 243            {
 73244                TryResolveContinuousCollision(
 73245                    startPosition,
 73246                    ref proposedPosition,
 73247                    remainingTime,
 73248                    elapsedTime,
 73249                    forceContinuous: true);
 250            }
 73251        }
 252        finally
 253        {
 77254            _continuousCollisionHandoffIgnoredCollider3D = null;
 77255            _continuousCollisionHandoffIgnoredCollider2D = null;
 77256        }
 76257        _position = proposedPosition;
 76258        _rotation = CanonicalizeRotation(proposedRotation);
 76259        if (updateColliderState)
 1260            Collider.Rebuild();
 261        else
 75262            Collider.RebuildRuntimeShapeOnly();
 263
 76264        if (updateSleepState)
 1265            UpdateSleepState();
 266
 76267        return true;
 268    }
 269
 270    internal void DiscardContinuousCollisionHandoff()
 271    {
 1510272        _continuousCollisionHandoffPending = false;
 1510273        _continuousCollisionHandoffRemainingTime = Fixed64.Zero;
 1510274        _continuousCollisionHandoffIgnoredCollider3D = null;
 1510275        _continuousCollisionHandoffIgnoredCollider2D = null;
 1510276    }
 277
 278    private bool TryResolveContinuousCollision(Vector2d startPosition, ref Vector2d proposedPosition) =>
 13597279        TryResolveContinuousCollision(
 13597280            startPosition,
 13597281            ref proposedPosition,
 13597282            Context.DeltaTime,
 13597283            Fixed64.Zero,
 13597284            forceContinuous: false);
 285
 286    private bool TryResolveContinuousCollision(
 287        Vector2d startPosition,
 288        ref Vector2d proposedPosition,
 289        Fixed64 initialRemainingTime,
 290        Fixed64 initialElapsedTime,
 291        bool forceContinuous)
 292    {
 13670293        LastContinuousCollisionToiIterationCount = 0;
 13670294        LastContinuousCollisionToiIterationLimitReached = false;
 295
 13670296        ContinuousCollisionMode mode = ContinuousCollisionMode.Continuous;
 13670297        if (!forceContinuous && !ShouldUseContinuousCollision(out mode))
 12831298            return false;
 299
 839300        Vector2d requestedDisplacement = _linearVelocity * initialRemainingTime;
 839301        Vector2d displacement = ContinuousCollisionSweepRange.ValidateEndpoint(
 839302            startPosition,
 839303            proposedPosition,
 839304            requestedDisplacement,
 839305            out _);
 306
 837307        Fixed64 displacementMagnitudeSquared = displacement.MagnitudeSquared;
 837308        if (displacementMagnitudeSquared <= Fixed64.Epsilon)
 137309            return false;
 310
 700311        Fixed64 proxyRadius = ResolveContinuousCollisionProxyRadius();
 700312        if (proxyRadius <= Fixed64.Epsilon
 700313            || (mode == ContinuousCollisionMode.Auto
 700314                && ContinuousCollisionMath.IsWithinProxyRadius(displacement, displacementMagnitudeSquared, proxyRadius))
 315        {
 2316            return false;
 317        }
 318
 698319        bool resolved = false;
 698320        int conservativeMixedRefinementCount = 0;
 698321        Vector2d currentPosition = startPosition;
 698322        Fixed64 remainingTime = initialRemainingTime;
 698323        Fixed64 elapsedTime = initialElapsedTime;
 698324        int maxToiIterations = Context.Settings.ContinuousCollisionMaxToiIterations;
 698325        LSCollider2D? originalIgnoredCollider2D = _continuousCollisionHandoffIgnoredCollider2D;
 698326        LSCollider? originalIgnoredCollider3D = _continuousCollisionHandoffIgnoredCollider3D;
 327        try
 328        {
 980329            while (LastContinuousCollisionToiIterationCount < maxToiIterations)
 330            {
 980331                Vector2d requestedSegmentDisplacement = ProjectLinearMotion(_linearVelocity) * remainingTime;
 980332                Vector2d requestedSegmentEnd = AddPositionDisplacement(currentPosition, requestedSegmentDisplacement);
 980333                Vector2d segmentDisplacement = ContinuousCollisionSweepRange.ValidateEndpoint(
 980334                    currentPosition,
 980335                    requestedSegmentEnd,
 980336                    requestedSegmentDisplacement,
 980337                    out Fixed64 segmentLength);
 980338                Vector2d segmentEnd = requestedSegmentEnd;
 339
 980340                Fixed64 elapsedFraction = elapsedTime / Context.DeltaTime;
 980341                if (!TryGetFirstContinuousCollisionHit(
 980342                        currentPosition,
 980343                        segmentEnd,
 980344                        proxyRadius,
 980345                        elapsedFraction,
 980346                        out Vector2d hitNormal,
 980347                        out Fixed64 hitDistance,
 980348                        out ContinuousCollisionTargetKind targetKind,
 980349                        out LSCollider2D? target2D,
 980350                        out LSCollider? target3D))
 351                {
 508352                    currentPosition = segmentEnd;
 508353                    break;
 354                }
 355
 472356                Fixed64 hitTime = FixedMath.Clamp01(hitDistance / segmentLength);
 472357                currentPosition = new FixedSegment2d(currentPosition, segmentEnd)
 472358                    .GetPointAtDistance(hitDistance, segmentLength);
 472359                Vector2d previousVelocity = _linearVelocity;
 472360                Fixed64 consumedTime = remainingTime * hitTime;
 472361                Fixed64 remainingAfterHit = remainingTime - consumedTime;
 472362                Fixed64 hitElapsedTime = elapsedTime + consumedTime;
 472363                if (targetKind
 472364                    == ContinuousCollisionTargetKind.UnresolvedMixed)
 365                {
 39366                    resolved = true;
 39367                    if (hitTime > Fixed64.Epsilon
 39368                        && conservativeMixedRefinementCount
 39369                            < ContinuousCollisionMath.RotationalIntervalMaxDepth)
 370                    {
 36371                        conservativeMixedRefinementCount++;
 36372                        remainingTime = remainingAfterHit;
 36373                        elapsedTime = hitElapsedTime;
 36374                        if (remainingTime > Fixed64.Epsilon)
 375                            continue;
 376                    }
 377
 3378                    LastContinuousCollisionToiIterationCount++;
 3379                    LastContinuousCollisionToiIterationLimitReached =
 3380                        remainingAfterHit > Fixed64.Epsilon;
 3381                    _ = Context.Physics2D
 3382                        .TryReserveContinuousCollisionCandidateRefresh(this);
 3383                    UpdateContinuousCollisionFrameTrajectory(
 3384                        currentPosition,
 3385                        Vector2d.Zero,
 3386                        hitElapsedTime);
 3387                    Context.Physics2D
 3388                        .RefreshContinuousCollisionCandidate(this);
 389
 3390                    if (LastContinuousCollisionToiIterationLimitReached)
 3391                        Context.Physics2D.ReportContinuousCollisionIterationLimit();
 392
 3393                    break;
 394                }
 395
 433396                if (!CanAppendContinuousCollisionFrameSegment(hitElapsedTime))
 397                {
 2398                    ContinuousCollisionMotionSegment2D activeSegment =
 2399                        ResolveContinuousCollisionSegment(
 2400                            ResolveContinuousCollisionFrameFraction(hitElapsedTime));
 2401                    currentPosition = activeSegment.StartPosition;
 2402                    elapsedTime = activeSegment.StartFraction * Context.DeltaTime;
 2403                    RemoveClosingContinuousCollisionVelocity(hitNormal);
 2404                    UpdateContinuousCollisionFrameTrajectory(
 2405                        currentPosition,
 2406                        _linearVelocity,
 2407                        elapsedTime);
 2408                    Context.Physics2D.RefreshContinuousCollisionCandidate(this);
 2409                    LastContinuousCollisionToiIterationCount++;
 2410                    LastContinuousCollisionToiIterationLimitReached = true;
 2411                    Context.Physics2D.ReportContinuousCollisionIterationLimit();
 2412                    resolved = true;
 2413                    break;
 414                }
 415
 431416                bool appliedResponse = TryApplyContinuousCollisionDynamicResponse(
 431417                        hitNormal,
 431418                        targetKind,
 431419                        target2D,
 431420                        target3D,
 431421                        currentPosition,
 431422                        hitElapsedTime,
 431423                        remainingAfterHit);
 431424                if (!appliedResponse)
 425                {
 207426                    RemoveClosingContinuousCollisionVelocity(hitNormal);
 207427                    UpdateContinuousCollisionFrameTrajectory(currentPosition, _linearVelocity, hitElapsedTime);
 428                }
 224429                else if (targetKind == ContinuousCollisionTargetKind.Dynamic2D)
 430                {
 37431                    _continuousCollisionHandoffIgnoredCollider2D = target2D;
 37432                    _continuousCollisionHandoffIgnoredCollider3D = null;
 433                }
 187434                else if (targetKind == ContinuousCollisionTargetKind.Dynamic3D)
 435                {
 15436                    _continuousCollisionHandoffIgnoredCollider2D = null;
 15437                    _continuousCollisionHandoffIgnoredCollider3D = target3D;
 438                }
 439
 431440                LastContinuousCollisionToiIterationCount++;
 431441                resolved = true;
 442
 431443                remainingTime = remainingAfterHit;
 431444                elapsedTime = hitElapsedTime;
 431445                if (remainingTime <= Fixed64.Epsilon || _linearVelocity.MagnitudeSquared <= Fixed64.Epsilon)
 446                    break;
 447
 255448                if (LastContinuousCollisionToiIterationCount >= maxToiIterations)
 449                {
 5450                    LastContinuousCollisionToiIterationLimitReached = true;
 5451                    break;
 452                }
 453
 250454                if (hitTime <= Fixed64.Epsilon && previousVelocity == _linearVelocity)
 455                    break;
 456            }
 457
 698458            proposedPosition = currentPosition;
 698459            return resolved;
 460        }
 461        finally
 462        {
 698463            _continuousCollisionHandoffIgnoredCollider2D = originalIgnoredCollider2D;
 698464            _continuousCollisionHandoffIgnoredCollider3D = originalIgnoredCollider3D;
 698465        }
 698466    }
 467
 468    private bool TryApplyContinuousCollisionDynamicResponse(
 469        Vector2d normalForSource,
 470        ContinuousCollisionTargetKind targetKind,
 471        LSCollider2D? target2D,
 472        LSCollider? target3D,
 473        Vector2d sourcePositionAtImpact,
 474        Fixed64 hitElapsedTime,
 475        Fixed64 remainingTime)
 476    {
 431477        if (targetKind == ContinuousCollisionTargetKind.Static3D)
 192478            return TryApplyStaticMixedContinuousCollisionResponse(target3D!, sourcePositionAtImpact,
 192479                hitElapsedTime, remainingTime);
 480
 239481        if (targetKind == ContinuousCollisionTargetKind.Dynamic2D)
 482        {
 38483            SolidBody2D targetBody = target2D!.Body!;
 38484            Fixed64 frameFraction = ResolveContinuousCollisionFrameFraction(hitElapsedTime);
 38485            Vector2d targetPositionAtImpact = targetBody.SampleContinuousCollisionPosition(
 38486                frameFraction);
 38487            return TryApplyContinuousCollisionDynamicResponse(
 38488                targetBody,
 38489                normalForSource,
 38490                sourcePositionAtImpact,
 38491                targetPositionAtImpact,
 38492                hitElapsedTime,
 38493                remainingTime);
 494        }
 495
 201496        if (targetKind == ContinuousCollisionTargetKind.Dynamic3D)
 497        {
 23498            SolidBody targetBody = target3D!.Body!;
 23499            Fixed64 frameFraction = ResolveContinuousCollisionFrameFraction(hitElapsedTime);
 23500            Vector3d targetPositionAtImpact = targetBody.SampleContinuousCollisionPosition(
 23501                frameFraction);
 23502            return TryApplyContinuousCollisionMixed3DResponse(
 23503                targetBody,
 23504                normalForSource,
 23505                sourcePositionAtImpact,
 23506                targetPositionAtImpact,
 23507                hitElapsedTime,
 23508                remainingTime);
 509        }
 510
 178511        return false;
 512    }
 513
 514    internal bool TryApplyContinuousCollisionDynamicResponse(
 515        SolidBody2D target,
 516        Vector2d normalForSource,
 517        Vector2d sourcePositionAtImpact,
 518        Vector2d targetPositionAtImpact,
 519        Fixed64 hitElapsedTime,
 520        Fixed64 remainingTime)
 521    {
 49522        if (!ContinuousCollisionImpulsePolicy.TryResolveImpactNormal(normalForSource, out Vector2d normal))
 1523            return false;
 524
 48525        Fixed64 constrainedInverseMassA = GetConstrainedInverseMass(normal);
 48526        Fixed64 constrainedInverseMassB = target.GetConstrainedInverseMass(normal);
 48527        Fixed64 frameFraction = ResolveContinuousCollisionFrameFraction(hitElapsedTime);
 48528        Vector2d targetLinearVelocity = target.SampleContinuousCollisionLinearVelocity(frameFraction);
 48529        bool inverseMassResolved = Fixed64.TryAdd(
 48530            constrainedInverseMassA,
 48531            constrainedInverseMassB,
 48532            out Fixed64 inverseMass);
 48533        bool relativeVelocityResolved = Vector2d.TrySubtract(
 48534            _linearVelocity,
 48535            targetLinearVelocity,
 48536            out Vector2d relativeVelocity);
 48537        if (!(inverseMassResolved & relativeVelocityResolved))
 538        {
 1539            return false;
 540        }
 541
 47542        Fixed64 normalVelocity = Vector2d.Dot(relativeVelocity, normal);
 47543        if (normalVelocity >= -Fixed64.Epsilon)
 1544            return false;
 545
 46546        Fixed64 restitution = ResolveContinuousCollisionRestitution(target, -normalVelocity);
 46547        bool responseFactorResolved = Fixed64.TryAdd(
 46548            Fixed64.One,
 46549            restitution,
 46550            out Fixed64 responseFactor);
 46551        bool closingSpeedResolved = Fixed64.TrySubtract(
 46552            Fixed64.Zero,
 46553            normalVelocity,
 46554            out Fixed64 closingSpeed);
 46555        bool responseSpeedResolved = Fixed64.TryMultiplyDivide(
 46556            closingSpeed,
 46557            responseFactor,
 46558            Fixed64.One,
 46559            out Fixed64 responseSpeed);
 46560        if (!(responseFactorResolved & closingSpeedResolved & responseSpeedResolved))
 561        {
 1562            return false;
 563        }
 564
 45565        Vector2d sourceResponseNormal = ProjectLinearMotion(normal);
 45566        Vector2d targetResponseNormal = target.ProjectLinearMotion(-normal);
 45567        bool sourceDeltaResolved = ContinuousCollisionImpulsePolicy.TryResolveVelocityDelta(
 45568            sourceResponseNormal,
 45569            responseSpeed,
 45570            EffectiveInverseMass,
 45571            inverseMass,
 45572            out Vector2d sourceVelocityDelta);
 45573        bool targetDeltaResolved = ContinuousCollisionImpulsePolicy.TryResolveVelocityDelta(
 45574            targetResponseNormal,
 45575            responseSpeed,
 45576            target.EffectiveInverseMass,
 45577            inverseMass,
 45578            out Vector2d targetVelocityDelta);
 45579        if (!(sourceDeltaResolved & targetDeltaResolved))
 580        {
 1581            return false;
 582        }
 583
 44584        bool sourceVelocityResolved = Vector2d.TryAdd(
 44585            _linearVelocity,
 44586            sourceVelocityDelta,
 44587            out Vector2d sourcePostLinearVelocity);
 44588        bool sourceTrajectoryAvailable = CanAppendContinuousCollisionFrameSegment(hitElapsedTime);
 44589        if (!(sourceVelocityResolved & sourceTrajectoryAvailable))
 590        {
 1591            return false;
 592        }
 593
 43594        Vector2d targetResolvedPosition = default;
 43595        Vector2d targetPostLinearVelocity = targetLinearVelocity;
 43596        Fixed64 targetRotationAtImpact = target.SampleContinuousCollisionRotation(frameFraction);
 43597        Fixed64 targetAngularVelocity = target.SampleContinuousCollisionAngularVelocity(frameFraction);
 43598        bool targetStateAvailable = true;
 43599        if (target.CanTranslate)
 600        {
 39601            bool targetVelocityResolved = Vector2d.TryAdd(
 39602                targetLinearVelocity,
 39603                targetVelocityDelta,
 39604                out targetPostLinearVelocity);
 39605            bool targetTrajectoryAvailable = target.CanApplyContinuousCollisionHandoffState(
 39606                targetPositionAtImpact,
 39607                targetRotationAtImpact,
 39608                remainingTime,
 39609                out targetResolvedPosition);
 39610            targetStateAvailable = targetVelocityResolved & targetTrajectoryAvailable;
 611        }
 612
 43613        if (!targetStateAvailable)
 614        {
 1615            return false;
 616        }
 617
 42618        ValidateContinuousCollisionPosition(sourcePositionAtImpact, sourcePostLinearVelocity, Context.DeltaTime - hitEla
 42619        if (target.CanTranslate)
 38620            target.ValidateContinuousCollisionHandoffPosition(targetResolvedPosition, targetPostLinearVelocity, targetAn
 621
 40622        _ = target.CanTranslate
 40623            ? Context.Physics2D.TryReserveContinuousCollisionCandidateRefresh(this, target)
 40624            : Context.Physics2D.TryReserveContinuousCollisionCandidateRefresh(this);
 625
 40626        ApplyContinuousCollisionSourceResponse(
 40627            sourcePositionAtImpact,
 40628            sourcePostLinearVelocity,
 40629            hitElapsedTime);
 40630        if (target.CanTranslate)
 631        {
 36632            target.ApplyContinuousCollisionHandoffStateReserved(
 36633                targetResolvedPosition,
 36634                targetRotationAtImpact,
 36635                targetPostLinearVelocity,
 36636                targetAngularVelocity,
 36637                remainingTime,
 36638                ignoredCollider2D: Collider);
 639        }
 640
 40641        return true;
 642    }
 643
 644    internal bool TryApplyContinuousCollisionMixed3DResponse(
 645        SolidBody target,
 646        Vector2d normalForSource,
 647        Vector2d sourcePositionAtImpact,
 648        Vector3d targetPositionAtImpact,
 649        Fixed64 hitElapsedTime,
 650        Fixed64 remainingTime)
 651    {
 32652        if (Collider is LSCompoundCollider2D || target.Collider is LSCompoundCollider or LSMeshCollider)
 6653            return TryApplyMixedTranslationalManifoldResponse(target, sourcePositionAtImpact,
 6654                targetPositionAtImpact, hitElapsedTime, remainingTime);
 26655        bool normalResolved = ContinuousCollisionImpulsePolicy.TryResolveImpactNormal(
 26656            normalForSource,
 26657            out Vector2d normal);
 26658        Vector3d normal3D = normal.ToVector3d(Fixed64.Zero);
 26659        Fixed64 constrainedInverseMassA = GetConstrainedInverseMass(normal);
 26660        Fixed64 constrainedInverseMassB = target.GetConstrainedInverseMass(normal3D);
 26661        Fixed64 frameFraction = ResolveContinuousCollisionFrameFraction(hitElapsedTime);
 26662        Vector3d targetVelocity = target.SampleContinuousCollisionLinearVelocity(
 26663            frameFraction);
 26664        bool inverseMassResolved = Fixed64.TryAdd(
 26665            constrainedInverseMassA,
 26666            constrainedInverseMassB,
 26667            out Fixed64 inverseMass);
 26668        bool relativeVelocityResolved = Vector3d.TrySubtract(
 26669            _linearVelocity.ToVector3d(Fixed64.Zero),
 26670            targetVelocity,
 26671            out Vector3d relativeVelocity);
 26672        Fixed64 normalVelocity = Vector3d.Dot(relativeVelocity, normal3D);
 26673        Fixed64 restitution = ResolveContinuousCollisionRestitution(target, -normalVelocity);
 26674        bool responseFactorResolved = Fixed64.TryAdd(
 26675            Fixed64.One,
 26676            restitution,
 26677            out Fixed64 responseFactor);
 26678        bool closingSpeedResolved = Fixed64.TrySubtract(
 26679            Fixed64.Zero,
 26680            normalVelocity,
 26681            out Fixed64 closingSpeed);
 26682        bool responseSpeedResolved = Fixed64.TryMultiplyDivide(
 26683            closingSpeed,
 26684            responseFactor,
 26685            Fixed64.One,
 26686            out Fixed64 responseSpeed);
 26687        Vector2d sourceResponseNormal = ProjectLinearMotion(normal);
 26688        Vector3d targetResponseNormal = target.ProjectLinearMotion(-normal3D);
 26689        bool sourceDeltaResolved = ContinuousCollisionImpulsePolicy.TryResolveVelocityDelta(
 26690            sourceResponseNormal,
 26691            responseSpeed,
 26692            EffectiveInverseMass,
 26693            inverseMass,
 26694            out Vector2d sourceVelocityDelta);
 26695        bool targetDeltaResolved = ContinuousCollisionImpulsePolicy.TryResolveVelocityDelta(
 26696            targetResponseNormal,
 26697            responseSpeed,
 26698            target.EffectiveInverseMass,
 26699            inverseMass,
 26700            out Vector3d targetVelocityDelta);
 26701        bool sourceVelocityResolved = Vector2d.TryAdd(
 26702            _linearVelocity,
 26703            sourceVelocityDelta,
 26704            out Vector2d sourcePostLinearVelocity);
 26705        bool sourceTrajectoryAvailable = CanAppendContinuousCollisionFrameSegment(hitElapsedTime);
 26706        Vector3d targetResolvedPosition = default;
 26707        Vector3d targetPostLinearVelocity = targetVelocity;
 26708        FixedQuaternion targetRotationAtImpact = target.SampleContinuousCollisionRotation(frameFraction);
 26709        Vector3d targetAngularVelocity = target.SampleContinuousCollisionAngularVelocity(frameFraction);
 26710        bool targetStateAvailable = true;
 26711        if (target.CanTranslate)
 712        {
 25713            bool targetVelocityResolved = Vector3d.TryAdd(
 25714                targetVelocity,
 25715                targetVelocityDelta,
 25716                out targetPostLinearVelocity);
 25717            bool targetTrajectoryAvailable = target.CanApplyContinuousCollisionHandoff(
 25718                targetPositionAtImpact,
 25719                targetRotationAtImpact,
 25720                remainingTime,
 25721                out targetResolvedPosition);
 25722            targetStateAvailable = targetVelocityResolved & targetTrajectoryAvailable;
 723        }
 724
 26725        bool responseAdmissible = normalResolved
 26726            & inverseMassResolved
 26727            & relativeVelocityResolved
 26728            & normalVelocity < -Fixed64.Epsilon
 26729            & responseFactorResolved
 26730            & closingSpeedResolved
 26731            & responseSpeedResolved
 26732            & sourceDeltaResolved
 26733            & targetDeltaResolved
 26734            & sourceVelocityResolved
 26735            & sourceTrajectoryAvailable
 26736            & targetStateAvailable;
 26737        if (!responseAdmissible)
 738        {
 9739            return false;
 740        }
 741
 17742        ValidateContinuousCollisionPosition(sourcePositionAtImpact, sourcePostLinearVelocity, Context.DeltaTime - hitEla
 17743        if (target.CanTranslate)
 16744            target.ValidateContinuousCollisionHandoffPosition(targetResolvedPosition, targetPostLinearVelocity, targetAn
 745
 15746        _ = Context.Physics2D.TryReserveContinuousCollisionCandidateRefresh(this);
 15747        if (target.CanTranslate)
 14748            _ = Context.Physics.TryReserveContinuousCollisionCandidateRefresh(target);
 749
 15750        ApplyContinuousCollisionSourceResponse(
 15751            sourcePositionAtImpact,
 15752            sourcePostLinearVelocity,
 15753            hitElapsedTime);
 15754        if (target.CanTranslate)
 755        {
 14756            target.ApplyContinuousCollisionHandoffReserved(
 14757                targetResolvedPosition,
 14758                targetRotationAtImpact,
 14759                targetPostLinearVelocity,
 14760                targetAngularVelocity,
 14761                remainingTime,
 14762                ignoredCollider2D: Collider);
 763        }
 764
 15765        return true;
 766    }
 767
 768    private void ApplyContinuousCollisionSourceResponse(
 769        Vector2d positionAtImpact,
 770        Vector2d postLinearVelocity,
 771        Fixed64 elapsedTime)
 772    {
 55773        postLinearVelocity = ProjectLinearMotion(postLinearVelocity);
 55774        WakeFromCollision();
 55775        _linearVelocity = postLinearVelocity;
 55776        RefreshLinearSpeed();
 55777        UpdateContinuousCollisionFrameTrajectory(positionAtImpact, _linearVelocity, elapsedTime);
 55778        Context.Physics2D.RefreshContinuousCollisionCandidate(this);
 55779    }
 780
 781    private void UpdateContinuousCollisionFrameTrajectory(
 782        Vector2d positionAtElapsedTime,
 783        Vector2d velocity,
 784        Fixed64 elapsedTime)
 785    {
 441786        AppendContinuousCollisionFrameSegment(
 441787            positionAtElapsedTime,
 441788            velocity,
 441789            elapsedTime);
 441790    }
 791
 792    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 793    private Fixed64 ResolveContinuousCollisionFrameFraction(Fixed64 hitElapsedTime) =>
 196794        FixedMath.Clamp01(hitElapsedTime / Context.DeltaTime);
 795
 796    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 797    private Fixed64 ResolveContinuousCollisionRestitution(SolidBody2D target, Fixed64 closingSpeed)
 798    {
 54799        if (closingSpeed <= Context.Settings.RestitutionVelocityThreshold)
 3800            return Fixed64.Zero;
 801
 51802        return PhysicsMaterial.CombineRestitution(Collider.Material, target.Collider.Material);
 803    }
 804
 805    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 806    private Fixed64 ResolveContinuousCollisionRestitution(SolidBody target, Fixed64 closingSpeed)
 807    {
 34808        if (closingSpeed <= Context.Settings.RestitutionVelocityThreshold)
 12809            return Fixed64.Zero;
 810
 22811        return PhysicsMaterial.CombineRestitution(Collider.Material, target.Collider.Material);
 812    }
 813
 814    private void RemoveClosingContinuousCollisionVelocity(Vector2d normal)
 815    {
 214816        if (normal.MagnitudeSquared <= Fixed64.Epsilon)
 8817            return;
 818
 206819        Fixed64 closingSpeed = Vector2d.Dot(_linearVelocity, normal);
 206820        if (closingSpeed >= Fixed64.Zero)
 1821            return;
 822
 205823        _linearVelocity -= normal * closingSpeed;
 205824        RefreshLinearSpeed();
 205825    }
 826
 827}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/2D/Bodies/SolidBody2D.ContinuousCollision.Helpers.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody2D.ContinuousCollision.Helpers.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
 8using System.Runtime.CompilerServices;
 9using FixedMathSharp;
 10using Gravitas.Colliders;
 11using Gravitas.CollisionHandling;
 12using Gravitas.Queries;
 13
 14namespace Gravitas;
 15
 16/// <content>Selects planar sweep modes and proxy radii and filters eligible continuous collision hits.</content>
 17public sealed partial class SolidBody2D
 18{
 19    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 20    private bool ShouldUseContinuousCollision(out ContinuousCollisionMode mode)
 21    {
 2849022        mode = ResolveContinuousCollisionMode();
 2849023        return mode == ContinuousCollisionMode.Continuous || mode == ContinuousCollisionMode.Auto;
 24    }
 25
 26    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 27    private ContinuousCollisionMode ResolveContinuousCollisionMode()
 28    {
 2849029        ContinuousCollisionMode mode = _continuousCollisionMode;
 2849030        if (mode != ContinuousCollisionMode.Inherit)
 295031            return mode;
 32
 2554033        SolidBody2D? parentBody = Collider.TopParent2D?.Body;
 2554034        if (parentBody != null && parentBody._continuousCollisionMode != ContinuousCollisionMode.Inherit)
 435            return parentBody._continuousCollisionMode;
 36
 2553637        mode = Context.Settings.DefaultContinuousCollisionMode;
 2553638        return mode == ContinuousCollisionMode.Inherit
 2553639            ? ContinuousCollisionMode.Discrete
 2553640            : mode;
 41    }
 42
 2591643    internal Fixed64 ResolveContinuousCollisionProxyRadius() => Collider.CanonicalPivotProxyRadius;
 44
 45    internal Fixed64 ResolveMixedContinuousCollisionProxyRadius() =>
 57346        ColliderGeometryPolicy.Mixed(ResolveContinuousCollisionProxyRadius(), Collider.MixedHalfThickness);
 47
 48    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 49    private bool IsValidContinuousCollisionHit(Physics2DHit hit) =>
 46350        IsValidContinuousCollisionTarget(hit.Collider);
 51
 52    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 53    private static bool IsClosingContinuousCollisionHit(Vector2d displacement, Vector2d normal) =>
 68054        Vector2d.Dot(displacement, normal) < -Fixed64.Epsilon;
 55
 56    private bool IsValidContinuousCollisionTarget(LSCollider2D hitCollider)
 57    {
 56258        SolidBody2D? hitBody = hitCollider.Body;
 56259        if (hitBody?.IsKinematic == true)
 60        {
 1661            hitBody.EnsureContinuousCollisionFramePrepared(Context.LateSimulateToken);
 1662            if (hitBody.HasContinuousCollisionMotion)
 763                return false;
 64        }
 65
 55566        return ContinuousCollisionTargetPolicy.AllowsStaticOrKinematic2DTarget(
 55567            ReferenceEquals(hitCollider, Collider),
 55568            ContinuousCollisionCandidateOrdering.IsIgnoredTarget(hitCollider, _continuousCollisionHandoffIgnoredCollider
 55569            hitCollider.IsTrigger,
 55570            Context.Physics2D.RequireCollisionPair(Collider, hitCollider),
 55571            hitCollider.IsStatic,
 55572            hitBody != null && hitBody.IsKinematic);
 73    }
 74
 75    private bool IsValidMixedContinuousCollisionHit(LSCollider hitCollider)
 76    {
 20777        SolidBody? hitBody = hitCollider.Body;
 20778        if (hitBody?.IsKinematic == true)
 79        {
 480            hitBody.EnsureContinuousCollisionFramePrepared(Context.LateSimulateToken);
 481            if (hitBody.HasContinuousCollisionMotion)
 382                return false;
 83        }
 84
 20485        return ContinuousCollisionTargetPolicy.AllowsMixedStaticOrKinematicTarget(
 20486            ContinuousCollisionCandidateOrdering.IsIgnoredTarget(hitCollider, _continuousCollisionHandoffIgnoredCollider
 20487            hitCollider.IsTrigger,
 20488            Context.MixedCollisions.RequireCollisionPair(hitCollider, Collider),
 20489            hitCollider.IsStatic,
 20490            hitBody != null && hitBody.IsKinematic);
 91    }
 92
 93}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/2D/Bodies/SolidBody2D.ContinuousCollision.Hits.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody2D.ContinuousCollision.Hits.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
 8using FixedMathSharp;
 9using FixedMathSharp.Geometry;
 10using Gravitas.Colliders;
 11using Gravitas.CollisionHandling;
 12using Gravitas.Queries;
 13using Gravitas.Support;
 14using SwiftCollections;
 15
 16namespace Gravitas;
 17
 18/// <content>Selects continuous collision hits against stationary and moving targets and prepares mixed-surface contacts
 19public sealed partial class SolidBody2D
 20{
 21    private ContactManifold2D? _surfaceContinuousCollisionManifold;
 22
 23    private bool TryGetFirstContinuousCollisionHit(
 24        Vector2d startPosition,
 25        Vector2d proposedPosition,
 26        Fixed64 proxyRadius,
 27        Fixed64 elapsedFrameFraction,
 28        out Vector2d normal,
 29        out Fixed64 distance,
 30        out ContinuousCollisionTargetKind targetKind,
 31        out LSCollider2D? target2D,
 32        out LSCollider? target3D)
 33    {
 98034        Vector2d originalPosition = _position;
 35        try
 36        {
 98037            _position = startPosition;
 98038            Collider.RebuildRuntimeShapeOnly();
 39
 98040            int hitCount = Context.Query2D.SweepCircleAgainstStaticAll(
 98041                startPosition,
 98042                proposedPosition,
 98043                proxyRadius,
 98044                PhysicsLayerMask.All,
 98045                _continuousCollisionHits,
 98046                Collider,
 98047                includeTriggers: false);
 98048            int mixedHitCount = Context.Settings.RuntimeMode.RunsMixedContacts()
 98049                ? Context.QueryMixed.SweepCircleAgainstStatic3DAll(
 98050                    startPosition,
 98051                    proposedPosition,
 98052                    proxyRadius,
 98053                    Collider.MixedSlabCenterY,
 98054                    Collider.MixedHalfThickness,
 98055                    PhysicsLayerMask.All,
 98056                    _continuousMixedCollisionHits,
 98057                    Collider,
 98058                    includeTriggers: false,
 98059                    cacheTargetPartitions: true,
 98060                    requireClosingContact: true)
 98061                : 0;
 62
 98063            bool found2D = TryGetFirstValidContinuousCollisionHit(startPosition, proposedPosition, proxyRadius, hitCount
 98064            ContinuousCollisionTargetKind hit2DKind = found2D
 98065                ? ContinuousCollisionTargetKind.Static2D
 98066                : ContinuousCollisionTargetKind.None;
 98067            bool foundDynamic2D = TryGetFirstDynamicContinuousCollisionHit(
 98068                startPosition,
 98069                proposedPosition,
 98070                proxyRadius,
 98071                elapsedFrameFraction,
 98072                out Physics2DHit dynamicHit2D,
 98073                out Fixed64 dynamicClosingSpeed2D);
 98074            if (ContinuousCollisionCandidateOrdering.ShouldReplaceHit(dynamicHit2D, dynamicClosingSpeed2D, foundDynamic2
 75            {
 3976                hit2D = dynamicHit2D;
 3977                found2D = true;
 3978                hit2DKind = ContinuousCollisionTargetKind.Dynamic2D;
 79            }
 80
 98081            bool foundMixed = TryGetFirstValidMixedContinuousCollisionHit(startPosition, proposedPosition, mixedHitCount
 98082            ContinuousCollisionTargetKind hitMixedKind = foundMixed
 98083                ? ContinuousCollisionTargetKind.Static3D
 98084                : ContinuousCollisionTargetKind.None;
 98085            Fixed64 hitMixedDistance = foundMixed
 98086                ? hitMixed.Distance
 98087                : Fixed64.Zero;
 98088            bool foundDynamicMixed = TryGetFirstDynamicMixedContinuousCollisionHit(
 98089                startPosition,
 98090                proposedPosition,
 98091                elapsedFrameFraction,
 98092                out DynamicMixedIntervalHit dynamicMixed);
 98093            if (DynamicMixedIntervalHit.ShouldReplaceStatic(
 98094                    dynamicMixed,
 98095                    foundDynamicMixed,
 98096                    hitMixed,
 98097                    foundMixed))
 98            {
 6399                hitMixed = dynamicMixed.ExactHit;
 63100                hitMixedDistance = dynamicMixed.SafeDistance;
 63101                foundMixed = true;
 63102                hitMixedKind = dynamicMixed.Status
 63103                    == IntervalSearchStatus.ExactHit
 63104                    ? ContinuousCollisionTargetKind.Dynamic3D
 63105                    : ContinuousCollisionTargetKind.UnresolvedMixed;
 106            }
 107
 980108            if (DynamicMixedIntervalHit.ShouldSelect2D(
 980109                    found2D,
 980110                    hit2D.Distance,
 980111                    foundMixed,
 980112                    hitMixedDistance))
 113            {
 217114                normal = hit2D.Normal;
 217115                distance = hit2D.Distance;
 217116                targetKind = hit2DKind;
 217117                target2D = hit2D.Collider;
 217118                target3D = null;
 217119                return true;
 120            }
 121
 763122            if (foundMixed)
 123            {
 255124                normal = hitMixedKind == ContinuousCollisionTargetKind.UnresolvedMixed
 255125                    ? Vector2d.Zero
 255126                    : hitMixed.NormalFor2DSource;
 255127                distance = hitMixedDistance;
 255128                targetKind = hitMixedKind;
 255129                target2D = null;
 255130                target3D = hitMixedKind == ContinuousCollisionTargetKind.UnresolvedMixed
 255131                    ? null
 255132                    : hitMixed.Collider3D;
 255133                return true;
 134            }
 508135        }
 136        finally
 137        {
 980138            _position = originalPosition;
 980139            Collider.RebuildRuntimeShapeOnly();
 980140        }
 141
 508142        normal = Vector2d.Zero;
 508143        distance = Fixed64.Zero;
 508144        targetKind = ContinuousCollisionTargetKind.None;
 508145        target2D = null;
 508146        target3D = null;
 508147        return false;
 472148    }
 149
 150    private bool TryGetFirstValidContinuousCollisionHit(
 151        Vector2d startPosition,
 152        Vector2d proposedPosition,
 153        Fixed64 proxyRadius,
 154        int hitCount,
 155        out Physics2DHit hit)
 156    {
 1058157        Vector2d displacement = proposedPosition - startPosition;
 1058158        bool exactCircleSource = Collider is LSCircleCollider2D circle && circle.LocalOffset == Vector2d.Zero
 1058159            && proxyRadius == circle.ScaledRadius;
 1058160        bool found = false;
 1058161        Physics2DHit best = default;
 3042162        for (int i = 0; i < hitCount; i++)
 163        {
 463164            Physics2DHit candidate = _continuousCollisionHits[i];
 463165            if (!IsValidContinuousCollisionHit(candidate))
 166                continue;
 167
 168            // A centered circle's collector uses this same physical circle;
 169            // its declared-target hit needs admission, not a duplicate sweep.
 459170            Physics2DHit refined = candidate;
 459171            if (!(exactCircleSource && candidate.Collider.StaticSurface != null)
 459172                && !QueryDetection2D.TrySweepMoverShape(Collider, displacement, candidate.Collider, out refined))
 173                continue;
 174            // Admission can replace anchors/normals, never this ordering key.
 426175            if (found && !Physics2DHitSorter.ComesBefore(refined, best))
 176                continue;
 387177            if (candidate.Collider.StaticSurface != null
 387178                && !AdmitStaticSurfaceContinuousCollisionHit(ref refined, startPosition, proposedPosition, displacement)
 179                continue;
 274180            if (!IsClosingContinuousCollisionHit(displacement, refined.Normal))
 181                continue;
 182
 215183            best = refined;
 215184            found = true;
 185        }
 186
 1058187        hit = best;
 1058188        return found;
 189    }
 190
 191    private bool AdmitStaticSurfaceContinuousCollisionHit(ref Physics2DHit hit,
 192        Vector2d start, Vector2d end, Vector2d displacement)
 193    {
 190194        Vector2d original = _position;
 195        try
 196        {
 190197            _position = new FixedSegment2d(start, end).GetPointAtDistance(hit.Distance, displacement.Magnitude);
 190198            Collider.RebuildRuntimeShapeOnly();
 190199            ContactManifold2D manifold = _surfaceContinuousCollisionManifold ??= new();
 190200            bool overlaps = CollisionDetection2D.TryCollide(new CollisionWorkItem2D(Collider, hit.Collider,
 190201                    ColliderSettings2D.GetCollisionType(Collider.Shape, hit.Collider.Shape)), manifold, Context.FrameCou
 190202            if (!overlaps)
 1203                return hit.Collider.StaticSurface!.AdmitContinuousWitness(Collider, hit.Collider, hit.Anchor);
 687204            foreach (ManifoldContact2D row in manifold)
 205            {
 193206                Vector2d normal = -row.Normal;
 193207                if (!IsClosingContinuousCollisionHit(displacement, normal)) continue;
 77208                hit = new(hit.Collider, row.AnchorB, normal, hit.Distance);
 77209                return true;
 210            }
 112211            return false;
 212        }
 570213        finally { _position = original; Collider.RebuildRuntimeShapeOnly(); }
 190214    }
 215
 216    private bool TryGetFirstValidMixedContinuousCollisionHit(
 217        Vector2d startPosition,
 218        Vector2d proposedPosition,
 219        int hitCount,
 220        out PhysicsMixedHit hit)
 221    {
 1058222        Vector2d displacement = proposedPosition - startPosition;
 2130223        for (int i = 0; i < hitCount; i++)
 224        {
 207225            PhysicsMixedHit candidate = _continuousMixedCollisionHits[i];
 207226            if (!IsValidMixedContinuousCollisionHit(candidate.Collider3D!))
 227                continue;
 228
 202229            if (UsesMixedStaticManifold(candidate.Collider3D!))
 230            {
 182231                Vector2d position = new FixedSegment2d(startPosition, proposedPosition)
 182232                    .GetPointAtDistance(candidate.Distance, displacement.Magnitude);
 182233                if (TryBuildStaticMixedContinuousCollisionManifold(candidate.Collider3D!, position, Rotation))
 234                {
 181235                    ContactAnchor center = ContactAnchor.FromWorldPoint(Vector3d.Zero);
 181236                    if (!MixedContact.TrySelectClosingContact(_rotationalMixedEarliestManifold,
 181237                            center, Vector3d.Zero, Vector3d.Zero, center, displacement, Fixed64.Zero,
 181238                            out MixedContact closing))
 239                        continue;
 179240                    candidate = new PhysicsMixedHit(candidate.Collider3D, Collider, closing.Anchor3D,
 179241                        closing.Anchor2D, closing.Normal3DTo2D, candidate.ReducerKind,
 179242                        candidate.Distance, candidate.Direction3D);
 243                }
 244            }
 245            // A proxy hit without a real witness retains the conservative stop.
 200246            if (!IsClosingContinuousCollisionHit(displacement, candidate.NormalFor2DSource))
 247                continue;
 248
 200249            hit = candidate;
 200250            return true;
 251        }
 252
 858253        hit = default;
 858254        return false;
 255    }
 256
 257    private bool UsesMixedStaticManifold(LSCollider target)
 394258        => target is LSCompoundCollider or LSMeshCollider || Collider is LSCompoundCollider2D;
 259
 260    private bool TryBuildStaticMixedContinuousCollisionManifold(LSCollider target,
 261        Vector2d position, Fixed64 rotation)
 262    {
 182263        Vector2d originalPosition = Position;
 182264        Fixed64 originalRotation = Rotation;
 265        try
 266        {
 182267            SetMixedContinuousCollisionSamplePose(position, rotation);
 268            // Translational moving-target searches overwrite Known only; the
 269            // mutually exclusive rotational path owns this same accepted buffer.
 182270            return CollisionDetectionMixed.TryBuildDiscreteManifold(target, Collider, _rotationalMixedEarliestManifold);
 271        }
 272        finally
 273        {
 182274            SetMixedContinuousCollisionSamplePose(originalPosition, originalRotation);
 182275        }
 182276    }
 277
 278    private bool TryApplyStaticMixedContinuousCollisionResponse(LSCollider target,
 279        Vector2d position, Fixed64 elapsedTime, Fixed64 remainingTime)
 280    {
 192281        if (!UsesMixedStaticManifold(target)
 192282            || !_rotationalMixedEarliestManifold.HasContact)
 19283            return false;
 173284        if (!TryApplyMixedRotationalContinuousCollisionResponse(target,
 173285                MixedContact.FromManifoldContact(_rotationalMixedEarliestManifold.PrimaryContact),
 173286                Fixed64.Zero, position, Vector2d.Zero, Rotation, Fixed64.Zero, elapsedTime, remainingTime,
 173287                _rotationalMixedEarliestManifold))
 1288            return false;
 172289        UpdateContinuousCollisionFrameTrajectory(position, _linearVelocity, elapsedTime);
 172290        Context.Physics2D.RefreshContinuousCollisionCandidate(this);
 172291        return true;
 292    }
 293
 294    private bool TryGetFirstDynamicContinuousCollisionHit(
 295        Vector2d startPosition,
 296        Vector2d proposedPosition,
 297        Fixed64 proxyRadius,
 298        Fixed64 elapsedFrameFraction,
 299        out Physics2DHit hit,
 300        out Fixed64 closingSpeed)
 301    {
 980302        hit = default;
 980303        closingSpeed = Fixed64.Zero;
 304
 980305        Vector2d sourceDisplacement = proposedPosition - startPosition;
 980306        Fixed64 sourceLength = sourceDisplacement.Magnitude;
 980307        bool found = false;
 980308        Physics2DHit best = default;
 980309        Fixed64 bestClosingSpeed = Fixed64.Zero;
 980310        long token = Context.LateSimulateToken;
 980311        SwiftList<int> candidateIds = Context.Physics2D.QueryPlanarContinuousCollisionCandidates(
 980312            DynamicCcdCandidateIndex2D.CreateSweptCircleBounds(startPosition, sourceDisplacement, proxyRadius));
 4172313        for (int candidateIndex = 0; candidateIndex < candidateIds.Count; candidateIndex++)
 314        {
 1106315            int dynamicId = candidateIds[candidateIndex];
 1106316            SolidBody2D target = Context.Physics2D.GetContinuousCollisionCandidate(dynamicId);
 1106317            if (!IsEligibleDynamicContinuousCollisionTarget(target))
 318            {
 319                continue;
 320            }
 321
 66322            target.EnsureContinuousCollisionFramePrepared(token);
 66323            if (!TryGetDynamicRelativeContinuousCollisionHit(
 66324                    target,
 66325                    startPosition,
 66326                    sourceDisplacement,
 66327                    proxyRadius,
 66328                    sourceLength,
 66329                    elapsedFrameFraction,
 66330                    out Physics2DHit candidate,
 66331                    out Fixed64 candidateClosingSpeed))
 332            {
 333                continue;
 334            }
 335
 50336            if (!ContinuousCollisionCandidateOrdering.ShouldReplaceHit(candidate, candidateClosingSpeed, true, found, be
 337                continue;
 338
 42339            best = candidate;
 42340            bestClosingSpeed = candidateClosingSpeed;
 42341            found = true;
 342        }
 343
 980344        hit = best;
 980345        closingSpeed = bestClosingSpeed;
 980346        return found;
 347    }
 348
 349    internal bool TryGetDynamicRelativeContinuousCollisionHit(
 350        SolidBody2D target,
 351        Vector2d sourceStart,
 352        Vector2d sourceDisplacement,
 353        Fixed64 sourceRadius,
 354        Fixed64 sourceLength,
 355        Fixed64 queryStartFraction,
 356        out Physics2DHit hit,
 357        out Fixed64 closingSpeed)
 358    {
 289359        hit = default;
 289360        closingSpeed = Fixed64.Zero;
 289361        Vector2d sourceEnd = sourceStart + sourceDisplacement;
 289362        Fixed64 targetRadius = target.ResolveContinuousCollisionProxyRadius();
 289363        bool found = false;
 289364        Physics2DHit best = default;
 289365        Fixed64 bestClosingSpeed = Fixed64.Zero;
 366
 289367        Vector2d originalSourcePosition = _position;
 289368        Fixed64 originalSourceRotation = _rotation;
 289369        Vector2d originalTargetPosition = target._position;
 289370        Fixed64 originalTargetRotation = target._rotation;
 371        try
 372        {
 289373            target.GetContinuousCollisionTrajectorySegmentRange(
 289374                queryStartFraction,
 289375                out int segmentStartIndex,
 289376                out int segmentEndExclusive);
 289377            for (int segmentIndex = segmentStartIndex;
 319378                segmentIndex < segmentEndExclusive;
 30379                segmentIndex++)
 380            {
 301381                ContinuousCollisionMotionSegment2D segment =
 301382                    target.GetContinuousCollisionTrajectorySegment(segmentIndex);
 301383                ContinuousCollisionMath.ClipTranslationalTrajectoryInterval(
 301384                    queryStartFraction,
 301385                    segment.StartFraction,
 301386                    segment.EndFraction,
 301387                    out Fixed64 overlapStart,
 301388                    out Fixed64 overlapEnd,
 301389                    out Fixed64 sourceStartTime,
 301390                    out Fixed64 sourceEndTime);
 391
 301392                Vector2d sourceSegmentStart = Vector2d.Lerp(
 301393                    sourceStart,
 301394                    sourceEnd,
 301395                    sourceStartTime);
 301396                Vector2d sourceSegmentEnd = Vector2d.Lerp(
 301397                    sourceStart,
 301398                    sourceEnd,
 301399                    sourceEndTime);
 301400                Vector2d sourceSegmentDisplacement =
 301401                    ContinuousCollisionSweepRange.ValidateEndpoint(
 301402                        sourceSegmentStart,
 301403                        sourceSegmentEnd,
 301404                        out Fixed64 sourceSegmentLength);
 301405                Vector2d targetStart = segment.SamplePosition(overlapStart);
 301406                Vector2d targetEnd = segment.SamplePosition(overlapEnd);
 301407                Vector2d targetDisplacement =
 301408                    ContinuousCollisionSweepRange.ValidateEndpoint(
 301409                        targetStart,
 301410                        targetEnd,
 301411                        out _);
 301412                LSCircleCollider2D? sourceCircle = Collider as LSCircleCollider2D;
 301413                LSCircleCollider2D? targetCircle = target.Collider as LSCircleCollider2D;
 301414                bool radialPair = sourceCircle is not null && targetCircle is not null;
 301415                Vector2d relativeDisplacement = default;
 301416                Fixed64 relativeLength = default;
 301417                if (!radialPair
 301418                    && !ContinuousCollisionMath.TryGetRelativeCircleOverlapDistanceInterval(
 301419                            sourceSegmentStart,
 301420                            sourceSegmentDisplacement,
 301421                            sourceRadius,
 301422                            targetStart,
 301423                            targetDisplacement,
 301424                            targetRadius,
 301425                            out _,
 301426                            out _,
 301427                            out relativeDisplacement,
 301428                            out relativeLength,
 301429                            out _,
 301430                            out _))
 431                {
 432                    continue;
 433                }
 301434                _position = sourceSegmentStart;
 301435                target._position = targetStart;
 301436                target._rotation = target.SampleContinuousCollisionRotation(
 301437                    overlapStart);
 301438                Collider.RebuildRuntimeShapeOnly();
 301439                target.Collider.RebuildRuntimeShapeOnly();
 440
 441                Physics2DHit relativeHit;
 301442                if (radialPair)
 443                {
 298444                    if (!ContinuousCollisionMath.TryGetRelativeCircleOverlapDistanceInterval(
 298445                            sourceCircle!.Center,
 298446                            sourceSegmentDisplacement,
 298447                            sourceCircle.ScaledRadius,
 298448                            targetCircle!.Center,
 298449                            targetDisplacement,
 298450                            targetCircle.ScaledRadius,
 298451                            out Fixed64 circleDistance,
 298452                            out _,
 298453                            out relativeDisplacement,
 298454                            out relativeLength,
 298455                            out Vector2d circleNormal,
 298456                            out _))
 457                    {
 458                        continue;
 459                    }
 460
 274461                    relativeHit = new Physics2DHit(
 274462                        targetCircle,
 274463                        new ContactAnchor2D(
 274464                            targetCircle.Center,
 274465                            circleNormal * targetCircle.ScaledRadius),
 274466                        circleNormal,
 274467                        circleDistance);
 468                }
 3469                else if (!QueryDetection2D.TrySweepMoverShape(
 3470                            Collider,
 3471                            relativeDisplacement,
 3472                            target.Collider,
 3473                            out relativeHit))
 474                {
 475                    continue;
 476                }
 477
 275478                Fixed64 successorStart = segmentIndex + 1
 275479                    < target.ContinuousCollisionTrajectoryCount
 275480                    ? target.GetContinuousCollisionTrajectorySegment(segmentIndex + 1)
 275481                        .StartFraction
 275482                    : Fixed64.Zero;
 275483                if (ContinuousCollisionMath.IsSupersededTranslationalBoundaryHit(
 275484                        relativeHit.Distance >= relativeLength,
 275485                        overlapEnd,
 275486                        segmentIndex,
 275487                        target.ContinuousCollisionTrajectoryCount,
 275488                        successorStart))
 489                {
 490                    continue;
 491                }
 492
 274493                Fixed64 localClosingSpeed =
 274494                    -Vector2d.Dot(relativeDisplacement, relativeHit.Normal);
 274495                if (!ContinuousCollisionMath.TryNormalizeTranslationalClosingSpeed(
 274496                        localClosingSpeed,
 274497                        sourceEndTime - sourceStartTime,
 274498                        out Fixed64 candidateClosingSpeed))
 499                    continue;
 500
 271501                ContactAnchor2D candidateAnchor = TranslateContactAnchor(
 271502                    relativeHit.Anchor,
 271503                    new FixedSegment2d(targetStart, targetEnd),
 271504                    relativeHit.Distance,
 271505                    relativeLength);
 271506                if (!Fixed64.TryMultiplyDivide(
 271507                        sourceSegmentLength,
 271508                        relativeHit.Distance,
 271509                        relativeLength,
 271510                        out Fixed64 localSourceDistance)
 271511                    || !Fixed64.TryMultiplyAdd(
 271512                        sourceLength,
 271513                        sourceStartTime,
 271514                        localSourceDistance,
 271515                        out Fixed64 sourceDistance))
 516                {
 517                    continue;
 518                }
 519
 271520                var candidate = new Physics2DHit(
 271521                    target.Collider,
 271522                    candidateAnchor,
 271523                    relativeHit.Normal,
 271524                    sourceDistance);
 271525                best = candidate;
 271526                bestClosingSpeed = candidateClosingSpeed;
 271527                found = true;
 271528                break;
 529            }
 18530        }
 531        finally
 532        {
 289533            _position = originalSourcePosition;
 289534            _rotation = originalSourceRotation;
 289535            target._position = originalTargetPosition;
 289536            target._rotation = originalTargetRotation;
 289537            target.Collider.RebuildRuntimeShapeOnly();
 289538            Collider.RebuildRuntimeShapeOnly();
 289539        }
 540
 289541        hit = best;
 289542        closingSpeed = bestClosingSpeed;
 289543        return found;
 544    }
 545
 546    private static ContactAnchor2D TranslateContactAnchor(
 547        ContactAnchor2D anchor,
 548        FixedSegment2d targetTrajectory,
 549        Fixed64 distance,
 550        Fixed64 totalDistance)
 551    {
 271552        Vector2d targetAtImpact = targetTrajectory.GetPointAtDistance(
 271553            distance,
 271554            totalDistance);
 271555        _ = Vector2d.TrySubtract(
 271556            targetAtImpact,
 271557            targetTrajectory.Start,
 271558            out Vector2d translation);
 271559        _ = Vector2d.TryAdd(anchor.Origin, translation, out Vector2d origin);
 271560        return new ContactAnchor2D(
 271561            origin,
 271562            anchor.Rotation,
 271563            anchor.LocalPoint,
 271564            anchor.LocalDisplacement);
 565    }
 566
 567    private bool TryGetFirstDynamicMixedContinuousCollisionHit(
 568        Vector2d startPosition,
 569        Vector2d proposedPosition,
 570        Fixed64 elapsedFrameFraction,
 571        out DynamicMixedIntervalHit hit)
 572    {
 980573        hit = default;
 574
 980575        if (!Context.Settings.RuntimeMode.RunsMixedContacts())
 532576            return false;
 577
 448578        Vector2d sourceDisplacement2D = proposedPosition - startPosition;
 448579        Fixed64 sourceLength = sourceDisplacement2D.Magnitude;
 448580        Vector3d sourceStart = new(startPosition.X, Collider.MixedSlabCenterY, startPosition.Y);
 448581        Vector3d sourceDisplacement = new(sourceDisplacement2D.X, Fixed64.Zero, sourceDisplacement2D.Y);
 448582        Fixed64 sourceRadius = ResolveMixedContinuousCollisionProxyRadius();
 448583        bool found = false;
 448584        DynamicMixedIntervalHit best = default;
 448585        long token = Context.LateSimulateToken;
 448586        SwiftList<int> candidateIds = Context.Physics.QueryContinuousCollisionCandidates(
 448587            DynamicCcdCandidateIndex.CreateSweptSphereBounds(sourceStart, sourceDisplacement, sourceRadius));
 1168588        for (int candidateIndex = 0; candidateIndex < candidateIds.Count; candidateIndex++)
 589        {
 136590            int dynamicId = candidateIds[candidateIndex];
 136591            SolidBody target = Context.Physics.GetContinuousCollisionCandidate(dynamicId);
 136592            if (!IsEligibleDynamicMixed3DTarget(target))
 593            {
 594                continue;
 595            }
 596
 66597            target.EnsureContinuousCollisionFramePrepared(token);
 66598            IntervalSearchStatus status =
 66599                TryGetDynamicMixed3DContinuousCollisionHit(
 66600                    target,
 66601                    sourceStart,
 66602                    sourceDisplacement,
 66603                    sourceRadius,
 66604                    sourceLength,
 66605                    elapsedFrameFraction,
 66606                    out DynamicMixedIntervalHit candidate);
 66607            if (status
 66608                == IntervalSearchStatus.CertifiedNoHit)
 609            {
 610                continue;
 611            }
 612
 64613            best = DynamicMixedIntervalHit.Select(
 64614                candidate,
 64615                best,
 64616                ref found);
 617        }
 618
 448619        hit = best;
 448620        return found;
 621    }
 622
 623    private IntervalSearchStatus
 624        TryGetDynamicMixed3DContinuousCollisionHit(
 625        SolidBody target,
 626        Vector3d sourceStart,
 627        Vector3d sourceDisplacement,
 628        Fixed64 sourceRadius,
 629        Fixed64 sourceLength,
 630        Fixed64 queryStartFraction,
 631        out DynamicMixedIntervalHit hit)
 632    {
 93633        hit = default;
 93634        Vector3d sourceEnd = sourceStart + sourceDisplacement;
 93635        Vector3d sourceDirection = sourceDisplacement.Normalized;
 93636        Fixed64 targetRadius =
 93637            target.ResolveContinuousCollisionProxyRadius();
 93638        target.GetContinuousCollisionTrajectorySegmentRange(
 93639            queryStartFraction,
 93640            out int segmentStartIndex,
 93641            out int segmentEndExclusive);
 93642        for (int segmentIndex = segmentStartIndex;
 103643            segmentIndex < segmentEndExclusive;
 10644            segmentIndex++)
 645        {
 95646            ContinuousCollisionMotionSegment3D segment =
 95647                target.GetContinuousCollisionTrajectorySegment(segmentIndex);
 95648            ContinuousCollisionMath.ClipTranslationalTrajectoryInterval(
 95649                queryStartFraction,
 95650                segment.StartFraction,
 95651                segment.EndFraction,
 95652                out Fixed64 overlapStart,
 95653                out Fixed64 overlapEnd,
 95654                out Fixed64 sourceStartTime,
 95655                out Fixed64 sourceEndTime);
 656
 95657            Vector3d sourceSegmentStart = Vector3d.Lerp(
 95658                sourceStart,
 95659                sourceEnd,
 95660                sourceStartTime);
 95661            Vector3d sourceSegmentEnd = Vector3d.Lerp(
 95662                sourceStart,
 95663                sourceEnd,
 95664                sourceEndTime);
 95665            Vector3d sourceSegmentDisplacement =
 95666                ContinuousCollisionSweepRange.ValidateEndpoint(
 95667                    sourceSegmentStart,
 95668                    sourceSegmentEnd,
 95669                    out Fixed64 sourceSegmentLength);
 95670            Vector3d targetStart = segment.SamplePosition(overlapStart);
 95671            Vector3d targetEnd = segment.SamplePosition(overlapEnd);
 95672            Vector3d targetDisplacement =
 95673                ContinuousCollisionSweepRange.ValidateEndpoint(
 95674                    targetStart,
 95675                    targetEnd,
 95676                    out Fixed64 targetSegmentLength);
 95677            if (!ContinuousCollisionMath.TryGetRelativeSphereOverlapDistanceInterval(
 95678                    sourceSegmentStart,
 95679                    sourceSegmentDisplacement,
 95680                    sourceRadius,
 95681                    targetStart,
 95682                    targetDisplacement,
 95683                    targetRadius,
 95684                    out Fixed64 entryDistance,
 95685                    out Fixed64 exitDistance,
 95686                    out Vector3d relativeDisplacement,
 95687                    out Fixed64 relativeLength,
 95688                    out _,
 95689                    out _))
 690            {
 691                continue;
 692            }
 693
 93694            Fixed64 successorStart = segmentIndex + 1
 93695                < target.ContinuousCollisionTrajectoryCount
 93696                ? target.GetContinuousCollisionTrajectorySegment(segmentIndex + 1)
 93697                    .StartFraction
 93698                : Fixed64.Zero;
 93699            if (ContinuousCollisionMath.IsSupersededTranslationalBoundaryHit(
 93700                    entryDistance >= relativeLength,
 93701                    overlapEnd,
 93702                    segmentIndex,
 93703                    target.ContinuousCollisionTrajectoryCount,
 93704                    successorStart))
 705            {
 706                continue;
 707            }
 708
 93709            Fixed64 sourceStartRotation =
 93710                SampleContinuousCollisionRotation(overlapStart);
 93711            Fixed64 sourceEndRotation =
 93712                SampleContinuousCollisionRotation(overlapEnd);
 93713            Fixed64 angularDelta = CanonicalizeRotation(
 93714                sourceEndRotation - sourceStartRotation);
 93715            Fixed64 intervalElapsedTime =
 93716                overlapStart * Context.DeltaTime;
 93717            Fixed64 intervalDuration =
 93718                (overlapEnd - overlapStart) * Context.DeltaTime;
 93719            bool foundExactContact =
 93720                TryGetExactSphereCircleTranslationalContact(
 93721                    target,
 93722                    segment,
 93723                    new FixedSegment(targetStart, targetEnd),
 93724                    targetDisplacement,
 93725                    targetSegmentLength,
 93726                    new FixedSegment2d(
 93727                        sourceSegmentStart.ToVector2d(),
 93728                        sourceSegmentEnd.ToVector2d()),
 93729                    sourceSegmentDisplacement.ToVector2d(),
 93730                    sourceSegmentLength,
 93731                    sourceStartRotation,
 93732                    angularDelta,
 93733                    out bool requiresRotationalFallback,
 93734                    out Fixed64 exactRelativeDistance,
 93735                    out Fixed64 sourceContactDistance,
 93736                    out Fixed64 exactLocalClosingSpeed,
 93737                    out MixedContact exactContact);
 93738            if (foundExactContact)
 739            {
 42740                if (ContinuousCollisionMath.IsSupersededTranslationalBoundaryHit(
 42741                        exactRelativeDistance >= relativeLength,
 42742                        overlapEnd,
 42743                        segmentIndex,
 42744                        target.ContinuousCollisionTrajectoryCount,
 42745                        successorStart))
 746                {
 747                    continue;
 748                }
 749
 40750                if (!ContinuousCollisionMath.TryNormalizeTranslationalClosingSpeed(
 40751                        exactLocalClosingSpeed,
 40752                        sourceEndTime - sourceStartTime,
 40753                        out Fixed64 exactClosingSpeed))
 754                {
 755                    continue;
 756                }
 757
 40758                if (!Fixed64.TryMultiplyAdd(
 40759                        sourceLength,
 40760                        sourceStartTime,
 40761                        sourceContactDistance,
 40762                        out Fixed64 exactSourceDistance))
 763                {
 764                    continue;
 765                }
 766
 40767                var analyticHit = new PhysicsMixedHit(
 40768                    target.Collider,
 40769                    null,
 40770                    exactContact.Anchor3D,
 40771                    exactContact.Anchor2D,
 40772                    exactContact.Normal3DTo2D,
 40773                    PhysicsQueryReducerKind.Exact,
 40774                    exactSourceDistance,
 40775                    sourceDirection);
 40776                hit = new DynamicMixedIntervalHit(
 40777                    IntervalSearchStatus.ExactHit,
 40778                    analyticHit,
 40779                    analyticHit.Distance,
 40780                    exactClosingSpeed,
 40781                    target.Collider.Id);
 40782                return IntervalSearchStatus.ExactHit;
 783            }
 51784            if (!requiresRotationalFallback)
 785                continue;
 786
 47787            Fixed64 entryTime = entryDistance / relativeLength;
 47788            Fixed64 exitTime = exitDistance / relativeLength;
 789
 47790            bool searchFound =
 47791                TryFindEarliestMixedRotationalContinuousCollisionAgainstTarget(
 47792                    target.Collider,
 47793                    sourceSegmentStart.ToVector2d(),
 47794                    sourceSegmentDisplacement.ToVector2d(),
 47795                    sourceStartRotation,
 47796                    angularDelta,
 47797                    angularDelta.Abs(),
 47798                    sourceRadius,
 47799                    intervalElapsedTime,
 47800                    intervalDuration,
 47801                    entryTime,
 47802                    exitTime,
 47803                    needsResponseContact: true,
 47804                    out Fixed64 safeTime,
 47805                    out MixedContact contact,
 47806                    out bool hasContact,
 47807                    out Fixed64 contactTime);
 47808            if (!searchFound)
 809            {
 810                continue;
 811            }
 812
 45813            bool closingResolved = Vector3d.TryDot(
 45814                relativeDisplacement,
 45815                contact.Normal3DTo2D,
 45816                out Fixed64 contactClosingSpeed);
 45817            contactClosingSpeed = -contactClosingSpeed;
 45818            bool hasOnlyTranslationalMotion =
 45819                angularDelta.Abs() <= Fixed64.Epsilon
 45820                & segment.AngularDistance <= Fixed64.Epsilon;
 45821            bool useContactClosingSpeed = hasContact & hasOnlyTranslationalMotion;
 45822            if (useContactClosingSpeed
 45823                & (!closingResolved
 45824                    | contactClosingSpeed <= Fixed64.Epsilon))
 825            {
 826                continue;
 827            }
 45828            Fixed64 candidateClosingSpeed =
 45829                (useContactClosingSpeed
 45830                    ? contactClosingSpeed
 45831                    : relativeLength)
 45832                / (sourceEndTime - sourceStartTime);
 833
 45834            Fixed64 safeSourceTime = FixedMath.Lerp(
 45835                sourceStartTime,
 45836                sourceEndTime,
 45837                safeTime);
 45838            bool exactHit = hasContact & contactTime == safeTime;
 45839            PhysicsMixedHit exact = exactHit
 45840                ? new PhysicsMixedHit(
 45841                    target.Collider,
 45842                    null,
 45843                    contact.Anchor3D,
 45844                    contact.Anchor2D,
 45845                    contact.Normal3DTo2D,
 45846                    PhysicsQueryReducerKind.ConservativeFallback,
 45847                    sourceLength * safeSourceTime,
 45848                    sourceDirection)
 45849                : default;
 45850            IntervalSearchStatus status = exactHit
 45851                ? IntervalSearchStatus.ExactHit
 45852                : IntervalSearchStatus.Unresolved;
 45853            hit = new DynamicMixedIntervalHit(
 45854                status,
 45855                exact,
 45856                sourceLength * safeSourceTime,
 45857                candidateClosingSpeed,
 45858                target.Collider.Id);
 45859            return status;
 860        }
 861
 8862        return IntervalSearchStatus.CertifiedNoHit;
 863    }
 864
 865    private bool IsEligibleDynamicContinuousCollisionTarget(SolidBody2D target)
 866    {
 1196867        return (IsKinematic || !target.ShouldOwnContinuousCollisionMovingPair(
 1196868                HasContinuousCollisionRotationalMotion))
 1196869            && !ContinuousCollisionCandidateOrdering.IsIgnoredTarget(
 1196870                target.Collider,
 1196871                _continuousCollisionHandoffIgnoredCollider2D)
 1196872            && ContinuousCollisionTargetPolicy.AllowsIndexed2DTarget(
 1196873                ReferenceEquals(target, this),
 1196874                target.Active,
 1196875                target.IsDynamic,
 1196876                target.IsKinematic,
 1196877                target.IsKinematic && target.HasContinuousCollisionMotion,
 1196878                target.Collider.IsTrigger,
 1196879                Context.Physics2D.RequireCollisionPair(Collider, target.Collider));
 880    }
 881
 882    private bool IsEligibleDynamicMixed3DTarget(SolidBody target)
 883    {
 154884        return (IsKinematic || !target.ShouldOwnContinuousCollisionMovingPair(
 154885                HasContinuousCollisionRotationalMotion))
 154886            && !ContinuousCollisionCandidateOrdering.IsIgnoredTarget(
 154887                target.Collider,
 154888                _continuousCollisionHandoffIgnoredCollider3D)
 154889            && ContinuousCollisionTargetPolicy.AllowsMixedIndexedTarget(
 154890                target.Active,
 154891                target.IsDynamic,
 154892                target.IsKinematic,
 154893                target.IsKinematic && target.HasContinuousCollisionMotion,
 154894                target.Collider.IsTrigger,
 154895                Context.MixedCollisions.RequireCollisionPair(target.Collider, Collider));
 896    }
 897
 898}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/2D/Bodies/SolidBody2D.ContinuousCollision.Kinematic.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody2D.ContinuousCollision.Kinematic.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
 8using FixedMathSharp;
 9using FixedMathSharp.Geometry;
 10using Gravitas.CollisionHandling;
 11using Gravitas.Queries;
 12using Gravitas.Support;
 13using SwiftCollections;
 14
 15namespace Gravitas;
 16
 17/// <content>Clips kinematic planar motion against static geometry and transfers pushes to dynamic targets.</content>
 18public sealed partial class SolidBody2D
 19{
 20    private bool TryResolveKinematicContinuousCollision(Vector2d startPosition, ref Vector2d proposedPosition)
 21    {
 13422        if (!ShouldUseContinuousCollision(out ContinuousCollisionMode mode))
 4623            return false;
 24
 8825        Vector2d displacement = ContinuousCollisionSweepRange.ValidateEndpoint(
 8826            startPosition,
 8827            proposedPosition,
 8828            out Fixed64 sourceLength);
 8829        Fixed64 displacementMagnitudeSquared = displacement.MagnitudeSquared;
 8830        if (displacementMagnitudeSquared <= Fixed64.Epsilon)
 731            return false;
 32
 8133        Fixed64 proxyRadius = ResolveContinuousCollisionProxyRadius();
 8134        if (proxyRadius <= Fixed64.Epsilon
 8135            || (mode == ContinuousCollisionMode.Auto
 8136                && ContinuousCollisionMath.IsWithinProxyRadius(displacement, displacementMagnitudeSquared, proxyRadius))
 37        {
 338            return false;
 39        }
 40
 7841        bool foundStatic = TryGetFirstKinematicStaticContinuousCollisionHit(
 7842            startPosition,
 7843            proposedPosition,
 7844            proxyRadius,
 7845            out Fixed64 staticHitDistance);
 7846        Fixed64 dynamicPushLimit = foundStatic ? staticHitDistance : sourceLength;
 7847        bool pushedDynamic = TryApplyKinematicDynamicContinuousCollisionPushes(
 7848            startPosition,
 7849            proposedPosition,
 7850            proxyRadius,
 7851            dynamicPushLimit,
 7852            sourceLength,
 7853            out bool hasUnresolvedMixedLimit,
 7854            out Fixed64 unresolvedMixedDistance);
 7855        staticHitDistance = FixedMath.Min(
 7856            dynamicPushLimit,
 7857            unresolvedMixedDistance);
 7858        foundStatic |= hasUnresolvedMixedLimit;
 7859        if (!foundStatic && !pushedDynamic)
 2260            return false;
 61
 5662        LastContinuousCollisionToiIterationCount++;
 5663        if (foundStatic)
 4664            proposedPosition = new FixedSegment2d(startPosition, proposedPosition)
 4665                .GetPointAtDistance(staticHitDistance, sourceLength);
 66
 5667        return true;
 68    }
 69
 70    private bool TryGetFirstKinematicStaticContinuousCollisionHit(
 71        Vector2d startPosition,
 72        Vector2d proposedPosition,
 73        Fixed64 proxyRadius,
 74        out Fixed64 distance)
 75    {
 7876        Vector2d originalPosition = _position;
 77        try
 78        {
 7879            _position = startPosition;
 7880            Collider.RebuildRuntimeShapeOnly();
 81
 7882            int hitCount = Context.Query2D.SweepCircleAgainstStaticAll(
 7883                startPosition,
 7884                proposedPosition,
 7885                proxyRadius,
 7886                PhysicsLayerMask.All,
 7887                _continuousCollisionHits,
 7888                Collider,
 7889                includeTriggers: false);
 7890            int mixedHitCount = Context.Settings.RuntimeMode.RunsMixedContacts()
 7891                ? Context.QueryMixed.SweepCircleAgainstStatic3DAll(
 7892                    startPosition,
 7893                    proposedPosition,
 7894                    proxyRadius,
 7895                    Collider.MixedSlabCenterY,
 7896                    Collider.MixedHalfThickness,
 7897                    PhysicsLayerMask.All,
 7898                    _continuousMixedCollisionHits,
 7899                    Collider,
 78100                    includeTriggers: false,
 78101                    cacheTargetPartitions: true,
 78102                    requireClosingContact: true)
 78103                : 0;
 104
 78105            bool found2D = TryGetFirstValidContinuousCollisionHit(startPosition, proposedPosition, proxyRadius, hitCount
 78106            bool foundMixed = TryGetFirstValidMixedContinuousCollisionHit(startPosition, proposedPosition, mixedHitCount
 78107            if (found2D
 78108                && (!foundMixed
 78109                    || ContinuousCollisionCandidateOrdering.Is2DHitFirst(
 78110                        hit2D.Distance,
 78111                        hitMixed.Distance)))
 112            {
 35113                distance = hit2D.Distance;
 35114                return true;
 115            }
 116
 43117            if (foundMixed)
 118            {
 8119                distance = hitMixed.Distance;
 8120                return true;
 121            }
 35122        }
 123        finally
 124        {
 78125            _position = originalPosition;
 78126            Collider.RebuildRuntimeShapeOnly();
 78127        }
 128
 35129        distance = Fixed64.Zero;
 35130        return false;
 43131    }
 132
 133    private bool TryApplyKinematicDynamicContinuousCollisionPushes(
 134        Vector2d startPosition,
 135        Vector2d proposedPosition,
 136        Fixed64 proxyRadius,
 137        Fixed64 maxDistance,
 138        Fixed64 sourceLength,
 139        out bool hasUnresolvedMixedLimit,
 140        out Fixed64 unresolvedMixedDistance)
 141    {
 78142        hasUnresolvedMixedLimit = false;
 78143        unresolvedMixedDistance = maxDistance;
 78144        _continuousCollisionHits.FastClear();
 78145        _continuousMixedCollisionHits.FastClear();
 78146        GatherKinematicDynamic2DContinuousCollisionHits(
 78147            startPosition,
 78148            proposedPosition,
 78149            proxyRadius,
 78150            maxDistance,
 78151            sourceLength);
 78152        GatherKinematicDynamicMixed3DContinuousCollisionHits(
 78153            startPosition,
 78154            proposedPosition,
 78155            maxDistance,
 78156            sourceLength,
 78157            ref hasUnresolvedMixedLimit,
 78158            ref unresolvedMixedDistance);
 159
 78160        Physics2DHitSorter.SortByDistance(_continuousCollisionHits);
 78161        PhysicsMixedHitSorter.SortByDistance(_continuousMixedCollisionHits);
 78162        Vector2d sourceDisplacement = proposedPosition - startPosition;
 78163        int hit2DIndex = 0;
 78164        int hit3DIndex = 0;
 78165        bool pushed = false;
 96166        while (hit2DIndex < _continuousCollisionHits.Count
 96167            || hit3DIndex < _continuousMixedCollisionHits.Count)
 168        {
 18169            bool take2D = hit3DIndex >= _continuousMixedCollisionHits.Count
 18170                || (hit2DIndex < _continuousCollisionHits.Count
 18171                    && _continuousCollisionHits[hit2DIndex].Distance
 18172                        <= _continuousMixedCollisionHits[hit3DIndex].Distance);
 18173            if (take2D)
 174            {
 8175                Physics2DHit hit = _continuousCollisionHits[hit2DIndex++];
 8176                pushed |= ApplyKinematicContinuousCollisionHandoff(
 8177                    hit.Body!,
 8178                    sourceDisplacement,
 8179                    hit.Normal,
 8180                    hit.Distance,
 8181                    sourceLength);
 182
 8183                continue;
 184            }
 185
 10186            PhysicsMixedHit hit3D = _continuousMixedCollisionHits[hit3DIndex++];
 10187            pushed |= ApplyKinematicContinuousCollisionHandoff(
 10188                hit3D.Body3D!,
 10189                sourceDisplacement,
 10190                hit3D.NormalFor2DSource,
 10191                hit3D.Distance,
 10192                sourceLength);
 193        }
 194
 78195        return pushed;
 196    }
 197
 198    private void GatherKinematicDynamic2DContinuousCollisionHits(
 199        Vector2d startPosition,
 200        Vector2d proposedPosition,
 201        Fixed64 proxyRadius,
 202        Fixed64 maxDistance,
 203        Fixed64 sourceLength)
 204    {
 78205        Vector2d sourceDisplacement = proposedPosition - startPosition;
 78206        long token = Context.LateSimulateToken;
 78207        SwiftList<int> candidateIds = Context.Physics2D.QueryPlanarContinuousCollisionCandidates(
 78208            DynamicCcdCandidateIndex2D.CreateSweptCircleBounds(startPosition, sourceDisplacement, proxyRadius));
 336209        for (int candidateIndex = 0; candidateIndex < candidateIds.Count; candidateIndex++)
 210        {
 90211            int dynamicId = candidateIds[candidateIndex];
 90212            SolidBody2D target = Context.Physics2D.GetContinuousCollisionCandidate(dynamicId);
 90213            if (!IsEligibleDynamicContinuousCollisionTarget(target))
 214            {
 215                continue;
 216            }
 217
 11218            target.EnsureContinuousCollisionFramePrepared(token);
 11219            if (!TryGetDynamicRelativeContinuousCollisionHit(
 11220                    target,
 11221                    startPosition,
 11222                    sourceDisplacement,
 11223                    proxyRadius,
 11224                    sourceLength,
 11225                    Fixed64.Zero,
 11226                    out Physics2DHit exactHit,
 11227                    out _))
 228            {
 229                continue;
 230            }
 231
 9232            if (exactHit.Distance > maxDistance)
 233                continue;
 234
 8235            _continuousCollisionHits.Add(exactHit);
 236        }
 78237    }
 238
 239    private void GatherKinematicDynamicMixed3DContinuousCollisionHits(
 240        Vector2d startPosition,
 241        Vector2d proposedPosition,
 242        Fixed64 maxDistance,
 243        Fixed64 sourceLength,
 244        ref bool hasUnresolvedMixedLimit,
 245        ref Fixed64 unresolvedMixedDistance)
 246    {
 78247        if (!Context.Settings.RuntimeMode.RunsMixedContacts())
 49248            return;
 249
 29250        Vector2d sourceDisplacement2D = proposedPosition - startPosition;
 29251        Vector3d sourceStart = new(startPosition.X, Collider.MixedSlabCenterY, startPosition.Y);
 29252        Vector3d sourceDisplacement = new(sourceDisplacement2D.X, Fixed64.Zero, sourceDisplacement2D.Y);
 29253        Fixed64 sourceRadius = ResolveMixedContinuousCollisionProxyRadius();
 29254        long token = Context.LateSimulateToken;
 29255        SwiftList<int> candidateIds = Context.Physics.QueryContinuousCollisionCandidates(
 29256            DynamicCcdCandidateIndex.CreateSweptSphereBounds(sourceStart, sourceDisplacement, sourceRadius));
 94257        for (int candidateIndex = 0; candidateIndex < candidateIds.Count; candidateIndex++)
 258        {
 18259            int dynamicId = candidateIds[candidateIndex];
 18260            SolidBody target = Context.Physics.GetContinuousCollisionCandidate(dynamicId);
 18261            if (!IsEligibleDynamicMixed3DTarget(target))
 262            {
 263                continue;
 264            }
 265
 17266            target.EnsureContinuousCollisionFramePrepared(token);
 17267            IntervalSearchStatus status =
 17268                TryGetDynamicMixed3DContinuousCollisionHit(
 17269                    target,
 17270                    sourceStart,
 17271                    sourceDisplacement,
 17272                    sourceRadius,
 17273                    sourceLength,
 17274                    Fixed64.Zero,
 17275                    out DynamicMixedIntervalHit candidate);
 17276            if (status
 17277                    == IntervalSearchStatus.CertifiedNoHit
 17278                || candidate.SafeDistance > maxDistance)
 279            {
 280                continue;
 281            }
 282
 13283            if (status
 13284                == IntervalSearchStatus.Unresolved)
 285            {
 3286                hasUnresolvedMixedLimit = true;
 3287                unresolvedMixedDistance = FixedMath.Min(
 3288                    unresolvedMixedDistance,
 3289                    candidate.SafeDistance);
 3290                continue;
 291            }
 292
 10293            _continuousMixedCollisionHits.Add(candidate.ExactHit);
 294        }
 29295    }
 296
 297    internal bool ApplyKinematicContinuousCollisionHandoff(
 298        SolidBody2D target,
 299        Vector2d sourceDisplacement,
 300        Vector2d normalForSource,
 301        Fixed64 hitDistance,
 302        Fixed64 sourceLength)
 303    {
 9304        _ = ContinuousCollisionImpulsePolicy.TryResolveSourceNormal(normalForSource, sourceDisplacement, out Vector2d no
 305
 9306        Fixed64 deltaTime = Context.DeltaTime;
 9307        Fixed64 constrainedInverseMass = target.GetConstrainedInverseMass(normal);
 9308        if (constrainedInverseMass <= Fixed64.Zero)
 1309            return false;
 310
 8311        Fixed64 hitTime = FixedMath.Clamp01(hitDistance / sourceLength);
 8312        Vector2d sourceVelocity = SampleContinuousCollisionLinearVelocity(hitTime);
 8313        Vector2d targetVelocity = target.SampleContinuousCollisionLinearVelocity(hitTime);
 8314        bool relativeVelocityResolved = Vector2d.TrySubtract(
 8315            sourceVelocity,
 8316            targetVelocity,
 8317            out Vector2d relativeVelocity);
 8318        Fixed64 normalVelocity = Vector2d.Dot(relativeVelocity, normal);
 8319        Fixed64 restitution = ResolveContinuousCollisionRestitution(target, -normalVelocity);
 8320        bool responseFactorResolved = Fixed64.TryAdd(
 8321            Fixed64.One,
 8322            restitution,
 8323            out Fixed64 responseFactor);
 8324        bool closingSpeedResolved = Fixed64.TrySubtract(
 8325            Fixed64.Zero,
 8326            normalVelocity,
 8327            out Fixed64 closingSpeed);
 8328        bool responseSpeedResolved = Fixed64.TryMultiplyDivide(
 8329            closingSpeed,
 8330            responseFactor,
 8331            Fixed64.One,
 8332            out Fixed64 responseSpeed);
 8333        Vector2d targetResponseNormal = target.ProjectLinearMotion(-normal);
 8334        bool targetDeltaResolved = ContinuousCollisionImpulsePolicy.TryResolveVelocityDelta(
 8335            targetResponseNormal,
 8336            responseSpeed,
 8337            target.EffectiveInverseMass,
 8338            constrainedInverseMass,
 8339            out Vector2d targetVelocityDelta);
 8340        bool targetVelocityResolved = Vector2d.TryAdd(
 8341            targetVelocity,
 8342            target.ProjectLinearMotion(targetVelocityDelta),
 8343            out Vector2d targetPostVelocity);
 8344        if (!(relativeVelocityResolved
 8345            & normalVelocity < -Fixed64.Epsilon
 8346            & responseFactorResolved
 8347            & closingSpeedResolved
 8348            & responseSpeedResolved
 8349            & targetDeltaResolved
 8350            & targetVelocityResolved))
 351        {
 1352            return false;
 353        }
 354
 7355        return target.ApplyContinuousCollisionHandoffState(
 7356            target.SampleContinuousCollisionPosition(hitTime),
 7357            target.SampleContinuousCollisionRotation(hitTime),
 7358            targetPostVelocity,
 7359            target.SampleContinuousCollisionAngularVelocity(hitTime),
 7360            deltaTime * (Fixed64.One - hitTime),
 7361            ignoredCollider2D: Collider);
 362    }
 363
 364    internal bool ApplyKinematicContinuousCollisionHandoff(
 365        SolidBody target,
 366        Vector2d sourceDisplacement,
 367        Vector2d normalForSource,
 368        Fixed64 hitDistance,
 369        Fixed64 sourceLength)
 370    {
 12371        if (!ContinuousCollisionImpulsePolicy.TryResolveImpactNormal(
 12372                normalForSource,
 12373                out Vector2d normal))
 374        {
 3375            return false;
 376        }
 377
 9378        Fixed64 deltaTime = Context.DeltaTime;
 9379        Fixed64 constrainedInverseMass = target.GetConstrainedInverseMass(normal.ToVector3d(Fixed64.Zero));
 9380        if (constrainedInverseMass <= Fixed64.Zero)
 1381            return false;
 382
 8383        Vector3d normal3D = normal.ToVector3d(Fixed64.Zero);
 8384        Fixed64 hitTime = FixedMath.Clamp01(hitDistance / sourceLength);
 8385        Vector3d sourceVelocity = SampleContinuousCollisionLinearVelocity(hitTime)
 8386            .ToVector3d(Fixed64.Zero);
 8387        Vector3d targetVelocity = target.SampleContinuousCollisionLinearVelocity(hitTime);
 8388        bool relativeVelocityResolved = Vector3d.TrySubtract(
 8389            sourceVelocity,
 8390            targetVelocity,
 8391            out Vector3d relativeVelocity);
 8392        Fixed64 normalVelocity = Vector3d.Dot(relativeVelocity, normal3D);
 8393        Fixed64 restitution = ResolveContinuousCollisionRestitution(target, -normalVelocity);
 8394        bool responseFactorResolved = Fixed64.TryAdd(
 8395            Fixed64.One,
 8396            restitution,
 8397            out Fixed64 responseFactor);
 8398        bool closingSpeedResolved = Fixed64.TrySubtract(
 8399            Fixed64.Zero,
 8400            normalVelocity,
 8401            out Fixed64 closingSpeed);
 8402        bool responseSpeedResolved = Fixed64.TryMultiplyDivide(
 8403            closingSpeed,
 8404            responseFactor,
 8405            Fixed64.One,
 8406            out Fixed64 responseSpeed);
 8407        Vector3d targetResponseNormal = target.ProjectLinearMotion(-normal3D);
 8408        bool targetDeltaResolved = ContinuousCollisionImpulsePolicy.TryResolveVelocityDelta(
 8409            targetResponseNormal,
 8410            responseSpeed,
 8411            target.EffectiveInverseMass,
 8412            constrainedInverseMass,
 8413            out Vector3d targetVelocityDelta);
 8414        bool targetVelocityResolved = Vector3d.TryAdd(
 8415            targetVelocity,
 8416            target.ProjectLinearMotion(targetVelocityDelta),
 8417            out Vector3d targetPostVelocity);
 8418        if (!(relativeVelocityResolved
 8419            & normalVelocity < -Fixed64.Epsilon
 8420            & responseFactorResolved
 8421            & closingSpeedResolved
 8422            & responseSpeedResolved
 8423            & targetDeltaResolved
 8424            & targetVelocityResolved))
 425        {
 2426            return false;
 427        }
 428
 6429        return target.ApplyContinuousCollisionHandoff(
 6430            target.SampleContinuousCollisionPosition(hitTime),
 6431            target.SampleContinuousCollisionRotation(hitTime),
 6432            targetPostVelocity,
 6433            target.SampleContinuousCollisionAngularVelocity(hitTime),
 6434            deltaTime * (Fixed64.One - hitTime),
 6435            ignoredCollider2D: Collider);
 436    }
 437
 438}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/2D/Bodies/SolidBody2D.ContinuousCollision.Rotational.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody2D.ContinuousCollision.Rotational.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
 8using System;
 9using FixedMathSharp;
 10using Gravitas.Colliders;
 11using Gravitas.CollisionHandling;
 12using Gravitas.Queries;
 13using Gravitas.Support;
 14using SwiftCollections;
 15using SwiftCollections.Query;
 16
 17namespace Gravitas;
 18
 19/// <content>Searches planar trajectories for the earliest rotational contact and resolves the resulting motion.</conten
 20public sealed partial class SolidBody2D
 21{
 22    /// <summary>Gets whether body rotation leaves this collider's occupied geometry unchanged.</summary>
 23    internal bool HasRotationInvariantCollider =>
 173524        Collider is LSCircleCollider2D && Collider.LocalOffset == Vector2d.Zero;
 25
 26    /// <summary>Reports whether the conservative source sweep admits a rotating target owned by this source.</summary>
 27    internal bool HasNearbyRotationalContinuousCollisionTarget(
 28        Vector2d startPosition,
 29        Vector2d displacement,
 30        Fixed64 pivotRadius)
 31    {
 95432        SwiftList<int> candidateIds = Context.Physics2D.QueryPlanarContinuousCollisionCandidates(
 95433            DynamicCcdCandidateIndex2D.CreateSweptCircleBounds(
 95434                startPosition,
 95435                displacement,
 95436                pivotRadius));
 401037        for (int i = 0; i < candidateIds.Count; i++)
 38        {
 106939            SolidBody2D target = Context.Physics2D.GetContinuousCollisionCandidate(candidateIds[i]);
 106940            if (IsRotatingContinuousCollisionTarget(target))
 41            {
 1842                return true;
 43            }
 44        }
 45
 93646        if (!Context.Settings.RuntimeMode.RunsMixedContacts())
 54947            return false;
 48
 38749        Vector3d start3D = new(startPosition.X, Collider.MixedSlabCenterY, startPosition.Y);
 38750        Vector3d end3D = new(
 38751            startPosition.X + displacement.X,
 38752            Collider.MixedSlabCenterY,
 38753            startPosition.Y + displacement.Y);
 38754        FixedBoundVolume bounds = DynamicCcdCandidateIndex.CreateBoundsBetween(
 38755            start3D,
 38756            end3D,
 38757            new Vector3d(pivotRadius, Collider.MixedHalfThickness, pivotRadius));
 38758        candidateIds = Context.Physics.QueryContinuousCollisionCandidates(bounds);
 97459        for (int i = 0; i < candidateIds.Count; i++)
 60        {
 10961            SolidBody target = Context.Physics.GetContinuousCollisionCandidate(candidateIds[i]);
 10962            if (IsRotatingContinuousCollisionTarget(target))
 963                return true;
 64        }
 65
 37866        return false;
 67    }
 68
 69    private bool IsRotatingContinuousCollisionTarget(SolidBody2D target)
 70    {
 106971        if (ReferenceEquals(target, this)
 106972            || !IsMovingRotationalContinuousCollisionTarget(target))
 73        {
 97574            return false;
 75        }
 76
 9477        target.EnsureContinuousCollisionFramePrepared(Context.LateSimulateToken);
 9478        return !target.HasRotationInvariantCollider && target.HasContinuousCollisionRotationalMotion;
 79    }
 80
 81    private bool IsRotatingContinuousCollisionTarget(SolidBody target)
 82    {
 10983        if (!IsMovingMixedRotationalContinuousCollisionTarget(target))
 6084            return false;
 85
 4986        target.EnsureContinuousCollisionFramePrepared(Context.LateSimulateToken);
 4987        return !target.HasRotationInvariantCollider && target.HasContinuousCollisionRotationalMotion;
 88    }
 89
 90    private bool ShouldUseRotationalContinuousCollisionArbiter(
 91        Vector2d startPosition,
 92        Vector2d proposedPosition,
 93        Fixed64 startRotation,
 94        Fixed64 proposedRotation,
 95        bool forceContinuous)
 96    {
 1401197        ContinuousCollisionMode mode = ContinuousCollisionMode.Continuous;
 1401198        if (!forceContinuous && !ShouldUseContinuousCollision(out mode))
 1287799            return false;
 100
 1134101        Fixed64 pivotRadius = ResolveContinuousCollisionProxyRadius();
 1134102        if (pivotRadius <= Fixed64.Epsilon)
 4103            return false;
 104
 1130105        Vector2d displacement = proposedPosition - startPosition;
 106        // Centered circles do not sweep new geometry when they turn. Nearby
 107        // rotating targets still require the shared rotational arbiter below.
 1130108        Fixed64 angularDistance = HasRotationInvariantCollider
 1130109            ? Fixed64.Zero
 1130110            : (proposedRotation - startRotation).Abs();
 1130111        bool targetRequiresRotationalSampling = angularDistance <= Fixed64.Epsilon
 1130112            && HasNearbyRotationalContinuousCollisionTarget(
 1130113                startPosition,
 1130114                displacement,
 1130115                pivotRadius);
 1130116        if (angularDistance <= Fixed64.Epsilon && !targetRequiresRotationalSampling)
 920117            return false;
 118
 210119        Fixed64 angularArcLength = angularDistance * pivotRadius;
 210120        if (!targetRequiresRotationalSampling
 210121            && angularArcLength <= Fixed64.Epsilon)
 2122            return false;
 208123        if (targetRequiresRotationalSampling)
 26124            return true;
 125
 182126        if (mode != ContinuousCollisionMode.Auto)
 178127            return true;
 128
 4129        return angularArcLength > pivotRadius
 4130            || !ContinuousCollisionMath.IsWithinProxyRadius(
 4131                displacement,
 4132                displacement.MagnitudeSquared,
 4133                pivotRadius);
 134    }
 135
 136    private bool TryResolveRotationalContinuousCollision(
 137        Vector2d startPosition,
 138        ref Vector2d proposedPosition,
 139        Fixed64 startRotation,
 140        ref Fixed64 proposedRotation) =>
 98141        TryResolveRotationalContinuousCollision(
 98142            startPosition,
 98143            ref proposedPosition,
 98144            startRotation,
 98145            ref proposedRotation,
 98146            Context.DeltaTime,
 98147            Fixed64.Zero,
 98148            forceContinuous: false);
 149
 150    private bool TryResolveRotationalContinuousCollision(
 151        Vector2d startPosition,
 152        ref Vector2d proposedPosition,
 153        Fixed64 startRotation,
 154        ref Fixed64 proposedRotation,
 155        Fixed64 remainingTime,
 156        Fixed64 elapsedTime,
 157        bool forceContinuous)
 158    {
 206159        Vector2d originalPosition = _position;
 206160        Fixed64 originalRotation = _rotation;
 206161        LSCollider2D? originalIgnoredCollider = _continuousCollisionHandoffIgnoredCollider2D;
 206162        LSCollider? originalIgnoredMixedCollider = _continuousCollisionHandoffIgnoredCollider3D;
 206163        _resolvedMixedSurfaceFamilies?.Clear();
 206164        bool resolved = false;
 206165        Vector2d currentPosition = startPosition;
 206166        Fixed64 currentRotation = startRotation;
 206167        Fixed64 pivotRadius = ResolveContinuousCollisionProxyRadius();
 206168        Vector2d kinematicLinearVelocity = (proposedPosition - startPosition) / remainingTime;
 206169        Fixed64 kinematicAngularVelocity =
 206170            CanonicalizeRotation(proposedRotation - startRotation) / remainingTime;
 206171        Vector2d motionSegmentStartPosition = startPosition;
 206172        Fixed64 motionSegmentStartRotation = startRotation;
 206173        Fixed64 motionSegmentStartElapsedTime = elapsedTime;
 206174        Vector2d motionSegmentLinearVelocity = IsKinematic
 206175            ? kinematicLinearVelocity
 206176            : ProjectLinearMotion(_linearVelocity);
 206177        Fixed64 motionSegmentAngularVelocity = IsKinematic
 206178            ? kinematicAngularVelocity
 206179            : _angularVelocity;
 206180        int maxToiIterations = Context.Settings.ContinuousCollisionMaxToiIterations;
 206181        int conservativeRefinementCount = 0;
 206182        bool reuseCandidates = false;
 206183        int hitCount = 0;
 184        try
 185        {
 382186            while (remainingTime > Fixed64.Epsilon)
 187            {
 382188                Fixed64 segmentEndElapsedTime = elapsedTime + remainingTime;
 382189                Fixed64 segmentElapsedTime = segmentEndElapsedTime
 382190                    - motionSegmentStartElapsedTime;
 382191                Vector2d segmentEnd = AddPositionDisplacement(
 382192                    motionSegmentStartPosition, ProjectLinearMotion(motionSegmentLinearVelocity) * segmentElapsedTime);
 382193                Fixed64 segmentEndRotation = CanonicalizeRotation(
 382194                    motionSegmentStartRotation
 382195                    + motionSegmentAngularVelocity * segmentElapsedTime);
 382196                Vector2d displacement = segmentEnd - currentPosition;
 382197                Fixed64 angularDelta = CanonicalizeRotation(
 382198                    segmentEndRotation - currentRotation);
 382199                Fixed64 angularDistance = angularDelta.Abs();
 382200                if (angularDistance <= Fixed64.Epsilon)
 201                {
 18202                    angularDelta = Fixed64.Zero;
 18203                    angularDistance = Fixed64.Zero;
 204                }
 382205                if (HasRotationInvariantCollider)
 30206                    angularDistance = Fixed64.Zero;
 382207                if (!reuseCandidates)
 314208                    hitCount = GatherRotationalContinuousCollisionCandidates(
 314209                        currentPosition,
 314210                        segmentEnd,
 314211                        displacement,
 314212                        pivotRadius);
 382213                Fixed64 safeTime = Fixed64.Zero;
 382214                Contact2D contact = default;
 382215                bool hasContact = false;
 382216                Fixed64 contactTime = Fixed64.Zero;
 382217                LSCollider2D? target = null;
 382218                bool foundSameDimension = hitCount != 0
 382219                    && TryFindEarliestRotationalContinuousCollision(
 382220                        currentPosition,
 382221                        displacement,
 382222                        currentRotation,
 382223                        angularDelta,
 382224                        angularDistance,
 382225                        pivotRadius,
 382226                        elapsedTime,
 382227                        remainingTime,
 382228                        out safeTime,
 382229                        out contact,
 382230                        out hasContact,
 382231                        out contactTime,
 382232                        out target);
 382233                bool foundMixed = TryFindEarliestMixedRotationalContinuousCollision(
 382234                    currentPosition,
 382235                    displacement,
 382236                    currentRotation,
 382237                    angularDelta,
 382238                    angularDistance,
 382239                    pivotRadius,
 382240                    elapsedTime,
 382241                    remainingTime,
 382242                    reuseCandidates,
 382243                    out RotationalMixed3DHit mixedHit);
 382244                reuseCandidates = false;
 245                // 2D targets precede 3D targets at equal normalized time.
 382246                bool useMixed = foundMixed
 382247                    && (!foundSameDimension || mixedHit.SafeTime < safeTime);
 382248                if (!foundSameDimension && !foundMixed)
 249                {
 132250                    currentPosition = segmentEnd;
 132251                    currentRotation = segmentEndRotation;
 132252                    break;
 253                }
 254
 250255                Fixed64 conservativeTime = useMixed ? mixedHit.SafeTime : safeTime;
 250256                Fixed64 witnessedTime = useMixed ? mixedHit.ContactTime : contactTime;
 250257                bool responseWitnessIsEarliest = witnessedTime == conservativeTime;
 250258                bool canResolveMovingPair = responseWitnessIsEarliest && (useMixed
 250259                    ? mixedHit.HasClosingContact
 250260                        && TryApplyMixedRotationalContinuousCollisionResponse(
 250261                            mixedHit.Target,
 250262                            mixedHit.Contact,
 250263                            mixedHit.ContactTime,
 250264                            currentPosition,
 250265                            displacement,
 250266                            currentRotation,
 250267                            angularDelta,
 250268                            elapsedTime,
 250269                            remainingTime,
 250270                            manifold: mixedHit.Manifold)
 250271                    : hasContact
 250272                        && target!.Body is SolidBody2D targetBody
 250273                        && TryApplyRotationalContinuousCollisionResponse(
 250274                            targetBody,
 250275                            contact,
 250276                            contactTime,
 250277                            currentPosition,
 250278                            currentRotation,
 250279                            displacement,
 250280                            angularDelta,
 250281                            elapsedTime,
 250282                            remainingTime));
 250283                bool deferUnresolvedWitness = !responseWitnessIsEarliest
 250284                    && witnessedTime > conservativeTime
 250285                    && conservativeTime > Fixed64.Epsilon
 250286                    && conservativeRefinementCount
 250287                        < ContinuousCollisionMath.RotationalIntervalMaxDepth;
 250288                Fixed64 eventTime = canResolveMovingPair
 250289                    ? witnessedTime
 250290                    : conservativeTime;
 250291                Fixed64 consumedTime = remainingTime * eventTime;
 250292                Fixed64 impactElapsedTime = elapsedTime + consumedTime;
 250293                if (!canResolveMovingPair
 250294                    && !deferUnresolvedWitness
 250295                    && (!CanAppendContinuousCollisionFrameSegment(impactElapsedTime)
 250296                        || !Context.Physics2D.TryReserveContinuousCollisionCandidateRefresh(this)))
 297                {
 2298                    LastContinuousCollisionToiIterationLimitReached = true;
 2299                    break;
 300                }
 301
 248302                Fixed64 impactSegmentElapsedTime = impactElapsedTime
 248303                    - motionSegmentStartElapsedTime;
 248304                currentPosition = motionSegmentStartPosition
 248305                    + motionSegmentLinearVelocity * impactSegmentElapsedTime;
 248306                currentRotation = CanonicalizeRotation(
 248307                    motionSegmentStartRotation
 248308                    + motionSegmentAngularVelocity * impactSegmentElapsedTime);
 248309                resolved = true;
 310
 248311                Fixed64 remainingAfterImpact = remainingTime - consumedTime;
 248312                if (deferUnresolvedWitness)
 313                {
 77314                    conservativeRefinementCount++;
 315                    // Pure rotation keeps the candidate proxy unchanged; responses regather.
 77316                    reuseCandidates = displacement == Vector2d.Zero;
 77317                    remainingTime = remainingAfterImpact;
 77318                    elapsedTime = impactElapsedTime;
 77319                    continue;
 320                }
 321
 171322                LastContinuousCollisionToiIterationCount++;
 171323                if (!canResolveMovingPair)
 324                {
 66325                    bool unresolvedInterval = !responseWitnessIsEarliest
 66326                        || !(useMixed ? mixedHit.HasContact : hasContact)
 66327                        || useMixed && !mixedHit.HasClosingContact;
 66328                    if (unresolvedInterval)
 329                    {
 330                        // A later witness cannot supply a response at this frontier.
 331                        // Retain momentum while the stationary tail bounds this frame.
 46332                        LastContinuousCollisionToiIterationLimitReached =
 46333                            remainingAfterImpact > Fixed64.Epsilon;
 46334                        if (LastContinuousCollisionToiIterationLimitReached)
 46335                            Context.Physics2D.ReportContinuousCollisionIterationLimit();
 336                    }
 20337                    else if (!IsKinematic)
 338                    {
 5339                        StopRotationalContinuousCollision(
 5340                            useMixed
 5341                                ? mixedHit.Contact.Normal3DTo2D.ToVector2d()
 5342                                : -contact.Normal);
 343                    }
 66344                    AppendContinuousCollisionFrameSegment(
 66345                        currentPosition,
 66346                        currentRotation,
 66347                        Vector2d.Zero,
 66348                        Fixed64.Zero,
 66349                        impactElapsedTime);
 66350                    Context.Physics2D.RefreshContinuousCollisionCandidate(this);
 66351                    break;
 352                }
 353
 105354                if (!IsKinematic)
 355                {
 54356                    AppendContinuousCollisionFrameSegment(
 54357                        currentPosition,
 54358                        currentRotation,
 54359                        _linearVelocity,
 54360                        _angularVelocity,
 54361                        impactElapsedTime);
 54362                    Context.Physics2D.RefreshContinuousCollisionCandidate(this);
 54363                    motionSegmentStartPosition = currentPosition;
 54364                    motionSegmentStartRotation = currentRotation;
 54365                    motionSegmentStartElapsedTime = impactElapsedTime;
 54366                    motionSegmentLinearVelocity = ProjectLinearMotion(_linearVelocity);
 54367                    motionSegmentAngularVelocity = _angularVelocity;
 368                }
 369
 105370                if (useMixed)
 371                {
 72372                    _continuousCollisionHandoffIgnoredCollider3D = mixedHit.Target;
 72373                    _continuousCollisionHandoffIgnoredCollider2D = null;
 374                }
 375                else
 376                {
 33377                    _resolvedMixedSurfaceFamilies?.Clear();
 33378                    _continuousCollisionHandoffIgnoredCollider2D = target;
 33379                    _continuousCollisionHandoffIgnoredCollider3D = null;
 380                }
 105381                remainingTime = remainingAfterImpact;
 105382                elapsedTime = impactElapsedTime;
 105383                if (LastContinuousCollisionToiIterationCount >= maxToiIterations)
 384                {
 6385                    LastContinuousCollisionToiIterationLimitReached = remainingTime > Fixed64.Epsilon;
 6386                    if (IsKinematic)
 387                    {
 4388                        currentPosition = proposedPosition;
 4389                        currentRotation = proposedRotation;
 390                    }
 4391                    break;
 392                }
 393            }
 394
 206395            proposedPosition = currentPosition;
 206396            proposedRotation = currentRotation;
 206397            return resolved;
 398        }
 399        finally
 400        {
 206401            _resolvedMixedSurfaceFamilies?.Clear();
 206402            _continuousCollisionHandoffIgnoredCollider2D = originalIgnoredCollider;
 206403            _position = originalPosition;
 206404            _rotation = originalRotation;
 206405            Collider.RebuildRuntimeShapeOnly();
 206406            _continuousCollisionHandoffIgnoredCollider3D = originalIgnoredMixedCollider;
 206407        }
 206408    }
 409
 410    private bool TryResolveKinematicRotationalContinuousCollision(
 411        Vector2d startPosition,
 412        ref Vector2d proposedPosition,
 413        Fixed64 startRotation,
 414        ref Fixed64 proposedRotation)
 105415        => TryResolveRotationalContinuousCollision(
 105416            startPosition,
 105417            ref proposedPosition,
 105418            startRotation,
 105419            ref proposedRotation,
 105420            Context.DeltaTime,
 105421            Fixed64.Zero,
 105422            forceContinuous: true);
 423
 424    internal int GatherRotationalContinuousCollisionCandidates(
 425        Vector2d startPosition,
 426        Vector2d proposedPosition,
 427        Vector2d displacement,
 428        Fixed64 pivotRadius)
 429    {
 331430        _rotationalContinuousCollisionCandidateIds.FastClear();
 431        // The live root registry cannot supply another target when it contains only this source.
 331432        if (Context.Physics2D.ColliderCount == 1
 331433            && ReferenceEquals(Context.Physics2D.GetColliderByServiceIndex(0), Collider))
 434        {
 187435            _continuousCollisionHits.Clear();
 187436            return 0;
 437        }
 438
 144439        int staticHitCount = displacement.MagnitudeSquared <= Fixed64.Epsilon
 144440            ? Context.Query2D.OverlapCircleAgainstStaticAll(
 144441                startPosition,
 144442                pivotRadius,
 144443                PhysicsLayerMask.All,
 144444                _continuousCollisionHits,
 144445                Collider,
 144446                includeTriggers: false)
 144447            : Context.Query2D.SweepCircleAgainstStaticAll(
 144448                startPosition,
 144449                proposedPosition,
 144450                pivotRadius,
 144451                PhysicsLayerMask.All,
 144452                _continuousCollisionHits,
 144453                Collider,
 144454                includeTriggers: false);
 455
 144456        SwiftList<int> candidateIds = Context.Physics2D.QueryPlanarContinuousCollisionCandidates(
 144457            DynamicCcdCandidateIndex2D.CreateSweptCircleBounds(
 144458                startPosition,
 144459                displacement,
 144460                pivotRadius));
 144461        _rotationalContinuousCollisionCandidateIds.EnsureCapacity(candidateIds.Count);
 726462        for (int i = 0; i < candidateIds.Count; i++)
 463        {
 219464            int candidateId = candidateIds[i];
 219465            if (candidateId != DynamicId)
 79466                _rotationalContinuousCollisionCandidateIds.Add(candidateId);
 467        }
 468
 144469        return staticHitCount + _rotationalContinuousCollisionCandidateIds.Count;
 470    }
 471
 472    private bool TryFindEarliestRotationalContinuousCollision(
 473        Vector2d startPosition,
 474        Vector2d displacement,
 475        Fixed64 startRotation,
 476        Fixed64 angularDelta,
 477        Fixed64 angularDistance,
 478        Fixed64 pivotRadius,
 479        Fixed64 elapsedTime,
 480        Fixed64 remainingTime,
 481        out Fixed64 safeTime,
 482        out Contact2D contact,
 483        out bool hasContact,
 484        out Fixed64 contactTime,
 485        out LSCollider2D? hitTarget)
 486    {
 153487        safeTime = Fixed64.Zero;
 153488        contact = default;
 153489        hasContact = false;
 153490        contactTime = Fixed64.Zero;
 153491        hitTarget = null;
 492
 153493        bool foundCollision = false;
 153494        Fixed64 earliestTime = Fixed64.One;
 153495        int earliestTargetId = int.MaxValue;
 153496        Contact2D earliestContact = default;
 153497        bool earliestHasContact = false;
 153498        Fixed64 earliestContactTime = Fixed64.Zero;
 153499        LSCollider2D? earliestTarget = null;
 500
 153501        int staticHitCount = _continuousCollisionHits.Count;
 504502        for (int hitIndex = 0; hitIndex < staticHitCount; hitIndex++)
 503        {
 99504            Physics2DHit hit = _continuousCollisionHits[hitIndex];
 99505            LSCollider2D target = hit.Collider;
 99506            if (!IsValidContinuousCollisionTarget(target)
 99507                || CanExcludeInvariantTangentialContact(hit, displacement)
 99508                || !TryFindEarliestRotationalContinuousCollisionAgainstTarget(
 99509                    target,
 99510                    startPosition,
 99511                    displacement,
 99512                    startRotation,
 99513                    angularDelta,
 99514                    angularDistance,
 99515                    pivotRadius,
 99516                    elapsedTime,
 99517                    remainingTime,
 99518                    out Fixed64 candidateTime,
 99519                    out Contact2D candidateContact,
 99520                    out bool candidateHasContact,
 99521                    out Fixed64 candidateContactTime))
 522            {
 523                continue;
 524            }
 525
 45526            if (!ContinuousCollisionMath.ShouldReplaceContinuousCollisionHit(
 45527                    candidateTime,
 45528                    target.Id,
 45529                    foundCollision,
 45530                    earliestTime,
 45531                    earliestTargetId))
 532            {
 533                continue;
 534            }
 535
 44536            foundCollision = true;
 44537            earliestTime = candidateTime;
 44538            earliestTargetId = target.Id;
 44539            earliestContact = candidateContact;
 44540            earliestHasContact = candidateHasContact;
 44541            earliestContactTime = candidateContactTime;
 44542            earliestTarget = target;
 543        }
 544
 153545        for (int candidateIndex = 0;
 244546            candidateIndex < _rotationalContinuousCollisionCandidateIds.Count;
 91547            candidateIndex++)
 548        {
 91549            int dynamicId = _rotationalContinuousCollisionCandidateIds[candidateIndex];
 91550            SolidBody2D targetBody = Context.Physics2D.GetContinuousCollisionCandidate(dynamicId);
 91551            if (!IsMovingRotationalContinuousCollisionTarget(targetBody)
 91552                || ColliderSettings2D.GetCollisionType(
 91553                    Collider.Shape,
 91554                    targetBody.Collider.Shape) == CollisionType2D.None
 91555                || !TryFindEarliestRotationalContinuousCollisionAgainstTarget(
 91556                    targetBody.Collider,
 91557                    startPosition,
 91558                    displacement,
 91559                    startRotation,
 91560                    angularDelta,
 91561                    angularDistance,
 91562                    pivotRadius,
 91563                    elapsedTime,
 91564                    remainingTime,
 91565                    out Fixed64 candidateTime,
 91566                    out Contact2D candidateContact,
 91567                    out bool candidateHasContact,
 91568                    out Fixed64 candidateContactTime))
 569            {
 570                continue;
 571            }
 572
 45573            if (!ContinuousCollisionMath.ShouldReplaceContinuousCollisionHit(
 45574                    candidateTime,
 45575                    targetBody.Collider.Id,
 45576                    foundCollision,
 45577                    earliestTime,
 45578                    earliestTargetId))
 579            {
 580                continue;
 581            }
 582
 43583            foundCollision = true;
 43584            earliestTime = candidateTime;
 43585            earliestTargetId = targetBody.Collider.Id;
 43586            earliestContact = candidateContact;
 43587            earliestHasContact = candidateHasContact;
 43588            earliestContactTime = candidateContactTime;
 43589            earliestTarget = targetBody.Collider;
 590        }
 591
 153592        if (!foundCollision)
 68593            return false;
 594
 85595        safeTime = earliestTime;
 85596        contact = earliestContact;
 85597        hasContact = earliestHasContact;
 85598        contactTime = earliestContactTime;
 85599        hitTarget = earliestTarget;
 85600        return true;
 601    }
 602
 603    private bool TryFindEarliestRotationalContinuousCollisionAgainstTarget(
 604        LSCollider2D target,
 605        Vector2d startPosition,
 606        Vector2d displacement,
 607        Fixed64 startRotation,
 608        Fixed64 angularDelta,
 609        Fixed64 angularDistance,
 610        Fixed64 pivotRadius,
 611        Fixed64 elapsedTime,
 612        Fixed64 remainingTime,
 613        out Fixed64 safeTime,
 614        out Contact2D contact,
 615        out bool hasContact,
 616        out Fixed64 contactTime)
 617    {
 137618        safeTime = Fixed64.Zero;
 137619        contact = default;
 137620        hasContact = false;
 137621        contactTime = Fixed64.Zero;
 137622        SolidBody2D? targetBody = target.Body;
 137623        bool samplesTargetMotion = targetBody != null
 137624            && IsMovingRotationalContinuousCollisionTarget(targetBody);
 137625        Vector2d originalTargetPosition = targetBody?._position ?? Vector2d.Zero;
 137626        Fixed64 originalTargetRotation = targetBody?._rotation ?? Fixed64.Zero;
 137627        if (samplesTargetMotion)
 61628            targetBody!.EnsureContinuousCollisionFramePrepared(Context.LateSimulateToken);
 629
 630        try
 631        {
 137632            Span<RotationalInterval> intervals =
 137633                stackalloc RotationalInterval[
 137634                    ContinuousCollisionMath.RotationalIntervalMaxDepth + 2];
 137635            int intervalCount = 1;
 137636            int processedNodeCount = 0;
 137637            bool hasKnownContact = false;
 137638            Fixed64 knownContactTime = Fixed64.One;
 137639            Contact2D knownContact = default;
 137640            intervals[0] = new RotationalInterval(
 137641                Fixed64.Zero,
 137642                Fixed64.One,
 137643                depth: 0);
 644
 2123645            while (intervalCount > 0)
 646            {
 2077647                RotationalInterval interval = intervals[--intervalCount];
 2077648                Fixed64 midpoint = (interval.LowerTime + interval.UpperTime) * Fixed64.Half;
 2077649                Fixed64 intervalSpan = interval.UpperTime - interval.LowerTime;
 2077650                Fixed64 midpointFrameFraction = ResolveRotationalFrameFraction(
 2077651                    elapsedTime,
 2077652                    remainingTime,
 2077653                    midpoint);
 2077654                SampleRotationalContinuousPairPose(
 2077655                    startPosition,
 2077656                    displacement,
 2077657                    startRotation,
 2077658                    angularDelta,
 2077659                    midpoint,
 2077660                    targetBody,
 2077661                    midpointFrameFraction,
 2077662                    samplesTargetMotion);
 2077663                processedNodeCount++;
 664
 2077665                bool hasMotionBound = ContinuousCollisionMath.TryResolveRotationalIntervalMotionBound(
 2077666                    displacement,
 2077667                    angularDistance,
 2077668                    pivotRadius,
 2077669                    intervalSpan,
 2077670                    out Fixed64 motionBound);
 2077671                if (hasMotionBound && samplesTargetMotion)
 672                {
 849673                    Fixed64 lowerFrameFraction = ResolveRotationalFrameFraction(
 849674                        elapsedTime,
 849675                        remainingTime,
 849676                        interval.LowerTime);
 849677                    Fixed64 upperFrameFraction = ResolveRotationalFrameFraction(
 849678                        elapsedTime,
 849679                        remainingTime,
 849680                        interval.UpperTime);
 849681                    Fixed64 targetRadius = targetBody!.ResolveContinuousCollisionProxyRadius();
 849682                    bool targetBoundResolved = targetBody.TryResolveContinuousCollisionMotionBound(
 849683                        lowerFrameFraction,
 849684                        upperFrameFraction,
 849685                        targetRadius,
 849686                        out Fixed64 targetMotionBound);
 849687                    bool combinedBoundResolved = Fixed64.TryAdd(
 849688                        motionBound,
 849689                        targetMotionBound,
 849690                        out motionBound);
 849691                    hasMotionBound = targetBoundResolved & combinedBoundResolved;
 692                }
 2077693                bool sampleHasContact = CollisionDetection2D.TryCollide(
 2077694                    Collider,
 2077695                    target,
 2077696                    out Contact2D sampleContact);
 2077697                if (sampleHasContact
 2077698                    && (!hasKnownContact || midpoint < knownContactTime))
 699                {
 679700                    hasKnownContact = true;
 679701                    knownContactTime = midpoint;
 679702                    knownContact = sampleContact;
 703                }
 704
 2077705                if (!sampleHasContact
 2077706                    && hasMotionBound
 2077707                    && IsRotationalIntervalSeparated(target, motionBound))
 708                {
 709                    continue;
 710                }
 711
 1437712                if (ContinuousCollisionMath.TryResolveRotationalSearchLimit(
 1437713                        interval,
 1437714                        processedNodeCount,
 1437715                        hasKnownContact,
 1437716                        knownContactTime,
 1437717                        out safeTime,
 1437718                        out contactTime,
 1437719                        out hasContact))
 720                {
 91721                    contact = knownContact;
 91722                    return true;
 723                }
 724
 1346725                int childDepth = interval.Depth + 1;
 1346726                if (sampleHasContact)
 727                {
 728                    // A real midpoint contact is already an upper bound. Only the
 729                    // earlier half can contain an earlier first contact.
 647730                    intervals[intervalCount++] = new RotationalInterval(
 647731                        interval.LowerTime,
 647732                        midpoint,
 647733                        childDepth);
 647734                    continue;
 735                }
 736
 699737                intervals[intervalCount++] = new RotationalInterval(
 699738                    midpoint,
 699739                    interval.UpperTime,
 699740                    childDepth);
 699741                intervals[intervalCount++] = new RotationalInterval(
 699742                    interval.LowerTime,
 699743                    midpoint,
 699744                    childDepth);
 745            }
 746
 46747            safeTime = knownContactTime;
 46748            contact = knownContact;
 46749            hasContact = hasKnownContact;
 46750            contactTime = knownContactTime;
 46751            return hasKnownContact;
 752        }
 753        finally
 754        {
 137755            if (targetBody != null)
 756            {
 128757                targetBody._position = originalTargetPosition;
 128758                targetBody._rotation = originalTargetRotation;
 128759                targetBody.Collider.RebuildRuntimeShapeOnly();
 760            }
 137761        }
 137762    }
 763
 764    private bool CanExcludeInvariantTangentialContact(Physics2DHit hit, Vector2d displacement) =>
 84765        HasRotationInvariantCollider
 84766        && hit.Collider is LSAABBoxCollider2D or LSCircleCollider2D or LSCapsuleCollider2D or LSPolygonCollider2D
 84767        && hit.Normal != Vector2d.Zero
 84768        && !IsClosingContinuousCollisionHit(displacement, hit.Normal);
 769
 770    private bool IsRotationalIntervalSeparated(LSCollider2D target, Fixed64 motionBound)
 771    {
 1398772        if (TryGetCirclePairSeparationGap(target, out Fixed64 closestPointGap))
 1020773            return closestPointGap > motionBound;
 774
 378775        return ContinuousCollisionBounds.AreSeparated(Collider, target, motionBound);
 776    }
 777
 778    private bool TryGetCirclePairSeparationGap(LSCollider2D target, out Fixed64 separationGap)
 779    {
 1398780        if (Collider is LSCircleCollider2D sourceCircle)
 82781            return TryGetCircleSeparationGap(sourceCircle, target, out separationGap);
 782
 1316783        if (target is LSCircleCollider2D targetCircle)
 938784            return TryGetCircleSeparationGap(targetCircle, Collider, out separationGap);
 785
 378786        separationGap = default;
 378787        return false;
 788    }
 789
 790    internal static bool TryGetCircleSeparationGap(
 791        LSCircleCollider2D circle,
 792        LSCollider2D other,
 793        out Fixed64 separationGap)
 794    {
 1024795        LSCircleCollider2D? otherCircle = other as LSCircleCollider2D;
 1024796        Fixed64 otherRadius = otherCircle?.ScaledRadius ?? Fixed64.Zero;
 797        Fixed64 distance;
 1024798        if (otherCircle != null)
 799        {
 26800            if (!Vector2d.TryGetDistance(
 26801                    circle.Center,
 26802                    otherCircle.Center,
 26803                    out distance))
 804            {
 1805                separationGap = default;
 1806                return false;
 807            }
 808        }
 998809        else if (other.ContainsPoint(circle.Center))
 810        {
 1811            distance = Fixed64.Zero;
 812        }
 997813        else if (!other.TryGetClosestBoundaryAnchor(
 997814                     circle.Center,
 997815                     out _,
 997816                     out distance))
 817        {
 1818            separationGap = default;
 1819            return false;
 820        }
 821
 822        // An unrepresentable center distance or radius sum cannot certify
 823        // separation. The uncertainty scale below may saturate only upward.
 1022824        if (!Fixed64.TryAdd(
 1022825                circle.ScaledRadius,
 1022826                otherRadius,
 1022827                out Fixed64 combinedRadius))
 828        {
 1829            separationGap = default;
 1830            return false;
 831        }
 832
 1021833        Fixed64 rawGap = distance - combinedRadius;
 1021834        Fixed64 characteristicScale = distance
 1021835            + combinedRadius
 1021836            + Vector2d.GetMagnitude(other.Bounds.Size);
 1021837        _ = ContinuousCollisionMath.TrySubtractClosestFeatureUncertainty(
 1021838            rawGap,
 1021839            characteristicScale,
 1021840            out separationGap);
 841
 1021842        return separationGap > Fixed64.Zero;
 843    }
 844
 845    private void SampleRotationalContinuousPairPose(
 846        Vector2d startPosition,
 847        Vector2d displacement,
 848        Fixed64 startRotation,
 849        Fixed64 angularDelta,
 850        Fixed64 sampleTime,
 851        SolidBody2D? targetBody,
 852        Fixed64 targetFrameFraction,
 853        bool samplesTargetMotion)
 854    {
 2077855        _position = startPosition + displacement * sampleTime;
 2077856        _rotation = startRotation + angularDelta * sampleTime;
 2077857        Collider.RebuildRuntimeShapeOnly();
 858
 2077859        if (!samplesTargetMotion)
 1228860            return;
 861
 849862        targetBody!._position = targetBody.SampleContinuousCollisionPosition(targetFrameFraction);
 849863        targetBody._rotation = targetBody.SampleContinuousCollisionRotation(targetFrameFraction);
 849864        targetBody.Collider.RebuildRuntimeShapeOnly();
 849865    }
 866
 867    private bool IsMovingRotationalContinuousCollisionTarget(SolidBody2D target)
 868    {
 869        // Query/index admission has already validated an active, nonself target.
 353870        if (target.IsKinematic)
 93871            target.EnsureContinuousCollisionFramePrepared(Context.LateSimulateToken);
 872
 353873        return (IsKinematic || !target.ShouldOwnContinuousCollisionMovingPair(
 353874                HasContinuousCollisionRotationalMotion))
 353875            && !ContinuousCollisionCandidateOrdering.IsIgnoredTarget(
 353876                target.Collider,
 353877                _continuousCollisionHandoffIgnoredCollider2D)
 353878            && !target.Collider.IsTrigger
 353879            && Context.Physics2D.RequireCollisionPair(Collider, target.Collider)
 353880            && (target.IsKinematic
 353881                ? target.HasContinuousCollisionMotion
 353882                : target.IsDynamic);
 883    }
 884
 885}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/2D/Bodies/SolidBody2D.ContinuousCollision.Rotational.Mixed.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody2D.ContinuousCollision.Rotational.Mixed.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
 8using System;
 9using FixedMathSharp;
 10using FixedMathSharp.Geometry;
 11using Gravitas.Colliders;
 12using Gravitas.CollisionHandling;
 13using Gravitas.Support;
 14using SwiftCollections;
 15using SwiftCollections.Query;
 16
 17namespace Gravitas;
 18
 19/// <content>Searches rotating planar trajectories against spatial targets and resolves mixed contact manifolds.</conten
 20public sealed partial class SolidBody2D
 21{
 644822    private readonly ContactManifold _rotationalMixedKnownManifold = new();
 644823    private readonly ContactManifold _rotationalMixedEarliestManifold = new();
 24    private bool _rotationalMixedKnownContactIsClosing;
 25    private bool _rotationalMixedEarliestContactIsClosing;
 26    private ContactWarmStartCache _mixedContinuousCollisionImpulses;
 27    private ExactContactImpulseState?[]? _mixedContinuousCollisionExactImpulses;
 644828    private readonly SwiftList<LSCollider> _rotationalMixedStatic3DCandidates = new();
 644829    private readonly SwiftList<int> _rotationalMixedMoving3DCandidateIds = new();
 30    private SwiftHashSet<StaticSurface>? _rotationalMixedSurfaceCandidates;
 31    private SwiftList<(LSMeshCollider Supplier, int Owner)>? _resolvedMixedSurfaceFamilies;
 32
 33    private ReadOnlySpan<(LSMeshCollider Supplier, int Owner)> ResolvedMixedSurfaceFamilies(StaticSurface surface) =>
 20234        _resolvedMixedSurfaceFamilies is { } families
 20235            && _continuousCollisionHandoffIgnoredCollider3D is LSMeshCollider previous
 20236            && previous.StaticSurface == surface ? families.AsReadOnlySpan() : default;
 37
 38    internal void SetMixedContinuousCollisionSamplePose(
 39        Vector2d position,
 40        Fixed64 rotation)
 41    {
 594742        _position = position;
 594743        _rotation = rotation;
 594744        Collider.RebuildRuntimeShapeOnly();
 594745    }
 46
 47    internal FixedBoundVolume ResolveMixedContinuousCollisionTrajectoryBounds(Fixed64 radius)
 48    {
 651349        DynamicCcdPlanarBounds planarBounds =
 651350            ResolveContinuousCollisionTrajectoryBounds(radius);
 651351        return new FixedBoundVolume(
 651352            new Vector3d(
 651353                planarBounds.MinX,
 651354                Collider.MixedSlabCenterY - Collider.MixedHalfThickness,
 651355                planarBounds.MinZ),
 651356            new Vector3d(
 651357                planarBounds.MaxX,
 651358                Collider.MixedSlabCenterY + Collider.MixedHalfThickness,
 651359                planarBounds.MaxZ));
 60    }
 61
 62    private readonly struct RotationalMixed3DHit
 63    {
 64        public RotationalMixed3DHit(
 65            Fixed64 safeTime,
 66            MixedContact contact,
 67            bool hasContact,
 68            bool hasClosingContact,
 69            Fixed64 contactTime,
 70            LSCollider target,
 71            ContactManifold manifold)
 72        {
 17073            SafeTime = safeTime;
 17074            Contact = contact;
 17075            HasContact = hasContact;
 17076            HasClosingContact = hasClosingContact;
 17077            ContactTime = contactTime;
 17078            Target = target;
 17079            Manifold = manifold;
 17080        }
 81
 82        public Fixed64 SafeTime { get; }
 83
 84        public MixedContact Contact { get; }
 85
 86        public bool HasContact { get; }
 87        public bool HasClosingContact { get; }
 88
 89        public Fixed64 ContactTime { get; }
 90
 91        public LSCollider Target { get; }
 92        public ContactManifold Manifold { get; }
 93    }
 94
 95    private bool TryFindEarliestMixedRotationalContinuousCollision(
 96        Vector2d startPosition,
 97        Vector2d displacement,
 98        Fixed64 startRotation,
 99        Fixed64 angularDelta,
 100        Fixed64 angularDistance,
 101        Fixed64 pivotRadius,
 102        Fixed64 elapsedTime,
 103        Fixed64 remainingTime,
 104        bool reuseCandidates,
 105        out RotationalMixed3DHit hit)
 106    {
 382107        hit = default;
 382108        if (!Context.Settings.RuntimeMode.RunsMixedContacts())
 143109            return false;
 110
 239111        _rotationalMixedSurfaceCandidates?.Clear();
 112        try
 113        {
 239114            if (!reuseCandidates)
 194115                GatherMixedRotationalContinuousCollisionCandidates(
 194116                    startPosition,
 194117                    displacement,
 194118                    pivotRadius);
 119
 239120            bool found = false;
 239121            Fixed64 earliestTime = Fixed64.One;
 239122            int earliestOrder = int.MaxValue;
 239123            MixedContact earliestContact = default;
 239124            bool earliestHasContact = false;
 239125            Fixed64 earliestContactTime = Fixed64.Zero;
 239126            LSCollider? earliestTarget = null;
 127
 762128            for (int i = 0; i < _rotationalMixedStatic3DCandidates.Count; i++)
 129            {
 142130                LSCollider target = _rotationalMixedStatic3DCandidates[i];
 142131                if (target is LSMeshCollider { StaticSurface: { } surface }
 142132                    && (!IsValidMixedRotationalContinuousCollisionTarget(target)
 142133                        || !(_rotationalMixedSurfaceCandidates ??= new()).Add(surface))) continue;
 94134                ConsiderMixedRotationalContinuousCollisionCandidate(
 94135                    target,
 94136                    startPosition,
 94137                    displacement,
 94138                    startRotation,
 94139                    angularDelta,
 94140                    angularDistance,
 94141                    pivotRadius,
 94142                    elapsedTime,
 94143                    remainingTime,
 94144                    ref found,
 94145                    ref earliestTime,
 94146                    ref earliestOrder,
 94147                    ref earliestContact,
 94148                    ref earliestHasContact,
 94149                    ref earliestContactTime,
 94150                    ref earliestTarget);
 151            }
 152
 802153            for (int i = 0; i < _rotationalMixedMoving3DCandidateIds.Count; i++)
 154            {
 162155                SolidBody targetBody = Context.Physics.GetContinuousCollisionCandidate(
 162156                    _rotationalMixedMoving3DCandidateIds[i]);
 162157                if (!IsMovingMixedRotationalContinuousCollisionTarget(targetBody))
 158                {
 159                    continue;
 160                }
 161
 112162                ConsiderMixedRotationalContinuousCollisionCandidate(
 112163                    targetBody.Collider,
 112164                    startPosition,
 112165                    displacement,
 112166                    startRotation,
 112167                    angularDelta,
 112168                    angularDistance,
 112169                    pivotRadius,
 112170                    elapsedTime,
 112171                    remainingTime,
 112172                    ref found,
 112173                    ref earliestTime,
 112174                    ref earliestOrder,
 112175                    ref earliestContact,
 112176                    ref earliestHasContact,
 112177                    ref earliestContactTime,
 112178                    ref earliestTarget);
 179            }
 180
 239181            if (!found)
 69182                return false;
 183
 170184            hit = new RotationalMixed3DHit(
 170185                earliestTime,
 170186                earliestContact,
 170187                earliestHasContact,
 170188                _rotationalMixedEarliestContactIsClosing,
 170189                earliestContactTime,
 170190                earliestTarget!,
 170191                _rotationalMixedEarliestManifold);
 170192            return true;
 193        }
 478194        finally { _rotationalMixedSurfaceCandidates?.Clear(); }
 239195    }
 196
 197    private void GatherMixedRotationalContinuousCollisionCandidates(
 198        Vector2d startPosition,
 199        Vector2d displacement,
 200        Fixed64 pivotRadius)
 201    {
 194202        _rotationalMixedStatic3DCandidates.FastClear();
 194203        _rotationalMixedMoving3DCandidateIds.FastClear();
 194204        Vector3d start3D = new(
 194205            startPosition.X,
 194206            Collider.MixedSlabCenterY,
 194207            startPosition.Y);
 194208        Vector3d end3D = new(
 194209            startPosition.X + displacement.X,
 194210            Collider.MixedSlabCenterY,
 194211            startPosition.Y + displacement.Y);
 194212        FixedBoundVolume bounds = DynamicCcdCandidateIndex.CreateBoundsBetween(
 194213            start3D,
 194214            end3D,
 194215            new Vector3d(
 194216                pivotRadius,
 194217                Collider.MixedHalfThickness,
 194218                pivotRadius));
 194219        Context.MixedCollisions.Collect3DCandidatesInMixedBounds(
 194220            bounds.Min,
 194221            bounds.Max,
 194222            PhysicsLayerMask.All,
 194223            _rotationalMixedStatic3DCandidates,
 194224            staticStyleOnly: true,
 194225            cachePartitionRefresh: true);
 194226        _rotationalMixedStatic3DCandidates.SortInPlace(default(ReplayOrderComparer<LSCollider>));
 227
 194228        SwiftList<int> candidateIds =
 194229            Context.Physics.QueryContinuousCollisionCandidates(bounds);
 194230        _rotationalMixedMoving3DCandidateIds.EnsureCapacity(candidateIds.Count);
 622231        for (int i = 0; i < candidateIds.Count; i++)
 117232            _rotationalMixedMoving3DCandidateIds.Add(candidateIds[i]);
 194233    }
 234
 235    private void ConsiderMixedRotationalContinuousCollisionCandidate(
 236        LSCollider target,
 237        Vector2d startPosition,
 238        Vector2d displacement,
 239        Fixed64 startRotation,
 240        Fixed64 angularDelta,
 241        Fixed64 angularDistance,
 242        Fixed64 pivotRadius,
 243        Fixed64 elapsedTime,
 244        Fixed64 remainingTime,
 245        ref bool found,
 246        ref Fixed64 earliestTime,
 247        ref int earliestOrder,
 248        ref MixedContact earliestContact,
 249        ref bool earliestHasContact,
 250        ref Fixed64 earliestContactTime,
 251        ref LSCollider? earliestTarget)
 252    {
 253        // Kinematic clamps against immovable targets consume geometry/time only;
 254        // dynamic stopping and movable-target response still require full contacts.
 206255        int candidateOrder = target is LSMeshCollider { StaticSurface: { } surface }
 206256            ? surface.ContinuousOrder : ((IPhysicsColliderRegistryItem)target).ReplayOrder;
 206257        if (!IsValidMixedRotationalContinuousCollisionTarget(target)
 206258            || !TryFindEarliestMixedRotationalContinuousCollisionAgainstTarget(
 206259                target,
 206260                startPosition,
 206261                displacement,
 206262                startRotation,
 206263                angularDelta,
 206264                angularDistance,
 206265                pivotRadius,
 206266                elapsedTime,
 206267                remainingTime,
 206268                Fixed64.Zero,
 206269                Fixed64.One,
 206270                needsResponseContact: !IsKinematic || target.Body?.HasSolverMobility == true,
 206271                out Fixed64 candidateTime,
 206272                out MixedContact candidateContact,
 206273                out bool candidateHasContact,
 206274                out Fixed64 candidateContactTime)
 206275            || !ContinuousCollisionMath.ShouldReplaceContinuousCollisionHit(
 206276                candidateTime,
 206277                candidateOrder,
 206278                found,
 206279                earliestTime,
 206280                earliestOrder))
 281        {
 36282            return;
 283        }
 284
 170285        found = true;
 170286        earliestTime = candidateTime;
 170287        earliestOrder = candidateOrder;
 170288        earliestContact = candidateContact;
 170289        earliestHasContact = candidateHasContact;
 170290        earliestContactTime = candidateContactTime;
 170291        earliestTarget = target;
 170292        _rotationalMixedEarliestManifold.CopyFrom(_rotationalMixedKnownManifold, Context.FrameCount);
 170293        _rotationalMixedEarliestContactIsClosing = _rotationalMixedKnownContactIsClosing;
 170294    }
 295
 296    private bool TryFindEarliestMixedRotationalContinuousCollisionAgainstTarget(
 297        LSCollider target,
 298        Vector2d startPosition,
 299        Vector2d displacement,
 300        Fixed64 startRotation,
 301        Fixed64 angularDelta,
 302        Fixed64 angularDistance,
 303        Fixed64 pivotRadius,
 304        Fixed64 elapsedTime,
 305        Fixed64 remainingTime,
 306        Fixed64 searchStart,
 307        Fixed64 searchEnd,
 308        bool needsResponseContact,
 309        out Fixed64 safeTime,
 310        out MixedContact contact,
 311        out bool hasContact,
 312        out Fixed64 contactTime)
 313    {
 240314        safeTime = Fixed64.Zero;
 240315        contact = default;
 240316        hasContact = false;
 240317        contactTime = Fixed64.Zero;
 240318        _rotationalMixedKnownManifold.BeginUpdate(Context.FrameCount);
 240319        _rotationalMixedKnownContactIsClosing = false;
 240320        StaticSurface? surface = (target as LSMeshCollider)?.StaticSurface;
 240321        ReadOnlySpan<(LSMeshCollider Supplier, int Owner)> resolved = surface == null
 240322            ? default : ResolvedMixedSurfaceFamilies(surface);
 240323        SolidBody? targetBody = target.Body;
 240324        bool samplesTargetMotion = targetBody != null
 240325            && IsMovingMixedRotationalContinuousCollisionTarget(targetBody);
 240326        Vector3d originalTargetPosition = targetBody?.Position3d ?? Vector3d.Zero;
 240327        FixedQuaternion originalTargetRotation = targetBody?.Rotation
 240328            ?? FixedQuaternion.Identity;
 240329        if (samplesTargetMotion)
 158330            targetBody!.EnsureContinuousCollisionFramePrepared(Context.LateSimulateToken);
 331
 332        try
 333        {
 240334            SampleMixedRotationalContinuousPairPose(startPosition, displacement, startRotation, angularDelta,
 240335                searchStart, elapsedTime, remainingTime, targetBody, samplesTargetMotion);
 336            // Resolve an actual closing start before midpoint refinement can
 337            // introduce penetration. Retain the complete native matched rows.
 240338            if (needsResponseContact && (surface?.IntersectsMixedContinuous(Collider, resolved: resolved)
 240339                    ?? CollisionDetectionMixed.IntersectsForContinuousCollision(target, Collider)))
 340            {
 20341                BuildMixedRotationalContinuousContact(target,
 20342                    ResolveRotationalFrameFraction(elapsedTime, remainingTime, searchStart),
 20343                    out contact, out _rotationalMixedKnownContactIsClosing);
 20344                if (_rotationalMixedKnownContactIsClosing)
 345                {
 14346                    _rotationalMixedKnownManifold.CopyFrom(Context.CollisionScratch.MixedManifold, Context.FrameCount);
 14347                    safeTime = contactTime = searchStart;
 14348                    hasContact = true;
 14349                    return true;
 350                }
 351            }
 226352            Span<RotationalInterval> intervals =
 226353                stackalloc RotationalInterval[
 226354                    ContinuousCollisionMath.RotationalIntervalMaxDepth + 2];
 226355            int intervalCount = 1;
 226356            int processedNodeCount = 0;
 226357            bool hasKnownContact = false;
 226358            Fixed64 knownContactTime = Fixed64.One;
 226359            intervals[0] = new RotationalInterval(
 226360                searchStart,
 226361                searchEnd,
 226362                depth: 0);
 363
 5771364            while (intervalCount > 0)
 365            {
 5749366                RotationalInterval interval = intervals[--intervalCount];
 5749367                Fixed64 midpoint =
 5749368                    (interval.LowerTime + interval.UpperTime) * Fixed64.Half;
 5749369                Fixed64 intervalSpan = interval.UpperTime - interval.LowerTime;
 5749370                SampleMixedRotationalContinuousPairPose(
 5749371                    startPosition,
 5749372                    displacement,
 5749373                    startRotation,
 5749374                    angularDelta,
 5749375                    midpoint,
 5749376                    elapsedTime,
 5749377                    remainingTime,
 5749378                    targetBody,
 5749379                    samplesTargetMotion);
 5749380                processedNodeCount++;
 381
 5749382                bool hasMotionBound =
 5749383                    ContinuousCollisionMath.TryResolveRotationalIntervalMotionBound(
 5749384                        displacement,
 5749385                        angularDistance,
 5749386                        pivotRadius,
 5749387                        intervalSpan,
 5749388                        out Fixed64 motionBound);
 5749389                if (hasMotionBound && samplesTargetMotion)
 390                {
 4785391                    Fixed64 lowerFraction = ResolveRotationalFrameFraction(
 4785392                        elapsedTime,
 4785393                        remainingTime,
 4785394                        interval.LowerTime);
 4785395                    Fixed64 upperFraction = ResolveRotationalFrameFraction(
 4785396                        elapsedTime,
 4785397                        remainingTime,
 4785398                        interval.UpperTime);
 4785399                    Fixed64 targetRadius = targetBody!.ResolveContinuousCollisionProxyRadius();
 4785400                    bool targetBoundResolved = targetBody.TryResolveContinuousCollisionMotionBound(
 4785401                        lowerFraction,
 4785402                        upperFraction,
 4785403                        targetRadius,
 4785404                        out Fixed64 targetMotionBound);
 4785405                    bool combinedBoundResolved = Fixed64.TryAdd(
 4785406                        motionBound,
 4785407                        targetMotionBound,
 4785408                        out motionBound);
 4785409                    hasMotionBound = targetBoundResolved & combinedBoundResolved;
 410                }
 411
 5749412                bool sampleHasContact = surface != null ? surface.IntersectsMixedContinuous(Collider, resolved: resolved
 5749413                    : CollisionDetectionMixed.IntersectsForContinuousCollision(target, Collider);
 5749414                if (sampleHasContact
 5749415                    && (!hasKnownContact || midpoint < knownContactTime))
 416                {
 1148417                    hasKnownContact = true;
 1148418                    knownContactTime = midpoint;
 419                }
 420
 5749421                Fixed64 shapeGap = default;
 5749422                bool shapeGapSupported = false;
 5749423                bool hasShapeGap = surface == null && CollisionDetectionMixed.TryGetRotationalSeparationGap(
 5749424                    target,
 5749425                    Collider,
 5749426                    out shapeGap,
 5749427                    out shapeGapSupported);
 5749428                Fixed64 characteristicScale = target is LSSphereCollider sphere
 5749429                    ? FixedMath.Max(pivotRadius, sphere.ScaledRadius)
 5749430                    : pivotRadius;
 5749431                bool shapeGapCertifiesSeparation = shapeGapSupported
 5749432                    && hasShapeGap
 5749433                    && ContinuousCollisionMath.TrySubtractClosestFeatureUncertainty(
 5749434                        shapeGap,
 5749435                        characteristicScale,
 5749436                        out Fixed64 conservativeShapeGap)
 5749437                    && conservativeShapeGap > motionBound;
 5749438                bool fallbackBoundsCertifySeparation = !sampleHasContact && hasMotionBound && !shapeGapSupported
 5749439                    && (surface != null ? !surface.IntersectsMixedContinuous(Collider, motionBound, resolved)
 5749440                        : ContinuousCollisionBounds.AreSeparated(target, Collider, motionBound)
 5749441                            || !CollisionDetectionMixed.IntersectsForContinuousCollision(target, Collider, motionBound))
 5749442                if (!sampleHasContact
 5749443                    && hasMotionBound
 5749444                    && (shapeGapCertifiesSeparation || fallbackBoundsCertifySeparation))
 445                {
 446                    continue;
 447                }
 448
 3854449                if (ContinuousCollisionMath.TryResolveRotationalSearchLimit(
 3854450                        interval,
 3854451                        processedNodeCount,
 3854452                        hasKnownContact,
 3854453                        knownContactTime,
 3854454                        out safeTime,
 3854455                        out contactTime,
 3854456                        out hasContact))
 457                {
 458                    // A later witness cannot authorize response at an earlier unresolved frontier.
 204459                    if (hasContact && needsResponseContact && contactTime == safeTime)
 460                    {
 461                        // Reconstruct complete matched rows at the admitted response pose.
 70462                        SampleMixedRotationalContinuousPairPose(startPosition, displacement, startRotation, angularDelta
 70463                            elapsedTime, remainingTime, targetBody, samplesTargetMotion);
 70464                        BuildMixedRotationalContinuousContact(target,
 70465                            ResolveRotationalFrameFraction(elapsedTime, remainingTime, contactTime),
 70466                            out contact, out _rotationalMixedKnownContactIsClosing);
 70467                        _rotationalMixedKnownManifold.CopyFrom(Context.CollisionScratch.MixedManifold, Context.FrameCoun
 468                    }
 204469                    return true;
 470                }
 471
 3650472                int childDepth = interval.Depth + 1;
 3650473                if (sampleHasContact)
 474                {
 1131475                    intervals[intervalCount++] =
 1131476                        new RotationalInterval(
 1131477                            interval.LowerTime,
 1131478                            midpoint,
 1131479                            childDepth);
 1131480                    continue;
 481                }
 482
 2519483                intervals[intervalCount++] =
 2519484                    new RotationalInterval(
 2519485                        midpoint,
 2519486                        interval.UpperTime,
 2519487                        childDepth);
 2519488                intervals[intervalCount++] =
 2519489                    new RotationalInterval(
 2519490                        interval.LowerTime,
 2519491                        midpoint,
 2519492                        childDepth);
 493            }
 494
 22495            return false;
 496        }
 497        finally
 498        {
 240499            if (targetBody != null)
 167500                targetBody.SetMixedContinuousCollisionSamplePose(
 167501                    originalTargetPosition,
 167502                    originalTargetRotation);
 240503        }
 240504    }
 505
 506    private void SampleMixedRotationalContinuousPairPose(
 507        Vector2d startPosition,
 508        Vector2d displacement,
 509        Fixed64 startRotation,
 510        Fixed64 angularDelta,
 511        Fixed64 sampleTime,
 512        Fixed64 elapsedTime,
 513        Fixed64 remainingTime,
 514        SolidBody? targetBody,
 515        bool samplesTargetMotion)
 516    {
 6059517        _position = startPosition + displacement * sampleTime;
 6059518        _rotation = startRotation + angularDelta * sampleTime;
 6059519        Collider.RebuildRuntimeShapeOnly();
 520
 6059521        if (!samplesTargetMotion)
 1069522            return;
 523
 4990524        Fixed64 frameFraction = ResolveRotationalFrameFraction(
 4990525            elapsedTime,
 4990526            remainingTime,
 4990527            sampleTime);
 4990528        targetBody!.SetMixedContinuousCollisionSamplePose(
 4990529            targetBody.SampleContinuousCollisionPosition(frameFraction),
 4990530            targetBody.SampleContinuousCollisionRotation(frameFraction));
 4990531    }
 532
 533    private bool TryGetExactSphereCircleTranslationalContact(
 534        SolidBody target,
 535        ContinuousCollisionMotionSegment3D targetSegment,
 536        FixedSegment targetPath,
 537        Vector3d targetDisplacement,
 538        Fixed64 targetTravelDistance,
 539        FixedSegment2d sourcePath,
 540        Vector2d sourceDisplacement,
 541        Fixed64 sourceTravelDistance,
 542        Fixed64 sourceStartRotation,
 543        Fixed64 sourceAngularDelta,
 544        out bool requiresRotationalFallback,
 545        out Fixed64 relativeContactDistance,
 546        out Fixed64 sourceContactDistance,
 547        out Fixed64 localClosingSpeed,
 548        out MixedContact contact)
 549    {
 93550        requiresRotationalFallback = true;
 93551        relativeContactDistance = default;
 93552        sourceContactDistance = default;
 93553        localClosingSpeed = default;
 93554        contact = default;
 93555        if (Collider is not LSCircleCollider2D circle
 93556            || target.Collider is not LSSphereCollider sphere
 93557            || (targetSegment.AngularDistance != Fixed64.Zero
 93558                && sphere.ScaledOffset != Vector3d.Zero)
 93559            || (sourceAngularDelta != Fixed64.Zero
 93560                && circle.ScaledLocalOffset != Vector2d.Zero))
 561        {
 47562            return false;
 563        }
 564
 46565        Vector2d originalSourcePosition = _position;
 46566        Fixed64 originalSourceRotation = _rotation;
 46567        Vector3d originalTargetPosition = target.Position3d;
 46568        FixedQuaternion originalTargetRotation = target.Rotation;
 569        try
 570        {
 46571            _position = sourcePath.Start;
 46572            _rotation = sourceStartRotation;
 46573            Collider.RebuildRuntimeShapeOnly();
 46574            target.SetMixedContinuousCollisionSamplePose(
 46575                targetPath.Start,
 46576                targetSegment.StartRotation);
 46577            Vector3d cylinderStart = new(
 46578                circle.Center.X,
 46579                circle.MixedSlabCenterY,
 46580                circle.Center.Y);
 46581            Vector3d sphereStart = sphere.Center;
 46582            Vector3d cylinderDisplacement = sourceDisplacement.ToVector3d(Fixed64.Zero);
 46583            Vector3d relativeDisplacement =
 46584                ContinuousCollisionSweepRange.ValidateRelativeDisplacement(
 46585                    targetDisplacement,
 46586                    cylinderDisplacement,
 46587                    out Fixed64 totalDistance);
 46588            if (!WideFiniteAxisIntersection
 46589                    .TryGetSphericallyExpandedFiniteCylinderDirectionFirstDistanceFromHalfAxisLength(
 46590                        sphereStart,
 46591                        relativeDisplacement,
 46592                        cylinderStart,
 46593                        Vector3d.Up,
 46594                        circle.MixedHalfThickness,
 46595                        circle.ScaledRadius,
 46596                        sphere.ScaledRadius,
 46597                        totalDistance,
 46598                        out Fixed64 contactDistance))
 599            {
 1600                requiresRotationalFallback = false;
 1601                return false;
 602            }
 603
 45604            relativeContactDistance = contactDistance;
 45605            _ = Fixed64.TryMultiplyDivide(
 45606                sourceTravelDistance,
 45607                contactDistance,
 45608                totalDistance,
 45609                out sourceContactDistance);
 45610            Fixed64 candidateDistance = contactDistance;
 92611            for (int attempt = 0; attempt < 2; attempt++)
 612            {
 46613                _ = Fixed64.TryMultiplyDivide(
 46614                    sourceTravelDistance,
 46615                    candidateDistance,
 46616                    totalDistance,
 46617                    out Fixed64 sampleSourceDistance);
 46618                _ = Fixed64.TryMultiplyDivide(
 46619                    targetTravelDistance,
 46620                    candidateDistance,
 46621                    totalDistance,
 46622                    out Fixed64 targetContactDistance);
 46623                _position = sourcePath.GetPointAtDistance(
 46624                    sampleSourceDistance,
 46625                    sourceTravelDistance);
 46626                _rotation = sourceStartRotation;
 46627                Collider.RebuildRuntimeShapeOnly();
 46628                target.SetMixedContinuousCollisionSamplePose(
 46629                    targetPath.GetPointAtDistance(
 46630                        targetContactDistance,
 46631                        targetTravelDistance),
 46632                    targetSegment.StartRotation);
 46633                ContactManifold manifold = Context.CollisionScratch.MixedManifold;
 46634                if (CollisionDetectionMixed.TryBuildManifold(target.Collider, Collider, manifold))
 635                {
 636                    // Relative displacement is sphere-minus-circle even when the source is 2D.
 45637                    contact = MixedContact.FromManifoldContact(manifold.PrimaryContact);
 45638                    bool closingResolved = Vector3d.TryDot(
 45639                            relativeDisplacement,
 45640                            contact.Normal3DTo2D,
 45641                            out localClosingSpeed);
 45642                    if (closingResolved
 45643                        && localClosingSpeed > Fixed64.Epsilon)
 644                    {
 42645                        return true;
 646                    }
 647
 3648                    if (closingResolved)
 3649                        requiresRotationalFallback = false;
 650
 3651                    break;
 652                }
 653
 1654                if (candidateDistance >= totalDistance)
 655                    break;
 656
 1657                candidateDistance = FixedMath.Min(
 1658                    totalDistance,
 1659                    candidateDistance + Fixed64.MinIncrement);
 660            }
 661
 3662            relativeContactDistance = default;
 3663            sourceContactDistance = default;
 3664            localClosingSpeed = default;
 3665            contact = default;
 3666            return false;
 667        }
 668        finally
 669        {
 46670            _position = originalSourcePosition;
 46671            _rotation = originalSourceRotation;
 46672            Collider.RebuildRuntimeShapeOnly();
 46673            target.SetMixedContinuousCollisionSamplePose(
 46674                originalTargetPosition,
 46675                originalTargetRotation);
 46676        }
 46677    }
 678
 679    private void BuildMixedRotationalContinuousContact(LSCollider target,
 680        Fixed64 frameFraction, out MixedContact contact, out bool contactIsClosing)
 681    {
 90682        ContactManifold manifold = Context.CollisionScratch.MixedManifold;
 90683        contact = default;
 90684        contactIsClosing = false;
 685        // Admission already retained this geometric witness; closing rows only authorize response.
 90686        if (target is LSMeshCollider { StaticSurface: { } surface })
 687        {
 25688            manifold.BeginUpdate(Context.FrameCount);
 25689            surface.AppendMixedContinuousContacts(Collider, manifold, ResolvedMixedSurfaceFamilies(surface));
 690        }
 65691        else CollisionDetectionMixed.TryBuildDiscreteManifold(target, Collider, manifold);
 90692        SolidBody? other = target.Body;
 90693        ContactAnchor center3D = other?.GetCenterOfMassAnchor() ?? ContactAnchor.FromWorldPoint(target.Center);
 90694        var center2D = new ContactAnchor(new Vector3d(_position.X, Collider.MixedSlabCenterY, _position.Y),
 90695            FixedQuaternion.FromAxisAngle(Vector3d.Up, -_rotation),
 90696            ExactContactLever2D.ToSpatial(LocalCenterOfMassOffset));
 90697        contactIsClosing = MixedContact.TrySelectClosingContact(manifold, center3D,
 90698            other?.SampleContinuousCollisionLinearVelocity(frameFraction) ?? Vector3d.Zero,
 90699            other?.SampleContinuousCollisionAngularVelocity(frameFraction) ?? Vector3d.Zero,
 90700            center2D, IsKinematic ? SampleContinuousCollisionLinearVelocity(frameFraction) : _linearVelocity,
 90701            IsKinematic ? SampleContinuousCollisionAngularVelocity(frameFraction) : _angularVelocity, out contact, Colli
 90702    }
 703
 704    private bool IsValidMixedRotationalContinuousCollisionTarget(LSCollider target)
 705    {
 684706        bool resolvedDomain = target is LSMeshCollider { StaticSurface: { } surface }
 684707            && !ResolvedMixedSurfaceFamilies(surface).IsEmpty;
 684708        return target.IsActive
 684709            && !target.IsTrigger
 684710            && (resolvedDomain || !ContinuousCollisionCandidateOrdering.IsIgnoredTarget(
 684711                target,
 684712                _continuousCollisionHandoffIgnoredCollider3D))
 684713            && Context.MixedCollisions.RequireCollisionPair(target, Collider);
 714    }
 715
 716    private bool IsMovingMixedRotationalContinuousCollisionTarget(SolidBody target)
 717    {
 438718        if (target.IsKinematic)
 159719            target.EnsureContinuousCollisionFramePrepared(Context.LateSimulateToken);
 720
 438721        return (IsKinematic || !target.ShouldOwnContinuousCollisionMovingPair(
 438722                HasContinuousCollisionRotationalMotion))
 438723            && IsValidMixedRotationalContinuousCollisionTarget(target.Collider)
 438724            && (target.IsKinematic
 438725                ? target.HasContinuousCollisionMotion
 438726                : target.IsDynamic);
 727    }
 728
 729    private bool TryApplyMixedTranslationalManifoldResponse(SolidBody target,
 730        Vector2d sourcePosition, Vector3d targetPosition, Fixed64 elapsedTime, Fixed64 remainingTime)
 731    {
 6732        Fixed64 fraction = ResolveContinuousCollisionFrameFraction(elapsedTime);
 6733        Fixed64 sourceRotation = SampleContinuousCollisionRotation(fraction);
 6734        FixedQuaternion targetRotation = target.SampleContinuousCollisionRotation(fraction);
 6735        Vector2d originalSourcePosition = Position;
 6736        Fixed64 originalSourceRotation = Rotation;
 6737        Vector3d originalTargetPosition = target.Position3d;
 6738        FixedQuaternion originalTargetRotation = target.Rotation;
 739        try
 740        {
 6741            SetMixedContinuousCollisionSamplePose(sourcePosition, sourceRotation);
 6742            target.SetMixedContinuousCollisionSamplePose(targetPosition, targetRotation);
 6743            if (!CollisionDetectionMixed.TryBuildManifold(target.Collider, Collider, _rotationalMixedKnownManifold))
 1744                return false;
 5745        }
 746        finally
 747        {
 6748            SetMixedContinuousCollisionSamplePose(originalSourcePosition, originalSourceRotation);
 6749            target.SetMixedContinuousCollisionSamplePose(originalTargetPosition, originalTargetRotation);
 6750        }
 5751        if (!TryApplyMixedRotationalContinuousCollisionResponse(target.Collider,
 5752                MixedContact.FromManifoldContact(_rotationalMixedKnownManifold.PrimaryContact), Fixed64.Zero,
 5753                sourcePosition, Vector2d.Zero, sourceRotation, Fixed64.Zero, elapsedTime, remainingTime,
 7754                _rotationalMixedKnownManifold)) return false;
 755        // The rotational outer loop appends this segment itself; the translational owner needs it here.
 3756        if (HasSolverMobility)
 757        {
 2758            UpdateContinuousCollisionFrameTrajectory(sourcePosition, _linearVelocity, elapsedTime);
 2759            Context.Physics2D.RefreshContinuousCollisionCandidate(this);
 760        }
 3761        return true;
 1762    }
 763
 764    internal bool TryApplyMixedRotationalContinuousCollisionResponse(
 765        LSCollider targetCollider,
 766        MixedContact contact,
 767        Fixed64 localContactTime,
 768        Vector2d sourceSegmentStart,
 769        Vector2d sourceDisplacement,
 770        Fixed64 sourceSegmentStartRotation,
 771        Fixed64 sourceAngularDelta,
 772        Fixed64 elapsedTime,
 773        Fixed64 remainingTime,
 774        ContactManifold? manifold = null)
 775    {
 256776        Fixed64 frameFraction = ResolveRotationalFrameFraction(
 256777            elapsedTime,
 256778            remainingTime,
 256779            localContactTime);
 256780        Fixed64 consumedTime = remainingTime * localContactTime;
 256781        Fixed64 impactElapsedTime = elapsedTime + consumedTime;
 256782        Fixed64 remainingAfterImpact = remainingTime - consumedTime;
 256783        Vector2d sourcePosition = sourceSegmentStart
 256784            + sourceDisplacement * localContactTime;
 256785        Fixed64 sourceRotation = CanonicalizeRotation(
 256786            sourceSegmentStartRotation + sourceAngularDelta * localContactTime);
 256787        Vector2d sourceLinearVelocity = IsKinematic
 256788            ? SampleContinuousCollisionLinearVelocity(frameFraction)
 256789            : _linearVelocity;
 256790        Fixed64 sourceAngularVelocity = IsKinematic
 256791            ? SampleContinuousCollisionAngularVelocity(frameFraction)
 256792            : _angularVelocity;
 793
 256794        SolidBody? target = targetCollider.Body;
 256795        Vector3d targetPosition = target?.SampleContinuousCollisionPosition(frameFraction)
 256796            ?? targetCollider.Center;
 256797        FixedQuaternion targetRotation = target?.SampleContinuousCollisionRotation(frameFraction)
 256798            ?? FixedQuaternion.Identity;
 256799        Vector3d targetLinearVelocity = target?.SampleContinuousCollisionLinearVelocity(frameFraction)
 256800            ?? Vector3d.Zero;
 256801        Vector3d targetAngularVelocity = target?.SampleContinuousCollisionAngularVelocity(frameFraction)
 256802            ?? Vector3d.Zero;
 256803        SolidBody? targetResponseBody = target?.HasSolverMobility == true ? target : null;
 256804        SolidBody2D? sourceResponseBody = HasSolverMobility ? this : null;
 256805        ContactAnchor targetCenter = new(targetPosition, targetRotation,
 256806            target?.LocalCenterOfMassOffset ?? Vector3d.Zero);
 256807        Vector3d sourcePosition3D = new(sourcePosition.X, Collider.MixedSlabCenterY, sourcePosition.Y);
 256808        var sourceCenter = new ContactAnchor(sourcePosition3D,
 256809            FixedQuaternion.FromAxisAngle(Vector3d.Up, -sourceRotation),
 256810            ExactContactLever2D.ToSpatial(LocalCenterOfMassOffset));
 256811        if (manifold == null)
 812        {
 4813            _rotationalMixedKnownManifold.BeginUpdate(Context.FrameCount);
 4814            _rotationalMixedKnownManifold.AddContact(default, contact.ToManifoldContact());
 4815            manifold = _rotationalMixedKnownManifold;
 816        }
 256817        if (!MixedContact.TrySelectClosingContact(manifold, targetCenter, targetLinearVelocity,
 256818                targetAngularVelocity, sourceCenter, sourceLinearVelocity, sourceAngularVelocity, out _, Collider))
 2819            return false;
 254820        var impact = new ContactResponseSnapshot(targetPosition, sourcePosition3D, targetCenter, sourceCenter,
 254821            targetLinearVelocity, targetAngularVelocity, ExactContactLever2D.ToSpatial(sourceLinearVelocity),
 254822            new Vector3d(Fixed64.Zero, -sourceAngularVelocity, Fixed64.Zero), sourceAngularVelocity);
 254823        if (!CollisionResponseMixed.TrySolveManifoldVelocity(targetCollider, Collider, manifold, impact,
 254824                ref _mixedContinuousCollisionImpulses, ref _mixedContinuousCollisionExactImpulses,
 255825                out ContactResponseSnapshot solved)) return false;
 253826        Vector3d postTargetLinearVelocity = solved.LinearA;
 253827        Vector3d postTargetAngularVelocity = solved.AngularA;
 253828        Vector2d postSourceLinearVelocity = solved.LinearB.ToVector2d();
 253829        Fixed64 postSourceAngularVelocity = solved.PlanarAngularB;
 253830        Vector3d targetResolvedPosition = default;
 253831        bool sourceStateAvailable = sourceResponseBody == null
 253832            || CanAppendContinuousCollisionFrameSegment(impactElapsedTime)
 253833                && Context.Physics2D.CanAdmitContinuousCollisionCandidateRefresh(this);
 253834        bool targetStateAvailable = targetResponseBody == null
 253835            || targetResponseBody.CanApplyContinuousCollisionHandoff(targetPosition, targetRotation,
 253836                remainingAfterImpact, out targetResolvedPosition);
 257837        if (!(sourceStateAvailable & targetStateAvailable)) return false;
 249838        if (sourceResponseBody != null)
 208839            ValidateContinuousCollisionPosition(sourcePosition, postSourceLinearVelocity, Context.DeltaTime - impactElap
 249840        if (targetResponseBody != null)
 42841            targetResponseBody.ValidateContinuousCollisionHandoffPosition(targetResolvedPosition, postTargetLinearVeloci
 842
 249843        if (sourceResponseBody != null)
 208844            _ = Context.Physics2D.TryReserveContinuousCollisionCandidateRefresh(this);
 845
 249846        if (targetResponseBody != null)
 847        {
 848            // Dirty overlay capacity is the registered-body count, so a valid
 849            // registered target cannot exhaust this reservation.
 42850            _ = Context.Physics.TryReserveContinuousCollisionCandidateRefresh(
 42851                targetResponseBody);
 42852            targetResponseBody.ApplyContinuousCollisionHandoffReserved(
 42853                targetResolvedPosition,
 42854                targetRotation,
 42855                postTargetLinearVelocity,
 42856                postTargetAngularVelocity,
 42857                remainingAfterImpact,
 42858                ignoredCollider2D: Collider);
 859        }
 860
 249861        if (sourceResponseBody != null)
 862        {
 208863            WakeFromCollision();
 208864            _linearVelocity = postSourceLinearVelocity;
 208865            _angularVelocity = postSourceAngularVelocity;
 208866            RefreshLinearSpeed();
 208867            RefreshAngularSpeed();
 868        }
 869
 870        // This static-target path has no host callbacks between response and
 871        // the next search. Retain only closing, producer-certified face families;
 872        // the prepared local mapping keeps unknown provenance conservative.
 249873        _resolvedMixedSurfaceFamilies?.Clear();
 249874        if (targetCollider is LSMeshCollider { StaticSurface: { } surface })
 104875            surface.CaptureMixedContinuousFamilies(manifold, impact, Collider, _resolvedMixedSurfaceFamilies ??= new());
 876
 249877        return true;
 878    }
 879}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/2D/Bodies/SolidBody2D.ContinuousCollision.Rotational.Response.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody2D.ContinuousCollision.Rotational.Response.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
 8using FixedMathSharp;
 9using Gravitas.CollisionHandling;
 10using Gravitas.Materials;
 11
 12namespace Gravitas;
 13
 14/// <content>Applies planar rotational contact response and stops remaining motion when required.</content>
 15public partial class SolidBody2D
 16{
 17    internal bool TryApplyRotationalContinuousCollisionResponse(
 18        SolidBody2D target,
 19        Contact2D contact,
 20        Fixed64 contactTime,
 21        Vector2d startPosition,
 22        Fixed64 startRotation,
 23        Vector2d displacement,
 24        Fixed64 angularDelta,
 25        Fixed64 elapsedTime,
 26        Fixed64 remainingTime)
 27    {
 5128        Fixed64 frameFraction = ResolveRotationalFrameFraction(
 5129            elapsedTime,
 5130            remainingTime,
 5131            contactTime);
 5132        Vector2d sourcePositionAtImpact = startPosition + displacement * contactTime;
 5133        Fixed64 sourceRotationAtImpact = CanonicalizeRotation(
 5134            startRotation + angularDelta * contactTime);
 5135        Vector2d targetPositionAtImpact = target.SampleContinuousCollisionPosition(frameFraction);
 5136        Fixed64 targetRotationAtImpact = target.SampleContinuousCollisionRotation(frameFraction);
 5137        Vector2d sourceLinearVelocity = IsKinematic
 5138            ? displacement / remainingTime
 5139            : _linearVelocity;
 5140        Fixed64 sourceAngularVelocity = IsKinematic
 5141            ? angularDelta / remainingTime
 5142            : _angularVelocity;
 5143        Vector2d targetLinearVelocity = target.SampleContinuousCollisionLinearVelocity(frameFraction);
 5144        Fixed64 targetAngularVelocity = target.SampleContinuousCollisionAngularVelocity(frameFraction);
 5145        SolidBody2D? sourceResponseBody = HasSolverMobility ? this : null;
 5146        SolidBody2D? targetResponseBody = target.HasSolverMobility ? target : null;
 5147        Fixed64 restitution = PhysicsMaterial.CombineRestitution(
 5148            Collider.Material,
 5149            target.Collider.Material);
 5150        var sourceCenter = new ContactAnchor2D(
 5151            sourcePositionAtImpact,
 5152            sourceRotationAtImpact,
 5153            _localCenterOfMassOffset);
 5154        var targetCenter = new ContactAnchor2D(
 5155            targetPositionAtImpact,
 5156            targetRotationAtImpact,
 5157            target._localCenterOfMassOffset);
 5158        ContactLever2D sourceContactArm =
 5159            ContactLever2D.Create(contact.AnchorA, sourceCenter);
 5160        ContactLever2D targetContactArm =
 5161            ContactLever2D.Create(contact.AnchorB, targetCenter);
 5162        ContactNormalVelocityDeltaResult2D response = default;
 5163        bool responseResolved = !sourceContactArm.IsExact
 5164            && !targetContactArm.IsExact
 5165            && ExactContactLever2D.CanUseCompactResponse(
 5166                sourceResponseBody,
 5167                sourceLinearVelocity,
 5168                sourceAngularVelocity,
 5169                sourceContactArm.Vector,
 5170                targetResponseBody,
 5171                targetLinearVelocity,
 5172                targetAngularVelocity,
 5173                targetContactArm.Vector,
 5174                contact.Normal)
 5175            && ContactNormalImpulse2D.TryCalculateVelocityDeltas(
 5176                sourceResponseBody,
 5177                sourceLinearVelocity,
 5178                sourceAngularVelocity,
 5179                sourceContactArm.Vector,
 5180                targetResponseBody,
 5181                targetLinearVelocity,
 5182                targetAngularVelocity,
 5183                targetContactArm.Vector,
 5184                contact.Normal,
 5185                restitution,
 5186                Context.Settings.RestitutionVelocityThreshold,
 5187                out response);
 5188        if (!responseResolved)
 89        {
 3490            responseResolved =
 3491                ContactNormalImpulse2D.TryCalculateVelocityDeltasExact(
 3492                    sourceResponseBody,
 3493                    sourceLinearVelocity,
 3494                    sourceAngularVelocity,
 3495                    contact.AnchorA.GetXZLeverFrom(sourceCenter),
 3496                    targetResponseBody,
 3497                    targetLinearVelocity,
 3498                    targetAngularVelocity,
 3499                    contact.AnchorB.GetXZLeverFrom(targetCenter),
 34100                    contact.Normal,
 34101                    restitution,
 34102                    Context.Settings.RestitutionVelocityThreshold,
 34103                    out response);
 104        }
 51105        if (!responseResolved
 51106            || !response.IsClosing
 51107            || (response.HasRepresentableNormalVelocity
 51108                && response.NormalVelocity >= -Fixed64.Epsilon))
 109        {
 12110            return false;
 111        }
 112
 39113        Vector2d targetPostLinearVelocity = targetLinearVelocity;
 39114        Fixed64 targetPostAngularVelocity = targetAngularVelocity;
 39115        Vector2d targetResolvedPosition = default;
 39116        Fixed64 impactElapsedTime = elapsedTime + remainingTime * contactTime;
 39117        Fixed64 remainingAfterImpact = remainingTime * (Fixed64.One - contactTime);
 39118        if (sourceResponseBody != null)
 119        {
 27120            bool sourceResponseAdmissible = CanApplyCollisionVelocityDeltas(
 27121                    response.LinearVelocityDeltaA,
 27122                    response.AngularVelocityDeltaA)
 27123                & CanAppendContinuousCollisionFrameSegment(impactElapsedTime)
 27124                & (targetResponseBody == null
 27125                    ? Context.Physics2D.CanAdmitContinuousCollisionCandidateRefresh(this)
 27126                    : Context.Physics2D.CanAdmitContinuousCollisionCandidateRefresh(
 27127                        this,
 27128                        target));
 27129            if (!sourceResponseAdmissible)
 1130                return false;
 131        }
 132
 38133        if (targetResponseBody != null)
 134        {
 19135            bool targetLinearVelocityResolved = Vector2d.TryAdd(
 19136                targetLinearVelocity,
 19137                response.LinearVelocityDeltaB,
 19138                out targetPostLinearVelocity);
 19139            bool targetAngularVelocityResolved = Fixed64.TryAdd(
 19140                targetAngularVelocity,
 19141                response.AngularVelocityDeltaB,
 19142                out targetPostAngularVelocity);
 19143            bool targetHandoffAdmissible = target.CanApplyContinuousCollisionHandoffState(
 19144                targetPositionAtImpact,
 19145                targetRotationAtImpact,
 19146                remainingAfterImpact,
 19147                out targetResolvedPosition);
 19148            if (!(targetLinearVelocityResolved
 19149                    & targetAngularVelocityResolved
 19150                    & targetHandoffAdmissible))
 151            {
 1152                return false;
 153            }
 154        }
 155
 37156        if (sourceResponseBody != null)
 157        {
 158            // Velocity admission above proved this addition before endpoint preflight.
 26159            Vector2d.TryAdd(_linearVelocity, ProjectLinearMotion(response.LinearVelocityDeltaA), out Vector2d postSource
 26160            ValidateContinuousCollisionPosition(sourcePositionAtImpact, postSourceVelocity, Context.DeltaTime - impactEl
 161        }
 36162        if (targetResponseBody != null)
 17163            target.ValidateContinuousCollisionHandoffPosition(targetResolvedPosition, targetPostLinearVelocity, targetPo
 164
 36165        if (sourceResponseBody != null)
 166        {
 167            // The admission check above is immediately followed by this
 168            // reservation in the single-threaded arbiter, so failure is not a
 169            // runtime state. Reserve the already-proven body set once.
 25170            _ = targetResponseBody == null
 25171                ? Context.Physics2D.TryReserveContinuousCollisionCandidateRefresh(this)
 25172                : Context.Physics2D.TryReserveContinuousCollisionCandidateRefresh(this, target);
 173        }
 174
 36175        if (targetResponseBody != null)
 176        {
 17177            target.ApplyContinuousCollisionHandoffStateReserved(
 17178                targetResolvedPosition,
 17179                targetRotationAtImpact,
 17180                targetPostLinearVelocity,
 17181                targetPostAngularVelocity,
 17182                remainingAfterImpact,
 17183                ignoredCollider2D: Collider);
 184        }
 185
 36186        if (sourceResponseBody != null)
 187        {
 25188            ApplyCollisionLinearVelocityDelta(response.LinearVelocityDeltaA);
 25189            ApplyCollisionAngularVelocityDelta(response.AngularVelocityDeltaA);
 190        }
 191
 36192        return true;
 193    }
 194
 195    [System.Runtime.CompilerServices.MethodImpl(
 196        System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
 197    private Fixed64 ResolveRotationalFrameFraction(
 198        Fixed64 elapsedTime,
 199        Fixed64 remainingTime,
 200        Fixed64 localTime) =>
 18732201        FixedMath.Clamp01((elapsedTime + remainingTime * localTime) / Context.DeltaTime);
 202
 203    private void StopRotationalContinuousCollision(Vector2d contactNormal)
 204    {
 5205        _angularVelocity = Fixed64.Zero;
 5206        _angularAccelerationStore = Fixed64.Zero;
 5207        _deltaAngularAcceleration = Fixed64.Zero;
 5208        RefreshAngularSpeed();
 5209        RemoveClosingContinuousCollisionVelocity(contactNormal);
 5210    }
 211}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/2D/Bodies/SolidBody2D.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody2D.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
 8using System.Runtime.CompilerServices;
 9using Chronicler;
 10using FixedMathSharp;
 11using Gravitas.Colliders;
 12using Gravitas.CollisionHandling;
 13using Gravitas.Queries;
 14using SwiftCollections;
 15
 16namespace Gravitas;
 17
 18/// <summary>
 19/// Represents deterministic pure 2D body state owned by one world context.
 20/// </summary>
 21/// <content>Owns core body state, motion configuration, initialization and sleep/wake lifecycle.</content>
 22public sealed partial class SolidBody2D : IRecordable
 23{
 24    private Vector2d _position;
 25    private Fixed64 _rotation;
 26    private Fixed64 _hostYawRotation;
 644827    private FixedQuaternion _hostYaw = FixedQuaternion.Identity;
 28    private Vector2d _linearVelocity;
 29    private Vector2d _linearAccelerationStore;
 30    private Vector2d _deltaAcceleration;
 31    private Fixed64 _linearSpeed;
 32    private Fixed64 _mass;
 33    private Vector2d _localCenterOfMassOffset;
 34    private bool _centerOfMassOffsetExplicit;
 35    private Fixed64 _momentOfInertia;
 36    private Fixed64 _inverseMomentOfInertia;
 37    private Fixed64 _angularVelocity;
 38    private Fixed64 _angularAccelerationStore;
 39    private Fixed64 _deltaAngularAcceleration;
 40    private Fixed64 _angularSpeed;
 41    private bool _isSleeping;
 42    private PhysicsMobilityType _motionType;
 43    private int _sleepFrameCount;
 644844    private bool _sleepEnabled = true;
 644845    private int _sleepFrameThreshold = 16;
 644846    private Fixed64 _sleepLinearSpeedThreshold = (Fixed64)0.001f;
 644847    private Fixed64 _sleepAngularSpeedThreshold = (Fixed64)0.001f;
 48    private ContinuousCollisionMode _continuousCollisionMode = ContinuousCollisionMode.Inherit;
 644849    private long _continuousCollisionFrameToken = -1;
 644850    private readonly SwiftList<ContinuousCollisionMotionSegment2D> _continuousCollisionTrajectory =
 644851        new(PhysicsSettings.DefaultContinuousCollisionMaxToiIterations + 1);
 52    private bool _continuousCollisionHandoffPending;
 644853    private long _continuousCollisionHandoffToken = -1;
 54    private Fixed64 _continuousCollisionHandoffRemainingTime;
 644855    private readonly SwiftList<Physics2DHit> _continuousCollisionHits = new();
 644856    private readonly SwiftList<int> _rotationalContinuousCollisionCandidateIds = new();
 644857    private readonly SwiftList<PhysicsMixedHit> _continuousMixedCollisionHits = new();
 58
 59    /// <summary>Creates a pure 2D body bound to a host agent and collider.</summary>
 644860    public SolidBody2D(IMatterAgent agent, LSCollider2D collider)
 61    {
 644862        ChronicleGuard.ThrowIfNull(agent, nameof(agent));
 644863        ChronicleGuard.ThrowIfNull(collider, nameof(collider));
 644864        Agent = agent;
 644865        Context = agent.Context;
 644866        Collider = collider;
 644867    }
 68
 69    /// <summary>Gets the host agent that owns this body.</summary>
 70    public IMatterAgent Agent { get; }
 71
 72    /// <summary>Gets the world context supplied by the host agent.</summary>
 73    public GravitasWorldContext Context { get; }
 74
 75    /// <summary>Gets the runtime 2D collider attached to this body.</summary>
 76    public LSCollider2D Collider { get; }
 77
 78    /// <summary>
 79    /// Gets the ephemeral simulated-body slot, or <c>-1</c> for a static or
 80    /// unregistered body.
 81    /// </summary>
 82    public int DynamicId { get; internal set; } = -1;
 83
 84    /// <summary>Gets whether the body is initialized and registered.</summary>
 85    public bool Active { get; private set; }
 86
 87    private BodyFreezeAxes2D _freezeAxes;
 88
 89    /// <summary>
 90    /// Gets or sets the planar translational and yaw rotational degrees of
 91    /// freedom frozen for pure 2D solver response, integration, CCD, and
 92    /// rotational CCD.
 93    /// </summary>
 94    public BodyFreezeAxes2D FreezeAxes
 95    {
 96        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 101797        get => _freezeAxes;
 98        set
 99        {
 486100            ChronicleGuard.ThrowIfArgument(
 486101                (value & ~BodyFreezeAxes2D.All) != BodyFreezeAxes2D.None,
 486102                nameof(value),
 486103                "Unsupported 2D freeze axis bits.");
 104
 486105            if (_freezeAxes == value)
 40106                return;
 107
 446108            _freezeAxes = value;
 446109            ApplyFreezeConstraintsToMotion();
 446110            RefreshPartitionMobility();
 111            // Frozen axes do not change the dynamic role or coverage, but can
 112            // change whether an existing membership participates in collisions.
 446113            if (Active)
 221114                RefreshPartitionAwakeState();
 446115        }
 116    }
 117
 118    /// <summary>
 119    /// Gets whether both planar translation axes are frozen.
 120    /// </summary>
 121    public bool IsPositionFullyFrozen
 122    {
 123        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 908429124        get => (_freezeAxes & BodyFreezeAxes2D.Position) == BodyFreezeAxes2D.Position;
 125    }
 126
 127    /// <summary>
 128    /// Gets this body's explicit solver-controlled, host-controlled, or static
 129    /// runtime role.
 130    /// </summary>
 353131    public PhysicsMobilityType MotionType => _motionType;
 132
 133    /// <summary>
 134    /// Gets whether the solver controls this body.
 135    /// </summary>
 1239918136    public bool IsDynamic => _motionType == PhysicsMobilityType.Dynamic;
 137
 138    /// <summary>
 139    /// Gets whether this body is excluded from per-frame motion.
 140    /// </summary>
 54785141    public bool IsStatic => _motionType == PhysicsMobilityType.Static;
 142
 143    /// <summary>
 144    /// Gets whether the host controls this body's pose.
 145    /// </summary>
 480510146    public bool IsKinematic => _motionType == PhysicsMobilityType.Kinematic;
 147
 148    /// <summary>
 149    /// Changes this registered body's runtime role between fixed-step
 150    /// transactions.
 151    /// </summary>
 152    /// <remarks>
 153    /// The transition preserves body, collider, pair, joint, and host identity,
 154    /// but clears incompatible motion, sleep, CCD, and solver-cache state before
 155    /// repartitioning. Freeze axes remain independent of the selected role.
 156    /// </remarks>
 157    /// <exception cref="System.ArgumentOutOfRangeException">
 158    /// <paramref name="motionType"/> is undefined.
 159    /// </exception>
 160    /// <exception cref="System.InvalidOperationException">
 161    /// The body is not currently registered, its context registration has been
 162    /// reset, or a simulation transaction or callback is active.
 163    /// </exception>
 164    public void SetMotionType(PhysicsMobilityType motionType)
 165    {
 66166        if (!PrepareMotionTypeTransition(motionType))
 3167            return;
 168
 55169        CommitMotionTypeTransition(motionType);
 55170    }
 171
 172    internal bool PrepareMotionTypeTransition(PhysicsMobilityType motionType)
 173    {
 137174        motionType.ThrowIfInvalid(nameof(motionType));
 136175        ThrowIfRuntimeRegistrationMissing();
 132176        if (_motionType == motionType)
 57177            return false;
 178
 75179        Context.ThrowIfFixedStepMutationNotAllowed();
 73180        Collider.ValidateCurrentRuntimeTransform();
 71181        if (motionType != PhysicsMobilityType.Dynamic)
 51182            PublishAuthoritativePose();
 183
 71184        return true;
 185    }
 186
 187    internal void CommitMotionTypeTransition(PhysicsMobilityType motionType)
 188    {
 70189        ReconcileMotionTypeRegistration(motionType);
 190
 70191        ClearMotionForMotionTypeTransition();
 70192        _isSleeping = false;
 70193        _sleepFrameCount = 0;
 70194        InvalidateContinuousCollisionFrame();
 70195        ApplyFreezeConstraintsToMotion();
 70196        RefreshMassPropertiesFromColliderShape();
 70197        RefreshPartitionMobility();
 70198    }
 199
 200    internal void ApplyLoadedMotionType(PhysicsMobilityType motionType)
 201    {
 83202        motionType.ThrowIfInvalid(nameof(motionType));
 83203        if (_motionType == motionType)
 65204            return;
 205
 18206        if (!Active)
 207        {
 4208            _motionType = motionType;
 4209            return;
 210        }
 211
 14212        ReconcileMotionTypeRegistration(motionType);
 14213        InvalidateContinuousCollisionFrame();
 14214    }
 215
 216    internal void PreflightLoadedMotionType(PhysicsMobilityType motionType)
 217    {
 88218        motionType.ThrowIfInvalid(nameof(motionType));
 87219        if (!Active)
 6220            return;
 221
 81222        ThrowIfRuntimeRegistrationMissing();
 79223        Context.ThrowIfFixedStepMutationNotAllowed();
 79224        Collider.ValidateCurrentRuntimeTransform();
 79225    }
 226
 227    private void ThrowIfRuntimeRegistrationMissing()
 228    {
 1306229        ChronicleGuard.ThrowIfTrue(
 1306230            !Active,
 1306231            nameof(SolidBody2D),
 1306232            "2D body runtime state cannot change before initialization or after deactivation.");
 1303233        ChronicleGuard.ThrowIfTrue(
 1303234            !Context.Physics2D.TryGetColliderById(Collider.Id, out LSCollider2D? registeredCollider)
 1303235                || !ReferenceEquals(registeredCollider, Collider),
 1303236            nameof(SolidBody2D),
 1303237            "2D body runtime state cannot change after its registration has been reset or replaced.");
 1295238    }
 239
 240    private void ReconcileMotionTypeRegistration(PhysicsMobilityType motionType)
 241    {
 84242        PhysicsMobilityType previousMotionType = _motionType;
 84243        InvalidateMassDependentSolverState();
 244
 84245        _motionType = motionType;
 84246        Context.Physics2D.RefreshBodyMotionTypeRegistration(this, previousMotionType);
 84247        Context.Physics2D.RefreshColliderServiceRefreshRegistration(Collider);
 84248    }
 249
 250    private void ClearMotionForMotionTypeTransition()
 251    {
 70252        _linearVelocity = Vector2d.Zero;
 70253        _linearAccelerationStore = Vector2d.Zero;
 70254        _deltaAcceleration = Vector2d.Zero;
 70255        _linearSpeed = Fixed64.Zero;
 70256        _angularVelocity = Fixed64.Zero;
 70257        _angularAccelerationStore = Fixed64.Zero;
 70258        _deltaAngularAcceleration = Fixed64.Zero;
 70259        _angularSpeed = Fixed64.Zero;
 70260    }
 261
 262    /// <summary>
 263    /// Selects the deterministic tunneling guard used when this pure 2D body commits frame movement.
 264    /// </summary>
 265    /// <exception cref="System.ArgumentOutOfRangeException">The value is not a declared continuous-collision mode.</exc
 266    public ContinuousCollisionMode ContinuousCollisionMode
 267    {
 268        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 161269        get => _continuousCollisionMode;
 270        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 271        set
 272        {
 716273            value.ThrowIfInvalid(nameof(value));
 712274            _continuousCollisionMode = value;
 712275        }
 276    }
 277
 278    /// <summary>
 279    /// Gets the number of continuous-collision impacts consumed by the most recent 2D late simulation step.
 280    /// </summary>
 281    public int LastContinuousCollisionToiIterationCount { get; private set; }
 282
 283    /// <summary>
 284    /// Gets whether a CCD work/state limit or unresolved refinement required a conservative clamp in the most recent 2D
 285    /// </summary>
 286    public bool LastContinuousCollisionToiIterationLimitReached { get; private set; }
 287
 288    /// <summary>
 289    /// Gets whether pure 2D yaw rotation is frozen.
 290    /// </summary>
 291    public bool IsRotationFullyFrozen
 292    {
 293        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 497492294        get => (_freezeAxes & BodyFreezeAxes2D.Rotation) == BodyFreezeAxes2D.Rotation;
 295    }
 296
 297    /// <summary>Gets or sets mass in kilograms. Non-positive mass disables solver motion.</summary>
 298    /// <remarks>
 299    /// A changed runtime value must be assigned between fixed steps. It refreshes inertia,
 300    /// invalidates cached impulses and CCD state, and wakes the body without changing its
 301    /// role, pose, velocities, or already accepted accelerations. Equal values are a no-op.
 302    /// </remarks>
 303    public Fixed64 Mass
 304    {
 305        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 1107306        get => _mass;
 307        set
 308        {
 7446309            if (_mass == value)
 231310                return;
 311
 7215312            if (!Active)
 313            {
 6291314                _mass = value;
 6291315                return;
 316            }
 317
 924318            ThrowIfRuntimeRegistrationMissing();
 922319            Context.ThrowIfFixedStepMutationNotAllowed();
 919320            Collider.ValidateCurrentRuntimeTransform();
 321            // Calculate before publishing mass: custom collider math may throw.
 919322            RefreshMomentOfInertia(value);
 918323            _mass = value;
 918324            InvalidateMassDependentSolverState();
 918325            InvalidateContinuousCollisionFrame();
 918326            _sleepFrameCount = 0;
 918327            _isSleeping = false;
 328            // Wake alone cannot synchronize an already-awake zero-mass body.
 918329            RefreshPartitionAwakeState();
 918330        }
 331    }
 332
 333    private void InvalidateMassDependentSolverState()
 334    {
 1075335        Context.Physics2D.ClearWarmStartCachesForCollider(Collider);
 1075336        Context.Constraints2D.ClearSolverCachesForBody(this);
 1075337        Context.Physics2D.InvalidateContinuousCollisionStateForBodyMutation(this, DynamicId);
 1075338    }
 339
 340    private void RefreshPartitionAwakeState()
 341    {
 1900342        Context.Collisions2D.RefreshPartitionAwakeState(Collider);
 1900343        Context.MixedCollisions.Refresh2DPartitionAwakeState(Collider);
 1900344    }
 345
 346    /// <summary>Gets the reciprocal mass, or zero when the mass is non-positive.</summary>
 805478347    public Fixed64 InverseMass => _mass > Fixed64.Zero ? Fixed64.One / _mass : Fixed64.Zero;
 348
 349    /// <summary>
 350    /// Gets whether solver-side response may translate this pure 2D body.
 351    /// </summary>
 745095352    public bool CanTranslate => Active && DynamicId >= 0 && IsDynamic && !IsPositionFullyFrozen && InverseMass > Fixed64
 353
 354    /// <summary>
 355    /// Gets whether solver-side response may rotate this pure 2D body around its yaw axis.
 356    /// </summary>
 532749357    public bool CanRotate => Active
 532749358        && DynamicId >= 0
 532749359        && IsDynamic
 532749360        && !IsRotationFullyFrozen
 532749361        && _inverseMomentOfInertia > Fixed64.Zero;
 362
 368945363    internal bool HasSolverMobility => CanTranslate || CanRotate;
 364
 365    /// <summary>
 366    /// Gets the inverse mass that should be used by pure 2D and mixed response.
 367    /// </summary>
 33547368    public Fixed64 EffectiveInverseMass => CanTranslate ? InverseMass : Fixed64.Zero;
 369
 370    /// <summary>
 371    /// Gets the inverse scalar moment that should be used by pure 2D angular response.
 372    /// </summary>
 78605373    public Fixed64 EffectiveInverseMomentOfInertia => CanRotate ? _inverseMomentOfInertia : Fixed64.Zero;
 374
 375    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 376    internal Vector2d ProjectLinearMotion(Vector2d value)
 377    {
 510629378        if (value == Vector2d.Zero || IsPositionFullyFrozen)
 313639379            return Vector2d.Zero;
 380
 196990381        Fixed64 x = (_freezeAxes & BodyFreezeAxes2D.PositionX) == BodyFreezeAxes2D.PositionX ? Fixed64.Zero : value.X;
 196990382        Fixed64 y = (_freezeAxes & BodyFreezeAxes2D.PositionY) == BodyFreezeAxes2D.PositionY ? Fixed64.Zero : value.Y;
 196990383        return new Vector2d(x, y);
 384    }
 385
 386    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 387    private Vector2d ProjectLinearEndpoint(Vector2d start, Vector2d end)
 388    {
 14214389        Fixed64 x = (_freezeAxes & BodyFreezeAxes2D.PositionX) == BodyFreezeAxes2D.PositionX ? start.X : end.X;
 14214390        Fixed64 y = (_freezeAxes & BodyFreezeAxes2D.PositionY) == BodyFreezeAxes2D.PositionY ? start.Y : end.Y;
 14214391        return new Vector2d(x, y);
 392    }
 393
 394    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 395    internal Fixed64 GetConstrainedInverseMass(Vector2d axis)
 396    {
 85081397        if (!CanTranslate || axis == Vector2d.Zero)
 9468398            return Fixed64.Zero;
 399
 75613400        Fixed64 axisMagnitudeSquared = axis.MagnitudeSquared;
 75613401        if (axisMagnitudeSquared <= Fixed64.Epsilon)
 2402            return Fixed64.Zero;
 403
 75611404        Vector2d allowedAxis = ProjectLinearMotion(axis);
 75611405        Fixed64 allowedScale = Vector2d.Dot(allowedAxis, axis) / axisMagnitudeSquared;
 75611406        return allowedScale > Fixed64.Zero ? InverseMass * allowedScale : Fixed64.Zero;
 407    }
 408
 409    /// <summary>Gets the scalar moment of inertia around the yaw axis.</summary>
 69410    public Fixed64 MomentOfInertia => _momentOfInertia;
 411
 412    /// <summary>Gets the reciprocal yaw moment of inertia.</summary>
 25413    public Fixed64 InverseMomentOfInertia => _inverseMomentOfInertia;
 414
 415    /// <summary>Gets the signed yaw velocity.</summary>
 254889416    public Fixed64 AngularVelocity => _angularVelocity;
 417
 418    /// <summary>Gets the accumulated yaw acceleration.</summary>
 8419    public Fixed64 AngularAcceleration => _angularAccelerationStore;
 420
 421    /// <summary>Gets the absolute yaw speed.</summary>
 19422    public Fixed64 AngularSpeed => _angularSpeed;
 423
 424    /// <summary>Gets the authoritative planar X/Z position.</summary>
 425    public Vector2d Position
 426    {
 427        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 111406428        get => _position;
 429    }
 430
 431    /// <summary>Gets the authoritative yaw rotation.</summary>
 432    public Fixed64 Rotation
 433    {
 434        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 83177435        get => _rotation;
 436    }
 437
 438    /// <summary>Gets the authoritative planar linear velocity.</summary>
 187319439    public Vector2d LinearVelocity => _linearVelocity;
 440
 441    /// <summary>Gets the magnitude of <see cref="LinearVelocity"/>.</summary>
 947442    public Fixed64 LinearSpeed => _linearSpeed;
 443
 6448444    private Vector2d _gravity = Vector2d.Zero;
 445
 446    /// <summary>Gets or sets the planar gravity acceleration applied to this body.</summary>
 447    public Vector2d Gravity
 448    {
 30567449        get => _gravity;
 450        set
 451        {
 145452            if (_gravity == value)
 31453                return;
 114454            Vector2d previousUp = ResolveGroundUpDirection();
 114455            _gravity = value;
 114456            if (previousUp != ResolveGroundUpDirection())
 5457                InvalidateAutomaticGroundSupport();
 114458        }
 459    }
 460
 6448461    private Fixed64 _gravityScale = Fixed64.One;
 462
 463    /// <summary>
 464    /// Multiplies this body's planar gravity vector. Zero disables body-authored gravity acceleration.
 465    /// </summary>
 466    public Fixed64 GravityScale
 467    {
 468        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 159469        get => _gravityScale;
 470        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 471        set
 472        {
 197473            ChronicleGuard.ThrowIfArgument(
 197474                value < Fixed64.Zero,
 197475                nameof(value),
 197476                "2D gravity scale cannot be negative.");
 196477            _gravityScale = value;
 196478        }
 479    }
 480
 481    /// <summary>
 482    /// Enables deterministic sleep evaluation for this body. Disabling sleep wakes a sleeping body immediately.
 483    /// </summary>
 484    public bool SleepEnabled
 485    {
 16688486        get => _sleepEnabled;
 487        set
 488        {
 733489            if (_sleepEnabled == value)
 124490                return;
 491
 609492            _sleepEnabled = value;
 609493            if (!value)
 512494                Wake();
 609495        }
 496    }
 497
 498    /// <summary>
 499    /// Number of consecutive fixed frames below sleep thresholds required before the body sleeps.
 500    /// </summary>
 501    public int SleepFrameThreshold
 502    {
 6934503        get => _sleepFrameThreshold;
 504        set
 505        {
 118506            ChronicleGuard.ThrowIfNegative(value, nameof(value));
 117507            _sleepFrameThreshold = value;
 117508        }
 509    }
 510
 511    /// <summary>
 512    /// Linear speed at or below which the body can count toward sleeping.
 513    /// </summary>
 514    public Fixed64 SleepLinearSpeedThreshold
 515    {
 5631516        get => _sleepLinearSpeedThreshold;
 517        set
 518        {
 86519            ChronicleGuard.ThrowIfArgument(
 86520                value < Fixed64.Zero,
 86521                nameof(value),
 86522                "2D sleep linear speed threshold cannot be negative.");
 85523            _sleepLinearSpeedThreshold = value;
 85524        }
 525    }
 526
 527    /// <summary>
 528    /// Angular speed at or below which the body can count toward sleeping.
 529    /// </summary>
 530    public Fixed64 SleepAngularSpeedThreshold
 531    {
 532        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 2549533        get => _sleepAngularSpeedThreshold;
 534        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 535        set
 536        {
 88537            ChronicleGuard.ThrowIfArgument(
 88538                value < Fixed64.Zero,
 88539                nameof(value),
 88540                "2D angular sleep threshold cannot be negative.");
 88541            _sleepAngularSpeedThreshold = value;
 88542        }
 543    }
 544
 545    /// <summary>Gets whether the body is sleeping.</summary>
 325538546    public bool IsSleeping => _isSleeping;
 547
 300199548    internal bool IsAwakeForCollision => HasSolverMobility && !IsSleeping;
 549
 550    /// <summary>Initializes and registers the body at an authoritative planar pose.</summary>
 551    public void Initialize(
 552        Vector2d position,
 553        Fixed64 rotation = default,
 554        PhysicsMobilityType motionType = PhysicsMobilityType.Dynamic)
 555    {
 6476556        motionType.ThrowIfInvalid(nameof(motionType));
 6476557        ChronicleGuard.ThrowIfTrue(Active, nameof(SolidBody2D), "2D body is already initialized.");
 6476558        ChronicleGuard.ThrowIfArgument(
 6476559            Collider.Id >= 0
 6476560                || (Collider.HasHostBinding && !ReferenceEquals(Collider.Body, this)),
 6476561            nameof(Collider),
 6476562            "Body collider must be unregistered and free of another host binding before initialization.");
 6474563        Fixed64 canonicalRotation = CanonicalizeRotation(rotation);
 6474564        Collider.PreflightBodyInitialization(this, position, canonicalRotation);
 565
 6459566        _position = position;
 6459567        _rotation = canonicalRotation;
 6459568        _linearVelocity = Vector2d.Zero;
 6459569        _linearAccelerationStore = Vector2d.Zero;
 6459570        _deltaAcceleration = Vector2d.Zero;
 6459571        _linearSpeed = Fixed64.Zero;
 6459572        _angularVelocity = Fixed64.Zero;
 6459573        _angularAccelerationStore = Fixed64.Zero;
 6459574        _deltaAngularAcceleration = Fixed64.Zero;
 6459575        _angularSpeed = Fixed64.Zero;
 6459576        _isSleeping = false;
 6459577        _motionType = motionType;
 6459578        _sleepFrameCount = 0;
 6459579        _collisionPositionPublicationPending = false;
 6459580        InvalidateContinuousCollisionFrame();
 6459581        ResetGroundingForInitialize(position);
 6459582        Active = true;
 6459583        Collider.Initialize(this);
 6459584        RefreshMassPropertiesFromColliderShape();
 6459585        Context.Physics2D.AssimilateBody(this, motionType);
 6459586        CheckGroundForSimulation();
 6459587    }
 588
 589    /// <summary>
 590    /// Gets a world-space point from the authoritative 2D body pose and committed collider scale.
 591    /// </summary>
 592    public Vector2d GetWorldPoint(Vector2d point)
 593    {
 4594        bool transformed = TryGetWorldPoint(point, out Vector2d result);
 4595        ChronicleGuard.ThrowIfTrue(
 4596            !transformed,
 4597            nameof(Collider),
 4598            "Cannot get the world point because the 2D collider has no committed scale or the final coordinate is not re
 3599        return result;
 600    }
 601
 602    /// <summary>
 603    /// Attempts to get a world-space point from the authoritative 2D body pose and committed collider scale.
 604    /// </summary>
 605    public bool TryGetWorldPoint(Vector2d point, out Vector2d result)
 606    {
 215607        if (!Collider.TryGetCommittedOwnerScale(out Vector2d scale))
 608        {
 2609            result = Vector2d.Zero;
 2610            return false;
 611        }
 612
 213613        return Vector2d.TryTransformScaledPoint(
 213614            Position,
 213615            point,
 213616            scale,
 213617            Rotation,
 213618            out result);
 619    }
 620
 621    /// <summary>
 622    /// Gets a body-local point from the authoritative 2D body pose and committed collider scale.
 623    /// </summary>
 624    public Vector2d GetLocalPoint(Vector2d point)
 625    {
 3626        bool transformed = TryGetLocalPoint(point, out Vector2d result);
 3627        ChronicleGuard.ThrowIfTrue(
 3628            !transformed,
 3629            nameof(Collider),
 3630            "Cannot get the local point because the 2D collider has no committed scale, its scale is singular, or the fi
 2631        return result;
 632    }
 633
 634    /// <summary>
 635    /// Attempts to get a body-local point from the authoritative 2D body pose and committed collider scale.
 636    /// </summary>
 637    public bool TryGetLocalPoint(Vector2d point, out Vector2d result)
 638    {
 214639        if (!Collider.TryGetCommittedOwnerScale(out Vector2d scale))
 640        {
 2641            result = Vector2d.Zero;
 2642            return false;
 643        }
 644
 212645        return Vector2d.TryInverseTransformScaledPoint(
 212646            Position,
 212647            point,
 212648            scale,
 212649            Rotation,
 212650            out result);
 651    }
 652
 653    /// <summary>
 654    /// Resets authoritative 2D pose and clears accumulated linear/angular motion for deterministic fixture reuse.
 655    /// </summary>
 656    /// <param name="position">The new X/Z-plane position.</param>
 657    /// <param name="rotation">The new yaw rotation in radians.</param>
 658    public void ResetPosition(Vector2d position = default, Fixed64 rotation = default)
 659    {
 694660        PreflightStaticPoseChange();
 693661        Fixed64 canonicalRotation = CanonicalizeRotation(rotation);
 693662        ChronicleGuard.ThrowIfTrue(
 693663            !Collider.TryPrepareBodyPose(position, canonicalRotation),
 693664            nameof(position),
 693665            "The requested 2D body pose produces collider geometry outside the representable coordinate domain.");
 692666        _linearVelocity = Vector2d.Zero;
 692667        _linearAccelerationStore = Vector2d.Zero;
 692668        _deltaAcceleration = Vector2d.Zero;
 692669        _linearSpeed = Fixed64.Zero;
 692670        _angularVelocity = Fixed64.Zero;
 692671        _angularAccelerationStore = Fixed64.Zero;
 692672        _deltaAngularAcceleration = Fixed64.Zero;
 692673        _angularSpeed = Fixed64.Zero;
 692674        bool wasSleeping = _isSleeping;
 692675        _isSleeping = false;
 692676        _sleepFrameCount = 0;
 692677        _position = position;
 692678        _rotation = canonicalRotation;
 692679        InvalidateContinuousCollisionFrame();
 692680        ResetGroundingForInitialize(position);
 681
 692682        if (!Active)
 1683            return;
 684
 691685        Collider.PublishPreparedExplicitBodyPose();
 691686        RefreshStaticColliderAfterExplicitPoseChange();
 691687        if (wasSleeping)
 1688            RefreshPartitionAwakeState();
 691689    }
 690
 691    /// <summary>Puts an eligible body to sleep and clears its motion.</summary>
 692    public void Sleep()
 693    {
 412694        if (!CanSleep)
 2695            return;
 696
 410697        _isSleeping = true;
 410698        _sleepFrameCount = SleepFrameThreshold;
 410699        _linearVelocity = Vector2d.Zero;
 410700        _linearAccelerationStore = Vector2d.Zero;
 410701        _deltaAcceleration = Vector2d.Zero;
 410702        _linearSpeed = Fixed64.Zero;
 410703        _angularVelocity = Fixed64.Zero;
 410704        _angularAccelerationStore = Fixed64.Zero;
 410705        _deltaAngularAcceleration = Fixed64.Zero;
 410706        _angularSpeed = Fixed64.Zero;
 410707        RefreshPartitionAwakeState();
 410708    }
 709
 710    /// <summary>Wakes the body and resumes collision participation.</summary>
 711    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 712    public void Wake()
 713    {
 5811714        bool wasSleeping = _isSleeping;
 5811715        _sleepFrameCount = 0;
 5811716        _isSleeping = false;
 5811717        if (Active && wasSleeping)
 104718            RefreshPartitionAwakeState();
 5811719    }
 720
 721    internal void WakeFromCollision()
 722    {
 7385723        if (!_isSleeping)
 7212724            return;
 725
 173726        _sleepFrameCount = 0;
 173727        _isSleeping = false;
 173728        RefreshPartitionAwakeState();
 173729    }
 730
 7731    internal void LateSimulate() => LateSimulate(updateSleepState: true, updateColliderState: true);
 732
 733    internal void LateSimulate(bool updateSleepState, bool updateColliderState)
 734    {
 15470735        if (!Active)
 1736            return;
 737
 15469738        CaptureGroundedStepState();
 739        try
 740        {
 15469741            LastContinuousCollisionToiIterationCount = 0;
 15469742            LastContinuousCollisionToiIterationLimitReached = false;
 743
 15469744            if (TryConsumeContinuousCollisionHandoff(updateSleepState, updateColliderState))
 40745                return;
 746
 15429747            if (!EnsureContinuousCollisionFramePrepared(Context.LateSimulateToken, refreshGroundSupport: true))
 1748                return;
 749
 15428750            if (IsKinematic)
 479751                UpdateKinematicPositionAndRotation(updateColliderState);
 752
 15428753            if (!HasSolverMobility)
 754            {
 492755                _linearAccelerationStore = Vector2d.Zero;
 492756                _deltaAcceleration = Vector2d.Zero;
 492757                _angularAccelerationStore = Fixed64.Zero;
 492758                _deltaAngularAcceleration = Fixed64.Zero;
 492759                return;
 760            }
 761
 14936762            if (_isSleeping)
 1241763                return;
 764
 13695765            Fixed64 startRotation = _rotation;
 13695766            Fixed64 proposedRotation = startRotation + _angularVelocity * Context.DeltaTime;
 767
 13695768            Vector2d startPosition = _position;
 13695769            Vector2d proposedPosition = AddPositionDisplacement(
 13695770                startPosition, ProjectLinearMotion(_linearVelocity) * Context.DeltaTime);
 13695771            if (ShouldUseRotationalContinuousCollisionArbiter(
 13695772                    startPosition,
 13695773                    proposedPosition,
 13695774                    startRotation,
 13695775                    proposedRotation,
 13695776                    forceContinuous: false))
 777            {
 98778                TryResolveRotationalContinuousCollision(
 98779                    startPosition,
 98780                    ref proposedPosition,
 98781                    startRotation,
 98782                    ref proposedRotation);
 783            }
 784            else
 785            {
 13597786                TryResolveContinuousCollision(startPosition, ref proposedPosition);
 787            }
 13694788            proposedPosition = ProjectLinearEndpoint(startPosition, proposedPosition);
 13694789            _position = proposedPosition;
 13694790            _rotation = CanonicalizeRotation(proposedRotation);
 13694791            if (updateColliderState)
 12792                Collider.Rebuild();
 793
 13694794            if (updateSleepState)
 2795                UpdateSleepState();
 13694796        }
 797        finally
 798        {
 799            // Service-owned motion defers the same step's grounding refresh
 800            // until after response; retain its original transition snapshot.
 15469801            if (updateSleepState)
 6802                CompleteGroundedStepState();
 15469803        }
 15468804    }
 805
 806    internal void OnVisualize()
 807    {
 21808        if (IsKinematic)
 1809            return;
 810
 20811        SetHostWorldPose(Agent.Transform, _position, _rotation);
 20812    }
 813
 814    private void UpdateKinematicPositionAndRotation(bool updateColliderState)
 815    {
 479816        Vector2d startPosition = _position;
 479817        Fixed64 startRotation = _rotation;
 479818        FixedTransform transform = Agent.Transform;
 479819        Vector2d kinematicPosition = ContinuousCollisionFrameEnd;
 479820        Fixed64 kinematicRotation = ContinuousCollisionFrameTargetRotation;
 479821        if (startPosition == kinematicPosition && startRotation == kinematicRotation)
 822        {
 240823            SetHostWorldPose(transform, kinematicPosition, kinematicRotation);
 240824            return;
 825        }
 826
 239827        Vector2d requestedHostPosition = transform.WorldPositionXZ;
 239828        Fixed64 requestedHostRotation = ReadHostRotation(transform);
 239829        Wake();
 239830        Vector2d resolvedPosition = kinematicPosition;
 239831        Fixed64 resolvedRotation = kinematicRotation;
 239832        if (ShouldUseRotationalContinuousCollisionArbiter(
 239833                startPosition,
 239834                resolvedPosition,
 239835                startRotation,
 239836                resolvedRotation,
 239837                forceContinuous: false))
 838        {
 105839            TryResolveKinematicRotationalContinuousCollision(
 105840                startPosition,
 105841                ref resolvedPosition,
 105842                startRotation,
 105843                ref resolvedRotation);
 844        }
 845        else
 846        {
 134847            TryResolveKinematicContinuousCollision(startPosition, ref resolvedPosition);
 848        }
 849
 239850        resolvedRotation = CanonicalizeRotation(resolvedRotation);
 239851        if (resolvedPosition != requestedHostPosition
 239852            || resolvedRotation != requestedHostRotation)
 78853            SetHostWorldPose(transform, resolvedPosition, resolvedRotation);
 854
 239855        _position = resolvedPosition;
 239856        _rotation = resolvedRotation;
 239857        if (updateColliderState)
 3858            Collider.Rebuild();
 239859    }
 860
 861    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 862    private static Fixed64 CanonicalizeRotation(Fixed64 rotation) =>
 32464863        PlanarRotation.Canonicalize(rotation);
 864
 865    private Fixed64 ReadHostRotation(FixedTransform transform)
 866    {
 758867        FixedQuaternion localYaw = GetHostYaw(_rotation);
 868        // Match the parent-relative orientation written by FixedTransform.TrySetWorldPose.
 758869        if (transform.Parent is FixedTransform parent)
 7870            localYaw = (parent.WorldRotation.Inverse() * localYaw).Normalized;
 871
 872        // The host pose can already represent the accepted scalar even when atan2 would round it differently.
 758873        FixedQuaternion localRotation = transform.LocalRotation;
 758874        if (localRotation == localYaw || localRotation == -localYaw)
 450875            return _rotation;
 876
 308877        return CanonicalizeRotation(transform.WorldRotationXZRadians);
 878    }
 879
 880    private FixedQuaternion GetHostYaw(Fixed64 rotation)
 881    {
 1226882        if (_hostYawRotation != rotation)
 883        {
 95884            _hostYaw = FixedQuaternion.FromAxisAngle(Vector3d.Up, -rotation).Normalized;
 95885            _hostYawRotation = rotation;
 886        }
 1226887        return _hostYaw;
 888    }
 889
 890    private void SetHostWorldPose(
 891        FixedTransform transform,
 892        Vector2d position,
 893        Fixed64 rotation)
 894    {
 468895        Vector3d currentWorldPosition = transform.WorldPosition;
 468896        Vector3d worldPosition = new(position.X, currentWorldPosition.Y, position.Y);
 468897        ChronicleGuard.ThrowIfTrue(
 468898            !transform.TrySetWorldPose(worldPosition, GetHostYaw(rotation)),
 468899            nameof(FixedTransform),
 468900            "Host transform cannot represent the requested world-space 2D pose.");
 468901    }
 902
 903    private void PublishAuthoritativePose() =>
 51904        SetHostWorldPose(Agent.Transform, _position, _rotation);
 905
 906    /// <summary>
 907    /// Removes this body and its collider from the pure 2D runtime service.
 908    /// </summary>
 909    public void Deactivate()
 910    {
 219911        if (!ReferenceEquals(Collider.Body, this))
 11912            return;
 913
 208914        DiscardContinuousCollisionHandoff();
 208915        InvalidateContinuousCollisionFrame();
 208916        Context.Physics2D.DessimilateBody(this);
 208917        Active = false;
 208918        DynamicId = -1;
 208919        Collider.IsActive = false;
 208920        Collider.ClearPhysicsState();
 208921        Collider.ClearBindingState();
 208922    }
 923}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/2D/Bodies/SolidBody2D.Grounding.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody2D.Grounding.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
 8using System;
 9using System.Runtime.CompilerServices;
 10using FixedMathSharp;
 11using Gravitas.Colliders;
 12using Gravitas.CollisionHandling;
 13using Gravitas.Queries;
 14
 15namespace Gravitas;
 16
 17/// <content>Maintains planar ground probes, support contacts, and grounded-state transitions.</content>
 18public sealed partial class SolidBody2D
 19{
 120    private static readonly Vector2d DefaultGroundUpDirection = Vector2d.Forward;
 21
 22    private GroundingMode _groundingMode = GroundingMode.Automatic;
 23    private GroundProbeMode2D _groundProbeMode = GroundProbeMode2D.Auto;
 644824    private bool _useGravityDerivedGroundUpDirection = true;
 644825    private Vector2d _groundUpDirection = DefaultGroundUpDirection;
 26    private Fixed64 _groundProbeRadius;
 644827    private Fixed64 _groundedDistanceRay = Fixed64.Half;
 644828    private Fixed64 _groundMinNormalDot = Fixed64.Half;
 644829    private long _lastGroundCheckFrame = -1;
 30    private const int GroundCheckFrameThreshold = 10;
 644831    private readonly Fixed64 _groundCheckPositionThreshold = Fixed64.FromFraction(1, 100);
 32
 33    private bool _isGrounded;
 34    private bool _wasGrounded;
 35    private bool _groundedTransitionCapturedForStep;
 36    private long _groundingStateVersion;
 37    private Vector2d _groundNormal;
 38    private bool _hasGroundPoint;
 39    private Vector2d _groundPoint;
 40    private Vector2d _lastGroundedPosition;
 41    private LSCollider2D? _groundCollider;
 42    private uint _groundColliderContactDependencyVersion;
 43    private long _groundColliderLifetimeVersion;
 44    private uint _groundOwnContactDependencyVersion;
 45
 46    private bool _hasGroundContactCandidate;
 47    private Fixed64 _groundContactCandidateUpDot;
 48    private Fixed64 _groundContactCandidateDepth;
 49    private int _groundContactCandidateColliderId;
 50    private ulong _groundContactCandidateId;
 51    private Vector2d _groundContactCandidateNormal;
 52    private bool _groundContactCandidateHasPoint;
 53    private Vector2d _groundContactCandidatePoint;
 54    private ColliderLifetimeToken2D _groundContactCandidateRegistration;
 55
 56    /// <summary>
 57    /// Selects whether Gravitas owns planar support detection or preserves host-supplied state.
 58    /// </summary>
 18059    public GroundingMode GroundingMode => _groundingMode;
 60
 61    /// <summary>
 62    /// Selects the pure 2D query primitive used when probing for planar support.
 63    /// </summary>
 64    public GroundProbeMode2D GroundProbeMode
 65    {
 66        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 19067        get => _groundProbeMode;
 68        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 69        set
 70        {
 6171            if (_groundProbeMode == value)
 272                return;
 5973            _groundProbeMode = value;
 5974            _lastGroundCheckFrame = -1;
 5975        }
 76    }
 77
 78    /// <summary>
 79    /// Gets or sets whether non-zero planar gravity defines the support up direction.
 80    /// </summary>
 81    public bool UseGravityDerivedGroundUpDirection
 82    {
 83        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 16884        get => _useGravityDerivedGroundUpDirection;
 85        set
 86        {
 2087            if (_useGravityDerivedGroundUpDirection == value)
 988                return;
 1189            Vector2d previousUp = ResolveGroundUpDirection();
 1190            _useGravityDerivedGroundUpDirection = value;
 1191            if (previousUp != ResolveGroundUpDirection())
 692                InvalidateAutomaticGroundSupport();
 1193        }
 94    }
 95
 96    /// <summary>
 97    /// Gets or sets the fallback support up direction in the X/Z simulation plane.
 98    /// </summary>
 99    public Vector2d GroundUpDirection
 100    {
 101        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 170102        get => _groundUpDirection;
 103        set
 104        {
 26105            ChronicleGuard.ThrowIfArgument(
 26106                value.MagnitudeSquared <= Fixed64.Epsilon,
 26107                nameof(value),
 26108                "2D ground up direction must be non-zero.");
 26109            Vector2d normalized = value.Normalized;
 26110            if (_groundUpDirection == normalized)
 10111                return;
 16112            Vector2d previousUp = ResolveGroundUpDirection();
 16113            _groundUpDirection = normalized;
 16114            if (previousUp != ResolveGroundUpDirection())
 11115                InvalidateAutomaticGroundSupport();
 16116        }
 117    }
 118
 119    /// <summary>
 120    /// Optional explicit radius for swept-circle support probes. A zero value derives the radius from the collider.
 121    /// </summary>
 122    public Fixed64 GroundProbeRadius
 123    {
 124        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 162125        get => _groundProbeRadius;
 126        set
 127        {
 56128            ChronicleGuard.ThrowIfArgument(value < Fixed64.Zero, nameof(value), "2D ground probe radius cannot be negati
 56129            ColliderGeometryPolicy.ValidateReach(value, nameof(value));
 55130            if (_groundProbeRadius == value)
 2131                return;
 53132            _groundProbeRadius = value;
 53133            _lastGroundCheckFrame = -1;
 53134        }
 135    }
 136
 137    /// <summary>
 138    /// Probe distance used while the body is already grounded.
 139    /// </summary>
 140    public Fixed64 GroundedDistanceRay
 141    {
 2044142        get => _groundedDistanceRay;
 143        set
 144        {
 187145            if (_groundedDistanceRay == value)
 82146                return;
 105147            _groundedDistanceRay = value;
 105148            _lastGroundCheckFrame = -1;
 105149        }
 150    }
 151
 152    /// <summary>
 153    /// Probe distance used while the body is airborne.
 154    /// </summary>
 155    public Fixed64 GroundDownDistanceOnAir { get; set; } = Fixed64.Half;
 156
 157    /// <summary>
 158    /// Minimum normalized support-normal dot product with resolved up, in [0, 1].
 159    /// Defaults to one half (slopes through 60 degrees). Zero permits any
 160    /// positive up-dot; horizontal, downward-facing and zero normals never support.
 161    /// </summary>
 162    public Fixed64 GroundMinNormalDot
 163    {
 1900164        get => _groundMinNormalDot;
 165        set
 166        {
 98167            ValidateGroundMinNormalDot(value);
 96168            if (_groundMinNormalDot == value)
 8169                return;
 88170            _groundMinNormalDot = value;
 88171            InvalidateAutomaticGroundSupport();
 88172        }
 173    }
 174
 175    /// <summary>
 176    /// Gets whether this body currently has planar support.
 177    /// </summary>
 178    public bool IsGrounded
 179    {
 344180        get => _isGrounded;
 181        private set
 182        {
 10765183            if (_isGrounded == value)
 10241184                return;
 185
 524186            _isGrounded = value;
 524187            OnGrounded?.Invoke(value);
 53188        }
 189    }
 190
 191    /// <summary>
 192    /// Gets whether this body was grounded before the latest authoritative grounding refresh or manual grounding change
 193    /// </summary>
 69194    public bool WasGrounded => _wasGrounded;
 195
 196    /// <summary>
 197    /// Gets the current support normal in the X/Z simulation plane.
 198    /// </summary>
 118199    public Vector2d GroundNormal => _groundNormal;
 200
 201    /// <summary>
 202    /// Gets whether the current support point can be represented in the X/Z
 203    /// simulation plane.
 204    /// </summary>
 70205    public bool HasGroundPoint => _hasGroundPoint;
 206
 207    /// <summary>
 208    /// Gets the current support point in the X/Z simulation plane.
 209    /// </summary>
 210    /// <exception cref="InvalidOperationException">
 211    /// No grounded point is available or the conceptual contact point is
 212    /// outside the scalar range.
 213    /// </exception>
 214    public Vector2d GroundPoint =>
 66215        _hasGroundPoint
 66216            ? _groundPoint
 66217            : throw new InvalidOperationException(
 66218                "No representable ground point is available. Use TryGetGroundPoint.");
 219
 220    /// <summary>
 221    /// Attempts to get the current support point.
 222    /// </summary>
 223    public bool TryGetGroundPoint(out Vector2d point)
 224    {
 51225        point = _hasGroundPoint ? _groundPoint : default;
 51226        return _hasGroundPoint;
 227    }
 228
 229    /// <summary>
 230    /// Gets the body position captured when the current support state was accepted.
 231    /// </summary>
 4232    public Vector2d LastGroundedPosition => _lastGroundedPosition;
 233
 234    /// <summary>
 235    /// Raised when grounded state changes.
 236    /// </summary>
 237    public Action<bool>? OnGrounded;
 238
 239    /// <summary>
 240    /// Gives the host ownership of grounded state and disables automatic support refresh.
 241    /// </summary>
 242    public void UseManualGrounding(bool clearGrounding = true)
 243    {
 536244        _groundingMode = GroundingMode.Manual;
 536245        if (!clearGrounding)
 2246            return;
 247
 534248        CaptureGroundedTransitionState();
 534249        ClearGrounding();
 534250    }
 251
 252    /// <summary>
 253    /// Returns grounded-state ownership to Gravitas and optionally refreshes support immediately.
 254    /// </summary>
 255    public void UseAutomaticGrounding(bool checkGroundImmediately = true)
 256    {
 11257        _groundingMode = GroundingMode.Automatic;
 11258        if (checkGroundImmediately && Active)
 259        {
 2260            CaptureGroundedTransitionState();
 2261            CheckGround(force: true);
 262        }
 11263    }
 264
 265    /// <summary>
 266    /// Sets host-owned planar grounded state for manual support sources such as deterministic tilemaps.
 267    /// </summary>
 268    public void SetManualGrounding(Vector2d groundPoint, Vector2d groundNormal)
 269    {
 20270        ChronicleGuard.ThrowIfArgument(
 20271            groundNormal.MagnitudeSquared <= Fixed64.Epsilon,
 20272            nameof(groundNormal),
 20273            "Manual 2D grounding normal must be non-zero.");
 274
 20275        _groundingMode = GroundingMode.Manual;
 20276        CaptureGroundedTransitionState();
 20277        SetGroundingState(groundPoint, groundNormal.Normalized, null);
 20278    }
 279
 280    /// <summary>
 281    /// Clears host-owned planar grounded state while leaving automatic support disabled.
 282    /// </summary>
 283    public void ClearManualGrounding()
 284    {
 2285        _groundingMode = GroundingMode.Manual;
 2286        CaptureGroundedTransitionState();
 2287        ClearGrounding();
 2288    }
 289
 290    /// <summary>
 291    /// Forces an immediate automatic support probe when Gravitas owns grounding.
 292    /// </summary>
 293    public void CheckGround()
 294    {
 81295        if (_groundingMode == GroundingMode.Manual)
 1296            return;
 297
 80298        CaptureGroundedTransitionState();
 80299        CheckGround(force: true);
 80300    }
 301
 302    internal void BeginAutomaticGroundingRefresh()
 303    {
 15347304        if (!CanUseAutomaticGrounding)
 305        {
 11090306            ClearGroundingForAutomaticRefresh();
 11090307            return;
 308        }
 309
 4257310        CaptureGroundedStepState();
 4257311        ClearGroundContactCandidate();
 4257312    }
 313
 314    internal void TryAcceptContactGroundCandidate(
 315        LSCollider2D ownCollider,
 316        LSCollider2D otherCollider,
 317        ManifoldContact2D contact,
 318        bool ownColliderIsA)
 319    {
 10738320        if (!CanUseAutomaticGrounding || !ReferenceEquals(ownCollider, Collider))
 9949321            return;
 322
 789323        if (!IsValidGroundCollider(otherCollider))
 78324            return;
 325
 711326        Vector2d normal = ownColliderIsA ? -contact.Normal : contact.Normal;
 711327        if (normal.MagnitudeSquared <= Fixed64.Epsilon)
 2328            return;
 329
 709330        normal = normal.Normalized;
 709331        if (!IsValidGroundNormal(normal, out Fixed64 upDot))
 553332            return;
 333
 334        Vector2d point;
 156335        bool hasPoint = ownColliderIsA
 156336            ? contact.TryGetPointA(out point)
 156337            : contact.TryGetPointB(out point);
 156338        int otherId = otherCollider.Id;
 156339        if (_hasGroundContactCandidate)
 340        {
 20341            int compare = CompareGroundContactCandidate(
 20342                upDot,
 20343                contact.Depth,
 20344                otherId,
 20345                contact.ContactId);
 20346            if (compare >= 0)
 12347                return;
 348        }
 349
 144350        _hasGroundContactCandidate = true;
 144351        _groundContactCandidateUpDot = upDot;
 144352        _groundContactCandidateDepth = contact.Depth;
 144353        _groundContactCandidateColliderId = otherId;
 144354        _groundContactCandidateId = contact.ContactId;
 144355        _groundContactCandidateNormal = normal;
 144356        _groundContactCandidateHasPoint = hasPoint;
 144357        _groundContactCandidatePoint = point;
 144358        _groundContactCandidateRegistration = new ColliderLifetimeToken2D(otherCollider);
 144359    }
 360
 361    internal void CompleteAutomaticGroundingRefresh()
 362    {
 15346363        if (!CanUseAutomaticGrounding)
 364        {
 11090365            CompleteGroundedStepState();
 11090366            return;
 367        }
 368
 4256369        ColliderLifetimeToken2D candidateRegistration = _groundContactCandidateRegistration;
 4256370        if (_hasGroundContactCandidate
 4256371            && candidateRegistration.IsActive
 4256372            && IsValidGroundCollider(candidateRegistration.Collider))
 373        {
 132374            var bodyRegistration = new ColliderLifetimeToken2D(Collider);
 132375            long groundingStateVersion = SetGroundingState(
 132376                _groundContactCandidatePoint,
 132377                _groundContactCandidateNormal,
 132378                candidateRegistration.Collider,
 132379                _groundContactCandidateHasPoint);
 132380            if (!RevalidateAutomaticGroundingWrite(
 132381                    groundingStateVersion,
 132382                    bodyRegistration,
 132383                    candidateRegistration))
 384            {
 1385                return;
 386            }
 387
 131388            ClearGroundContactCandidate();
 131389            CompleteGroundedStepState();
 131390            return;
 391        }
 392
 4124393        ClearGroundContactCandidate();
 4124394        CheckGround(force: false);
 4124395        CompleteGroundedStepState();
 4124396    }
 397
 6459398    internal void CheckGroundForSimulation() => CheckGround(force: false);
 399
 400    private bool RefreshAutomaticGroundSupportBeforeMotion()
 401    {
 31085402        if (_groundingMode != GroundingMode.Automatic || !_isGrounded)
 28673403            return true;
 404
 405        // Reconstruct a loaded dependency and recheck changed support through
 406        // the same query. Equivalent witnesses preserve saved sleep state.
 2412407        if (_lastGroundCheckFrame < 0 || !HasCurrentGroundCollider())
 408        {
 54409            var registration = new ColliderLifetimeToken2D(Collider);
 54410            CaptureGroundedStepState();
 54411            CheckGround(force: true);
 54412            return registration.IsActive;
 413        }
 2358414        return true;
 415    }
 416
 417    private bool HasCurrentGroundCollider() =>
 4003418        _groundCollider != null
 4003419        && Collider.ContactDependencyVersion == _groundOwnContactDependencyVersion
 4003420        && _groundCollider.LifetimeVersion == _groundColliderLifetimeVersion
 4003421        && _groundCollider.ContactDependencyVersion == _groundColliderContactDependencyVersion
 4003422        && IsValidGroundCollider(_groundCollider);
 423
 424    internal void WakeForContactChange()
 425    {
 606426        if (!_isSleeping)
 564427            return;
 428
 42429        if (_groundingMode == GroundingMode.Automatic)
 3430            _lastGroundCheckFrame = -1;
 42431        WakeFromCollision();
 42432    }
 433
 434    internal Vector2d RemoveIntoGroundComponent(Vector2d value)
 435    {
 27633436        value = ProjectLinearMotion(value);
 27633437        if (!_isGrounded || _groundNormal.MagnitudeSquared <= Fixed64.Epsilon)
 25604438            return value;
 439
 440        // Motion preparation precedes support refresh. A changed automatic
 441        // policy must not suppress motion using the previous support normal.
 2029442        if (_groundingMode == GroundingMode.Automatic
 2029443            && (_lastGroundCheckFrame < 0 || !IsValidGroundNormal(_groundNormal, out _)))
 8444            return value;
 445
 2021446        Fixed64 intoGround = Vector2d.Dot(value, _groundNormal);
 2021447        return intoGround < Fixed64.Zero
 2021448            ? ProjectLinearMotion(value - _groundNormal * intoGround)
 2021449            : value;
 450    }
 451
 452    private bool CanUseAutomaticGrounding =>
 52580453        Active && CanTranslate && _groundingMode == GroundingMode.Automatic;
 454
 455    private void CheckGround(bool force)
 456    {
 10719457        if (_groundingMode == GroundingMode.Manual)
 52458            return;
 459
 10667460        if (!CanUseAutomaticGrounding)
 461        {
 2021462            ClearGrounding();
 2021463            return;
 464        }
 465
 8646466        bool frameGuard = !force
 8646467            && _isGrounded
 8646468            && HasCurrentGroundCollider()
 8646469            && Vector2d.Distance(_lastGroundedPosition, _position) < _groundCheckPositionThreshold
 8646470            && _lastGroundCheckFrame >= 0
 8646471            && Context.FrameCount - _lastGroundCheckFrame < GroundCheckFrameThreshold;
 8646472        if (frameGuard)
 1050473            return;
 474
 7596475        _lastGroundCheckFrame = Context.FrameCount;
 7596476        Vector2d up = ResolveGroundUpDirection();
 7596477        Vector2d down = -up;
 7596478        Fixed64 distance = _isGrounded ? GroundedDistanceRay : GroundDownDistanceOnAir;
 7596479        if (distance <= Fixed64.Zero)
 480        {
 21481            ClearGrounding();
 21482            return;
 483        }
 484
 7575485        GroundProbeMode2D mode = ResolveGroundProbeMode();
 7575486        Fixed64 radius = mode == GroundProbeMode2D.SweptCircle
 7575487            ? ResolveGroundProbeRadius()
 7575488            : Fixed64.Zero;
 7575489        Vector2d start = _position;
 7575490        Vector2d end = start + down * distance;
 7575491        bool foundGround = TryFindGroundHit(mode, radius, start, end, out Physics2DHit hit);
 7575492        Context.Diagnostics.EmitGroundProbe(this, mode, start, end, radius, foundGround, hit);
 493
 7575494        if (!foundGround)
 495        {
 7218496            ClearGrounding();
 7218497            return;
 498        }
 499
 357500        var bodyRegistration = new ColliderLifetimeToken2D(Collider);
 357501        var supportRegistration = new ColliderLifetimeToken2D(hit.Collider);
 357502        bool hasGroundPoint = hit.TryGetPoint(out Vector2d groundPoint);
 357503        long groundingStateVersion = SetGroundingState(
 357504            groundPoint,
 357505            hit.Normal.Normalized,
 357506            hit.Collider,
 357507            hasGroundPoint);
 357508        RevalidateAutomaticGroundingWrite(groundingStateVersion, bodyRegistration, supportRegistration);
 357509    }
 510
 511    private bool TryFindGroundHit(
 512        GroundProbeMode2D mode,
 513        Fixed64 radius,
 514        Vector2d start,
 515        Vector2d end,
 516        out Physics2DHit hit)
 517    {
 7575518        if (mode == GroundProbeMode2D.SweptCircle && radius > Fixed64.Epsilon)
 7504519            return Context.Query2D.SweepCircleForGrounding(start, end, radius, this, out hit);
 520
 71521        return Context.Query2D.RaycastForGrounding(start, end, this, out hit);
 522    }
 523
 524    // Query reducers apply this pure body policy to each collider's already
 525    // reduced witness, preserving compound-part ordering before ground filtering.
 526    internal bool IsValidGroundHit(Physics2DHit hit)
 527    {
 1163528        LSCollider2D hitCollider = hit.Collider;
 1163529        if (ReferenceEquals(hitCollider, Collider))
 58530            return false;
 531
 1105532        if (!IsValidGroundCollider(hitCollider))
 53533            return false;
 534
 1052535        return IsValidGroundNormal(hit.Normal.Normalized, out _);
 536    }
 537
 538    private bool IsValidGroundCollider(LSCollider2D collider)
 539    {
 5986540        if (!collider.IsActive
 5986541            || collider.IsTrigger
 5986542            || !Context.Settings.GroundCheckLayerMask.Includes(collider.Layer)
 5986543            || !ColliderCollisionFilter.AllowsPhysicalPair(Collider, collider))
 544        {
 68545            return false;
 546        }
 547
 5918548        SolidBody2D? body = collider.Body;
 5918549        return collider.IsStatic || body!.IsKinematic;
 550    }
 551
 552    private GroundProbeMode2D ResolveGroundProbeMode()
 553    {
 7575554        if (_groundProbeMode != GroundProbeMode2D.Auto)
 79555            return _groundProbeMode;
 556
 7496557        return ResolveGroundProbeRadius() > Fixed64.Epsilon
 7496558            ? GroundProbeMode2D.SweptCircle
 7496559            : GroundProbeMode2D.Ray;
 560    }
 561
 562    private Fixed64 ResolveGroundProbeRadius()
 563    {
 15000564        if (_groundProbeRadius > Fixed64.Zero)
 23565            return _groundProbeRadius;
 566
 14977567        return Collider switch
 14977568        {
 14977569            LSCircleCollider2D or
 14977570            LSCapsuleCollider2D or
 14977571            LSAABBoxCollider2D or
 14977572            LSPolygonCollider2D or
 14977573            LSCompoundCollider2D =>
 14969574                Collider.CanonicalGroundProbeRadius,
 8575            _ => Fixed64.Zero
 14977576        };
 577    }
 578
 579    private static void ValidateGroundMinNormalDot(Fixed64 value) =>
 188580        ChronicleGuard.ThrowIfArgument(value < Fixed64.Zero || value > Fixed64.One,
 188581            nameof(value), "2D ground support normal threshold must be between zero and one.");
 582
 583    private bool IsValidGroundNormal(Vector2d normalizedNormal, out Fixed64 upDot)
 584    {
 4183585        upDot = Vector2d.Dot(normalizedNormal, ResolveGroundUpDirection());
 586        // Zero means any upward support, not walls or an absent normal. Both
 587        // query and contact support apply the same inclusive slope boundary.
 4183588        return upDot > Fixed64.Zero && upDot >= _groundMinNormalDot;
 589    }
 590
 591    private void InvalidateAutomaticGroundSupport()
 592    {
 110593        if (_groundingMode != GroundingMode.Automatic)
 4594            return;
 106595        _lastGroundCheckFrame = -1;
 596        // A callback can change another body's policy after contacts have been
 597        // collected but before that body's grounding refresh completes.
 106598        ClearGroundContactCandidate();
 106599        Wake();
 106600    }
 601
 602    private Vector2d ResolveGroundUpDirection()
 603    {
 12061604        if (_useGravityDerivedGroundUpDirection && Gravity.MagnitudeSquared > Fixed64.Epsilon)
 2821605            return (-Gravity).Normalized;
 606
 9240607        return _groundUpDirection;
 608    }
 609
 610    private void CaptureGroundedTransitionState()
 611    {
 15722612        _wasGrounded = _isGrounded;
 15722613        _groundedTransitionCapturedForStep = true;
 15722614    }
 615
 616    private void CaptureGroundedStepState()
 617    {
 20224618        if (!_groundedTransitionCapturedForStep)
 15084619            CaptureGroundedTransitionState();
 20224620    }
 621
 15795622    private void CompleteGroundedStepState() => _groundedTransitionCapturedForStep = false;
 623
 624    private void ClearGrounding()
 625    {
 10256626        if (_isGrounded && _groundingMode == GroundingMode.Automatic)
 75627            WakeFromCollision();
 10256628        _groundingStateVersion++;
 10256629        _groundNormal = Vector2d.Zero;
 10256630        _hasGroundPoint = false;
 10256631        _groundPoint = Vector2d.Zero;
 10256632        _groundCollider = null;
 10256633        _groundColliderContactDependencyVersion = 0;
 10256634        _groundColliderLifetimeVersion = 0;
 10256635        _groundOwnContactDependencyVersion = 0;
 10256636        IsGrounded = false;
 10256637    }
 638
 639    private void ClearGroundingForAutomaticRefresh()
 640    {
 11090641        if (_groundingMode != GroundingMode.Automatic)
 10646642            return;
 643
 444644        CaptureGroundedStepState();
 444645        ClearGrounding();
 444646        CompleteGroundedStepState();
 444647    }
 648
 649    private long SetGroundingState(
 650        Vector2d groundPoint,
 651        Vector2d groundNormal,
 652        LSCollider2D? groundCollider,
 653        bool hasGroundPoint = true)
 654    {
 509655        bool changedSupport = _isGrounded
 509656            && (_groundNormal != groundNormal
 509657                || _hasGroundPoint != hasGroundPoint
 509658                || hasGroundPoint && _groundPoint != groundPoint);
 509659        long groundingStateVersion = ++_groundingStateVersion;
 509660        _hasGroundPoint = hasGroundPoint;
 509661        _groundPoint = groundPoint;
 509662        _groundNormal = groundNormal;
 509663        _lastGroundedPosition = _position;
 509664        _lastGroundCheckFrame = Context.FrameCount;
 509665        _groundCollider = groundCollider;
 509666        _groundColliderContactDependencyVersion = groundCollider?.ContactDependencyVersion ?? 0;
 509667        _groundColliderLifetimeVersion = groundCollider?.LifetimeVersion ?? 0;
 509668        _groundOwnContactDependencyVersion = Collider.ContactDependencyVersion;
 509669        if (changedSupport && _groundingMode == GroundingMode.Automatic)
 37670            WakeFromCollision();
 509671        IsGrounded = true;
 509672        return groundingStateVersion;
 673    }
 674
 675    private void ResetGroundingForInitialize(Vector2d position)
 676    {
 7151677        _groundingStateVersion++;
 7151678        _isGrounded = false;
 7151679        _wasGrounded = false;
 7151680        _groundedTransitionCapturedForStep = false;
 7151681        _groundNormal = Vector2d.Zero;
 7151682        _hasGroundPoint = false;
 7151683        _groundPoint = Vector2d.Zero;
 7151684        _lastGroundedPosition = position;
 7151685        _groundCollider = null;
 7151686        _groundColliderContactDependencyVersion = 0;
 7151687        _groundColliderLifetimeVersion = 0;
 7151688        _groundOwnContactDependencyVersion = 0;
 7151689        _lastGroundCheckFrame = -1;
 7151690        ClearGroundContactCandidate();
 7151691    }
 692
 693    private bool RevalidateAutomaticGroundingWrite(
 694        long groundingStateVersion,
 695        in ColliderLifetimeToken2D bodyRegistration,
 696        in ColliderLifetimeToken2D supportRegistration)
 697    {
 489698        if (!bodyRegistration.IsCurrentLifetime)
 1699            return false;
 700
 488701        if (_groundingStateVersion != groundingStateVersion)
 3702            return true;
 703
 485704        if (_groundingMode == GroundingMode.Automatic
 485705            && (!bodyRegistration.IsActive
 485706                || !CanUseAutomaticGrounding
 485707                || !supportRegistration.IsActive
 485708                || !HasCurrentGroundCollider()
 485709                || _lastGroundCheckFrame < 0
 485710                || !IsValidGroundNormal(_groundNormal, out _)))
 711        {
 16712            ClearGrounding();
 713        }
 714
 485715        return bodyRegistration.IsCurrentLifetime;
 716    }
 717
 718    private void ClearGroundContactCandidate()
 719    {
 15850720        _hasGroundContactCandidate = false;
 15850721        _groundContactCandidateUpDot = Fixed64.Zero;
 15850722        _groundContactCandidateDepth = Fixed64.Zero;
 15850723        _groundContactCandidateColliderId = int.MaxValue;
 15850724        _groundContactCandidateId = ulong.MaxValue;
 15850725        _groundContactCandidateNormal = Vector2d.Zero;
 15850726        _groundContactCandidateHasPoint = false;
 15850727        _groundContactCandidatePoint = Vector2d.Zero;
 15850728        _groundContactCandidateRegistration = default;
 15850729    }
 730
 731    private int CompareGroundContactCandidate(
 732        Fixed64 upDot,
 733        Fixed64 depth,
 734        int colliderId,
 735        ulong contactId)
 736    {
 20737        if (upDot != _groundContactCandidateUpDot)
 3738            return upDot > _groundContactCandidateUpDot ? -1 : 1;
 739
 17740        if (depth != _groundContactCandidateDepth)
 6741            return depth > _groundContactCandidateDepth ? -1 : 1;
 742
 11743        if (colliderId != _groundContactCandidateColliderId)
 2744            return colliderId < _groundContactCandidateColliderId ? -1 : 1;
 745
 9746        if (contactId != _groundContactCandidateId)
 8747            return contactId < _groundContactCandidateId ? -1 : 1;
 748
 1749        return 0;
 750    }
 751}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/2D/Bodies/SolidBody2D.Motion.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody2D.Motion.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
 8using System;
 9using System.Runtime.CompilerServices;
 10using FixedMathSharp;
 11
 12namespace Gravitas;
 13
 14/// <content>Applies planar forces, impulses, and pose changes and updates motion, mobility, and sleep state.</content>
 15public sealed partial class SolidBody2D
 16{
 17    /// <summary>
 18    /// Queues a planar force in mass-distance-per-time-squared units for
 19    /// integration during the next fixed step.
 20    /// </summary>
 21    /// <param name="force">The X/Z-plane force to apply.</param>
 22    public void AddForce(Vector2d force)
 23    {
 23624        Vector2d accelerationDelta = ProjectLinearMotion(force * EffectiveInverseMass);
 23625        if (accelerationDelta == Vector2d.Zero)
 126            return;
 27
 23528        Wake();
 23529        _deltaAcceleration += accelerationDelta;
 23530    }
 31
 32    /// <summary>
 33    /// Queues a yaw torque in mass-distance-squared-per-time-squared units for
 34    /// integration during the next fixed step.
 35    /// </summary>
 36    /// <param name="torque">The signed yaw torque applied to the body.</param>
 37    public void AddTorque(Fixed64 torque)
 38    {
 3939        Fixed64 accelerationDelta = torque * EffectiveInverseMomentOfInertia;
 3940        if (accelerationDelta == Fixed64.Zero)
 641            return;
 42
 3343        Wake();
 3344        _deltaAngularAcceleration += accelerationDelta;
 3345    }
 46
 47    /// <summary>
 48    /// Applies an X/Z-plane linear impulse immediately as a velocity change.
 49    /// The impulse is expressed in mass-distance-per-time units and does not
 50    /// advance the fixed-step simulation or apply a time-step factor.
 51    /// </summary>
 52    /// <param name="impulse">The planar linear impulse to apply.</param>
 53    public void AddLinearImpulse(Vector2d impulse)
 54    {
 234255        Vector2d velocityDelta = ProjectLinearMotion(impulse * EffectiveInverseMass);
 234256        if (velocityDelta == Vector2d.Zero)
 557            return;
 58
 233759        Wake();
 233760        _linearVelocity += velocityDelta;
 233761        RefreshLinearSpeed();
 233762    }
 63
 64    /// <summary>
 65    /// Applies an immediate yaw-velocity change derived from the supplied
 66    /// angular impulse and inverse moment of inertia. The impulse is expressed
 67    /// in mass-distance-squared-per-time units and does not apply a time-step
 68    /// factor.
 69    /// </summary>
 70    /// <param name="impulse">The signed yaw impulse applied to the body.</param>
 71    public void AddAngularImpulse(Fixed64 impulse)
 72    {
 12673        Fixed64 velocityDelta = impulse * EffectiveInverseMomentOfInertia;
 12674        if (velocityDelta == Fixed64.Zero)
 775            return;
 76
 11977        Wake();
 11978        _angularVelocity += velocityDelta;
 11979        RefreshAngularSpeed();
 11980    }
 81
 82    /// <summary>Sets the authoritative planar position and refreshes collider state.</summary>
 83    public void SetPosition(Vector2d position)
 84    {
 123285        PreflightStaticPoseChange();
 122986        ChronicleGuard.ThrowIfTrue(
 122987            !Collider.TryPrepareBodyPose(position, _rotation),
 122988            nameof(position),
 122989            "The requested 2D body position produces collider geometry outside the representable coordinate domain.");
 122790        bool poseChanged = _position != position;
 122791        if (poseChanged)
 113492            Wake();
 93
 122794        _position = position;
 122795        Collider.PublishPreparedExplicitBodyPose(poseChanged);
 122796        RefreshStaticColliderAfterExplicitPoseChange();
 122797    }
 98
 99    /// <summary>Sets the authoritative yaw rotation and refreshes collider state.</summary>
 100    public void SetRotation(Fixed64 rotation)
 101    {
 559102        PreflightStaticPoseChange();
 558103        Fixed64 canonicalRotation = CanonicalizeRotation(rotation);
 558104        ChronicleGuard.ThrowIfTrue(
 558105            !Collider.TryPrepareBodyPose(_position, canonicalRotation),
 558106            nameof(rotation),
 558107            "The requested 2D body rotation produces collider geometry outside the representable coordinate domain.");
 557108        bool poseChanged = _rotation != canonicalRotation;
 557109        if (poseChanged)
 522110            Wake();
 111
 557112        _rotation = canonicalRotation;
 557113        Collider.PublishPreparedExplicitBodyPose(poseChanged);
 557114        RefreshStaticColliderAfterExplicitPoseChange();
 557115    }
 116
 117    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 118    private void RefreshStaticColliderAfterExplicitPoseChange()
 119    {
 2475120        if (Active && IsStatic && Context.Settings.RuntimeMode.RunsMixedContacts())
 15121            Context.MixedCollisions.Refresh2DColliderPartition(Collider);
 2475122    }
 123
 124    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 125    private void PreflightStaticPoseChange()
 126    {
 2485127        if (Active && IsStatic)
 128        {
 72129            ThrowIfRuntimeRegistrationMissing();
 71130            Context.ThrowIfFixedStepMutationNotAllowed();
 68131            Collider.ValidateCurrentRuntimeTransform();
 132        }
 2480133    }
 134
 135    private void RefreshPartitionMobility()
 136    {
 516137        if (!Active)
 225138            return;
 139
 291140        if (Collider.IsPartitioned)
 285141            Collider.Simulate();
 142
 291143        Context.Collisions2D.RefreshColliderPartition(Collider);
 291144        if (Context.Settings.RuntimeMode.RunsMixedContacts())
 105145            Context.MixedCollisions.Refresh2DColliderPartition(Collider);
 291146    }
 147
 148    internal void ApplyCollisionLinearVelocityDelta(Vector2d velocityDelta)
 149    {
 109970150        velocityDelta = ProjectLinearMotion(velocityDelta);
 109970151        if (!CanTranslate || velocityDelta == Vector2d.Zero)
 105222152            return;
 153
 4748154        WakeFromCollision();
 4748155        _linearVelocity += velocityDelta;
 4748156        RefreshLinearSpeed();
 4748157    }
 158
 159    internal void ApplyCollisionAngularVelocityDelta(Fixed64 velocityDelta)
 160    {
 109606161        if (!CanRotate || velocityDelta == Fixed64.Zero)
 109034162            return;
 163
 572164        WakeFromCollision();
 572165        _angularVelocity += velocityDelta;
 572166        RefreshAngularSpeed();
 572167    }
 168
 169    internal bool CanApplyCollisionVelocityDeltas(
 170        Vector2d linearVelocityDelta,
 171        Fixed64 angularVelocityDelta)
 172    {
 73972173        linearVelocityDelta = ProjectLinearMotion(linearVelocityDelta);
 73972174        bool linearVelocityFits = Vector2d.TryAdd(
 73972175            _linearVelocity,
 73972176            linearVelocityDelta,
 73972177            out _);
 73972178        bool angularVelocityFits = Fixed64.TryAdd(
 73972179            _angularVelocity,
 73972180            angularVelocityDelta,
 73972181            out _);
 73972182        return (!CanTranslate | linearVelocityFits)
 73972183            & (!CanRotate | angularVelocityFits);
 184    }
 185
 186    internal Vector2d PrepareCollisionPositionCorrection(Vector2d positionCorrection) =>
 4347187        CanTranslate
 4347188            ? AddPositionDisplacement(_position, ProjectLinearMotion(positionCorrection))
 4347189            : _position;
 190
 191    private bool _collisionPositionPublicationPending;
 192
 193    internal void ApplyCollisionPositionState(Vector2d position)
 194    {
 4504195        if (_position == position)
 657196            return;
 3847197        _position = position;
 198        // Recovery needs current geometry; spatial membership can coalesce until publication or a query.
 3847199        Collider.RebuildRuntimeShapeOnly(skipUnchangedMassProperties: true);
 3847200        _collisionPositionPublicationPending = true;
 3847201        Context.Physics2D.MarkCollisionPositionPublicationPending();
 3847202    }
 203
 204    internal void PublishCollisionPositionState()
 205    {
 3743206        if (!_collisionPositionPublicationPending)
 138207            return;
 3605208        _collisionPositionPublicationPending = false;
 3605209        Context.Collisions2D.RefreshColliderPartition(Collider);
 3605210        if (Context.Settings.RuntimeMode.RunsMixedContacts())
 2516211            Context.MixedCollisions.Refresh2DColliderPartition(Collider, rebuildShape: false);
 3605212    }
 213
 214    // Callers project frozen axes first; numeric exhaustion is not a physical wall.
 215    private static Vector2d AddPositionDisplacement(Vector2d start, Vector2d displacement)
 216    {
 35338217        if (!Vector2d.TryAdd(start, displacement, out Vector2d position))
 15218            throw new OverflowException("The requested body position is outside the representable coordinate domain.");
 35323219        return position;
 220    }
 221
 14801222    private bool CanSleep => SleepEnabled && HasSolverMobility;
 223
 224    private void ApplyFreezeConstraintsToMotion()
 225    {
 516226        _linearVelocity = ProjectLinearMotion(_linearVelocity);
 516227        _linearAccelerationStore = ProjectLinearMotion(_linearAccelerationStore);
 516228        _deltaAcceleration = ProjectLinearMotion(_deltaAcceleration);
 516229        RefreshLinearSpeed();
 230
 516231        if (!CanRotate)
 232        {
 450233            _angularVelocity = Fixed64.Zero;
 450234            _angularAccelerationStore = Fixed64.Zero;
 450235            _deltaAngularAcceleration = Fixed64.Zero;
 450236            _angularSpeed = Fixed64.Zero;
 237        }
 516238    }
 239
 240    private void UpdateSleepState()
 241    {
 3242        if (AdvanceSleepCounter())
 1243            Sleep();
 3244    }
 245
 14389246    private bool CanCountTowardSleep => CanSleep
 14389247        && _linearSpeed <= SleepLinearSpeedThreshold && _angularSpeed <= SleepAngularSpeedThreshold
 14389248        && _deltaAcceleration == Vector2d.Zero && _deltaAngularAcceleration == Fixed64.Zero;
 249
 501250    internal bool ReadyForAutomaticSleep => _sleepFrameCount >= SleepFrameThreshold && CanCountTowardSleep;
 251
 252    internal bool AdvanceSleepCounter()
 253    {
 15310254        if (_isSleeping)
 1187255            return false;
 14123256        if (!CanCountTowardSleep)
 257        {
 12055258            _sleepFrameCount = 0;
 12055259            return false;
 260        }
 261
 2068262        if (_sleepFrameCount < SleepFrameThreshold)
 1829263            _sleepFrameCount++;
 264
 2068265        return _sleepFrameCount >= SleepFrameThreshold;
 266    }
 267
 268    private void RefreshLinearSpeed()
 269    {
 22181270        _linearSpeed = _linearVelocity.Magnitude;
 22181271        if (_linearSpeed <= Fixed64.Epsilon)
 272        {
 7623273            _linearVelocity = Vector2d.Zero;
 7623274            _linearSpeed = Fixed64.Zero;
 275        }
 22181276    }
 277
 278    private void RefreshAngularSpeed()
 279    {
 7062280        _angularSpeed = _angularVelocity.Abs();
 7062281        if (_angularSpeed <= Fixed64.Epsilon)
 282        {
 5373283            _angularVelocity = Fixed64.Zero;
 5373284            _angularSpeed = Fixed64.Zero;
 285        }
 7062286    }
 287
 288    internal void RefreshMassPropertiesFromColliderShape()
 289    {
 30350290        if (!_centerOfMassOffsetExplicit)
 30240291            _localCenterOfMassOffset = Collider.CalculateLocalCenterOfMassOffset();
 292
 30350293        RefreshMomentOfInertia(_mass);
 30350294    }
 295
 296    private void RefreshMomentOfInertia(Fixed64 mass)
 297    {
 31269298        Fixed64 inertia = mass > Fixed64.Zero
 31269299            ? Collider.CalculateMomentOfInertia(mass, _localCenterOfMassOffset)
 31269300            : Fixed64.Zero;
 31268301        Fixed64 inverseInertia = inertia > Fixed64.Zero
 31268302            ? Fixed64.One / inertia
 31268303            : Fixed64.Zero;
 31268304        _momentOfInertia = inertia;
 31268305        _inverseMomentOfInertia = inverseInertia;
 31268306    }
 307
 308}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/2D/Bodies/SolidBody2D.ReplayHash.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody2D.ReplayHash.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
 8using Chronicler.Hashing;
 9using FixedMathSharp.Chronicler;
 10
 11namespace Gravitas;
 12
 13/// <content>Writes planar body state and continuous collision trajectories to the replay hash.</content>
 14public sealed partial class SolidBody2D
 15{
 16    internal void ContributeReplayHash(
 17        ref ChronicleHashWriter writer,
 18        GravitasReplayHashMode mode)
 19    {
 172720        writer.WriteSection("body.2d", 4);
 172721        writer.WriteInt32(DynamicId);
 172722        writer.WriteBool(Active);
 172723        writer.WriteEnum(_freezeAxes);
 172724        writer.WriteEnum(_motionType);
 172725        writer.WriteVector2d(_position);
 172726        writer.WriteFixed64(_rotation);
 172727        writer.WriteVector2d(_localCenterOfMassOffset);
 172728        writer.WriteBool(_centerOfMassOffsetExplicit);
 172729        writer.WriteVector2d(_linearVelocity);
 172730        writer.WriteVector2d(_linearAccelerationStore);
 172731        writer.WriteVector2d(_deltaAcceleration);
 172732        writer.WriteFixed64(_linearSpeed);
 172733        writer.WriteFixed64(_angularVelocity);
 172734        writer.WriteFixed64(_angularAccelerationStore);
 172735        writer.WriteFixed64(_deltaAngularAcceleration);
 172736        writer.WriteFixed64(_angularSpeed);
 172737        writer.WriteBool(_isSleeping);
 172738        writer.WriteInt32(_sleepFrameCount);
 172739        writer.WriteFixed64(_mass);
 172740        writer.WriteVector2d(Gravity);
 172741        writer.WriteFixed64(_gravityScale);
 172742        writer.WriteBool(SleepEnabled);
 172743        writer.WriteInt32(SleepFrameThreshold);
 172744        writer.WriteFixed64(SleepLinearSpeedThreshold);
 172745        writer.WriteFixed64(_sleepAngularSpeedThreshold);
 172746        writer.WriteEnum(_groundingMode);
 172747        writer.WriteEnum(_groundProbeMode);
 172748        writer.WriteBool(_useGravityDerivedGroundUpDirection);
 172749        writer.WriteVector2d(_groundUpDirection);
 172750        writer.WriteFixed64(_groundProbeRadius);
 172751        writer.WriteFixed64(GroundedDistanceRay);
 172752        writer.WriteFixed64(GroundDownDistanceOnAir);
 172753        writer.WriteFixed64(GroundMinNormalDot);
 172754        writer.WriteBool(_isGrounded);
 172755        writer.WriteBool(_wasGrounded);
 172756        writer.WriteVector2d(_groundNormal);
 172757        writer.WriteBool(_hasGroundPoint);
 172758        writer.WriteVector2d(_groundPoint);
 172759        writer.WriteVector2d(_lastGroundedPosition);
 172760        writer.WriteEnum(_continuousCollisionMode);
 61
 172762        writer.WriteSection("body.2d.ccd-authoritative", 4);
 172763        writer.WriteBool(_continuousCollisionHandoffPending);
 172764        if (_continuousCollisionHandoffPending)
 65        {
 4066            writer.WriteInt64(_continuousCollisionHandoffToken);
 4067            writer.WriteFixed64(_continuousCollisionHandoffRemainingTime);
 4068            writer.WriteInt32(_continuousCollisionHandoffIgnoredCollider3D?.ReplayOrdinal ?? -1);
 4069            writer.WriteInt32(_continuousCollisionHandoffIgnoredCollider2D?.ReplayOrdinal ?? -1);
 4070            WriteContinuousCollisionTrajectory(ref writer);
 71        }
 72
 172773        if (mode != GravitasReplayHashMode.AuthoritativeWithSolverCaches)
 127974            return;
 75
 44876        writer.WriteSection("body.2d.solver-caches", 4);
 44877        writer.WriteInt64(_continuousCollisionFrameToken);
 44878        WriteContinuousCollisionTrajectory(ref writer);
 44879        writer.WriteInt64(_continuousCollisionHandoffToken);
 44880        writer.WriteFixed64(_continuousCollisionHandoffRemainingTime);
 44881        writer.WriteInt32(LastContinuousCollisionToiIterationCount);
 44882        writer.WriteBool(LastContinuousCollisionToiIterationLimitReached);
 44883        writer.WriteFixed64(_momentOfInertia);
 44884        writer.WriteFixed64(_inverseMomentOfInertia);
 44885        writer.WriteInt32(_continuousCollisionHits.Count);
 44886        writer.WriteInt32(_continuousMixedCollisionHits.Count);
 44887    }
 88
 89    private void WriteContinuousCollisionTrajectory(ref ChronicleHashWriter writer)
 90    {
 48891        writer.WriteInt32(_continuousCollisionTrajectory.Count);
 180692        for (int i = 0; i < _continuousCollisionTrajectory.Count; i++)
 93        {
 41594            Gravitas.CollisionHandling.ContinuousCollisionMotionSegment2D segment =
 41595                _continuousCollisionTrajectory[i];
 41596            writer.WriteFixed64(segment.StartFraction);
 41597            writer.WriteFixed64(segment.EndFraction);
 41598            writer.WriteVector2d(segment.StartPosition);
 41599            writer.WriteVector2d(segment.EndPosition);
 415100            writer.WriteVector2d(segment.Displacement);
 415101            writer.WriteFixed64(segment.StartRotation);
 415102            writer.WriteFixed64(segment.AngularDelta);
 415103            writer.WriteFixed64(segment.AngularVelocity);
 104        }
 488105    }
 106}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/2D/Bodies/SolidBody2D.Serialization.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody2D.Serialization.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
 8using Chronicler;
 9using FixedMathSharp;
 10using Gravitas.Colliders;
 11
 12namespace Gravitas;
 13
 14/// <content>Records planar body state and rebuilds its dependent state after loading.</content>
 15public sealed partial class SolidBody2D
 16{
 17    /// <summary>Records or restores deterministic body state through Chronicler.</summary>
 18    public void RecordData(IChronicler chronicler)
 19    {
 15520        bool active = Active;
 15521        BodyFreezeAxes2D freezeAxes = FreezeAxes;
 15522        PhysicsMobilityType motionType = MotionType;
 15523        Fixed64 mass = Mass;
 15524        Vector2d gravity = Gravity;
 15525        Fixed64 gravityScale = GravityScale;
 15526        bool sleepEnabled = SleepEnabled;
 15527        int sleepFrameThreshold = SleepFrameThreshold;
 15528        Fixed64 sleepLinearSpeedThreshold = SleepLinearSpeedThreshold;
 15529        Fixed64 sleepAngularSpeedThreshold = SleepAngularSpeedThreshold;
 15530        GroundingMode groundingMode = GroundingMode;
 15531        GroundProbeMode2D groundProbeMode = GroundProbeMode;
 15532        bool useGravityDerivedGroundUpDirection = UseGravityDerivedGroundUpDirection;
 15533        Vector2d groundUpDirection = GroundUpDirection;
 15534        Fixed64 groundProbeRadius = GroundProbeRadius;
 15535        Fixed64 groundedDistanceRay = GroundedDistanceRay;
 15536        Fixed64 groundDownDistanceOnAir = GroundDownDistanceOnAir;
 15537        Fixed64 groundMinNormalDot = GroundMinNormalDot;
 15538        RecordValues.Look(chronicler, ref groundMinNormalDot, "GroundMinNormalDot", Fixed64.Half);
 15539        if (chronicler.Mode == SerializationMode.Loading)
 9040            ValidateGroundMinNormalDot(groundMinNormalDot);
 15341        ContinuousCollisionMode continuousCollisionMode = ContinuousCollisionMode;
 42
 15343        RecordValues.Look(chronicler, ref active, "Active", false);
 15344        RecordValues.Look(chronicler, ref freezeAxes, "FreezeAxes", BodyFreezeAxes2D.None);
 15345        RecordValues.Look(chronicler, ref motionType, "MotionType", PhysicsMobilityType.Dynamic);
 15346        if (chronicler.Mode == SerializationMode.Loading)
 47        {
 8848            PreflightLoadedMotionType(motionType);
 8549            ChronicleGuard.ThrowIfArgument(
 8550                (freezeAxes & ~BodyFreezeAxes2D.All) != BodyFreezeAxes2D.None,
 8551                nameof(freezeAxes),
 8552                "Unsupported 2D freeze axis bits.");
 53        }
 54
 14955        RecordValues.Look(chronicler, ref _position, "Position");
 14956        RecordValues.Look(chronicler, ref _rotation, "Rotation");
 14957        RecordValues.Look(chronicler, ref _localCenterOfMassOffset, "LocalCenterOfMassOffset");
 14958        RecordValues.Look(chronicler, ref _centerOfMassOffsetExplicit, "CenterOfMassOffsetExplicit", false);
 14959        RecordValues.Look(chronicler, ref _linearVelocity, "LinearVelocity");
 14960        RecordValues.Look(chronicler, ref _linearAccelerationStore, "LinearAccelerationStore");
 14961        RecordValues.Look(chronicler, ref _deltaAcceleration, "DeltaAcceleration");
 14962        RecordValues.Look(chronicler, ref _linearSpeed, "LinearSpeed");
 14963        RecordValues.Look(chronicler, ref _angularVelocity, "AngularVelocity");
 14964        RecordValues.Look(chronicler, ref _angularAccelerationStore, "AngularAccelerationStore");
 14965        RecordValues.Look(chronicler, ref _deltaAngularAcceleration, "DeltaAngularAcceleration");
 14966        RecordValues.Look(chronicler, ref _angularSpeed, "AngularSpeed");
 14967        RecordValues.Look(chronicler, ref _isSleeping, "IsSleeping");
 14968        RecordValues.Look(chronicler, ref _sleepFrameCount, "SleepFrameCount");
 14969        RecordValues.Look(chronicler, ref mass, "Mass");
 14970        RecordValues.Look(chronicler, ref gravity, "Gravity", Vector2d.Zero);
 14971        RecordValues.Look(chronicler, ref gravityScale, "GravityScale", Fixed64.One);
 14972        RecordValues.Look(chronicler, ref sleepEnabled, "SleepEnabled", true);
 14973        RecordValues.Look(chronicler, ref sleepFrameThreshold, "SleepFrameThreshold", 16);
 14974        RecordValues.Look(chronicler, ref sleepLinearSpeedThreshold, "SleepLinearSpeedThreshold", (Fixed64)0.001f);
 14975        RecordValues.Look(chronicler, ref sleepAngularSpeedThreshold, "SleepAngularSpeedThreshold", (Fixed64)0.001f);
 14976        RecordValues.Look(chronicler, ref groundingMode, "GroundingMode", GroundingMode.Automatic);
 14977        RecordValues.Look(chronicler, ref groundProbeMode, "GroundProbeMode", GroundProbeMode2D.Auto);
 14978        RecordValues.Look(chronicler, ref useGravityDerivedGroundUpDirection, "UseGravityDerivedGroundUpDirection", true
 14979        RecordValues.Look(chronicler, ref groundUpDirection, "GroundUpDirection", Vector2d.Forward);
 14980        RecordValues.Look(chronicler, ref groundProbeRadius, "GroundProbeRadius");
 14981        if (chronicler.Mode == SerializationMode.Loading)
 8482            ChronicleGuard.ThrowIfArgument(groundProbeRadius > ColliderGeometryPolicy.MaximumGeometryReach,
 8483                nameof(GroundProbeRadius), "Ground probe radius must not exceed 4,096 world units.");
 14884        RecordValues.Look(chronicler, ref groundedDistanceRay, "GroundedDistanceRay", Fixed64.Half);
 14885        RecordValues.Look(chronicler, ref groundDownDistanceOnAir, "GroundDownDistanceOnAir", Fixed64.Half);
 14886        RecordValues.Look(chronicler, ref _isGrounded, "IsGrounded");
 14887        RecordValues.Look(chronicler, ref _wasGrounded, "WasGrounded");
 14888        RecordValues.Look(chronicler, ref _groundNormal, "GroundNormal");
 14889        RecordValues.Look(chronicler, ref _hasGroundPoint, "HasGroundPoint");
 14890        RecordValues.Look(chronicler, ref _groundPoint, "GroundPoint");
 14891        RecordValues.Look(chronicler, ref _lastGroundedPosition, "LastGroundedPosition");
 14892        RecordValues.Look(chronicler, ref continuousCollisionMode, "ContinuousCollisionMode", ContinuousCollisionMode.In
 93
 14894        if (chronicler.Mode == SerializationMode.Loading)
 95        {
 8396            ApplyLoadedMotionType(motionType);
 8397            DiscardContinuousCollisionHandoff();
 8398            InvalidateContinuousCollisionFrame();
 8399            ContinuousCollisionMode = continuousCollisionMode;
 81100            _rotation = CanonicalizeRotation(_rotation);
 81101            _freezeAxes = freezeAxes;
 102            // Restore saved sleep/motion; the public setter is a waking host mutation.
 81103            _mass = mass;
 104            // Restore policy without the public setters' cache/wake mutations.
 81105            _gravity = gravity;
 81106            GravityScale = gravityScale;
 81107            SleepEnabled = sleepEnabled;
 81108            SleepFrameThreshold = sleepFrameThreshold;
 81109            SleepLinearSpeedThreshold = sleepLinearSpeedThreshold;
 81110            SleepAngularSpeedThreshold = sleepAngularSpeedThreshold;
 81111            _groundingMode = groundingMode;
 81112            _groundProbeMode = groundProbeMode;
 81113            _useGravityDerivedGroundUpDirection = useGravityDerivedGroundUpDirection;
 81114            _groundUpDirection = groundUpDirection.MagnitudeSquared > Fixed64.Epsilon
 81115                ? groundUpDirection.Normalized
 81116                : DefaultGroundUpDirection;
 81117            _groundProbeRadius = groundProbeRadius < Fixed64.Zero ? Fixed64.Zero : groundProbeRadius;
 81118            GroundedDistanceRay = groundedDistanceRay;
 81119            GroundDownDistanceOnAir = groundDownDistanceOnAir;
 81120            _groundMinNormalDot = groundMinNormalDot;
 81121            _groundNormal = _groundNormal.MagnitudeSquared > Fixed64.Epsilon
 81122                ? _groundNormal.Normalized
 81123                : Vector2d.Zero;
 81124            _groundedTransitionCapturedForStep = false;
 81125            _groundCollider = null;
 81126            _groundColliderContactDependencyVersion = 0;
 81127            _groundColliderLifetimeVersion = 0;
 81128            _groundOwnContactDependencyVersion = 0;
 81129            _lastGroundCheckFrame = -1;
 81130            ClearGroundContactCandidate();
 131        }
 132
 146133        Collider.RecordData(chronicler);
 134
 144135        if (chronicler.Mode == SerializationMode.Loading)
 136        {
 79137            RefreshMassPropertiesFromColliderShape();
 79138            ApplyLoadedFreezeConstraintsToMotion();
 79139            ApplyLoadedState();
 79140            if (Active)
 141            {
 73142                InvalidateMassDependentSolverState();
 73143                RefreshPartitionAwakeState();
 144            }
 79145            if (!active || Collider.Id < 0)
 8146                Deactivate();
 79147            _groundingStateVersion++;
 148        }
 144149    }
 150
 151    private void ApplyLoadedState()
 152    {
 79153        SetHostWorldPose(Agent.Transform, _position, _rotation);
 79154        Collider.Rebuild();
 79155    }
 156
 157    private void ApplyLoadedFreezeConstraintsToMotion()
 158    {
 79159        _linearVelocity = ProjectLinearMotion(_linearVelocity);
 79160        _linearAccelerationStore = ProjectLinearMotion(_linearAccelerationStore);
 79161        _deltaAcceleration = ProjectLinearMotion(_deltaAcceleration);
 79162        RefreshLinearSpeed();
 163
 79164        if (IsRotationFullyFrozen)
 165        {
 2166            _angularVelocity = Fixed64.Zero;
 2167            _angularAccelerationStore = Fixed64.Zero;
 2168            _deltaAngularAcceleration = Fixed64.Zero;
 2169            _angularSpeed = Fixed64.Zero;
 2170            return;
 171        }
 172
 77173        RefreshAngularSpeed();
 77174    }
 175}

Methods/Properties

get_LocalCenterOfMassOffset()
set_LocalCenterOfMassOffset(FixedMathSharp.Vector2d)
get_WorldCenterOfMass()
TryGetWorldCenterOfMass(FixedMathSharp.Vector2d&)
TryGetOffsetFromCenterOfMass(Gravitas.CollisionHandling.ContactAnchor2D,FixedMathSharp.Vector2d&)
GetCenterOfMassAnchor()
ResetCenterOfMassFromCollider()
TryReconfigureCollider(Gravitas.Colliders.ColliderShapeDefinition2D,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,Gravitas.Colliders.LSCollider2D&,System.Exception&,System.Action)
CaptureSynchronizedStatePublication(System.Action,SwiftCollections.SwiftList`1<System.Exception>&)
get_ContinuousCollisionFrameStart()
get_ContinuousCollisionFrameDisplacement()
get_ContinuousCollisionFrameRotation()
get_ContinuousCollisionFrameEnd()
get_ContinuousCollisionFrameTargetRotation()
get_ContinuousCollisionTrajectoryCount()
GetContinuousCollisionTrajectorySegment(System.Int32)
get_HasContinuousCollisionMotion()
ShouldOwnContinuousCollisionMovingPair(System.Boolean)
get_HasContinuousCollisionRotationalMotion()
SampleContinuousCollisionPosition(FixedMathSharp.Fixed64)
SampleContinuousCollisionRotation(FixedMathSharp.Fixed64)
SampleContinuousCollisionLinearVelocity(FixedMathSharp.Fixed64)
SampleContinuousCollisionAngularVelocity(FixedMathSharp.Fixed64)
ResolveLegacyContinuousCollisionFrameStart()
ResolveLegacyContinuousCollisionFrameDisplacement()
EnsureContinuousCollisionFramePrepared(System.Int64,System.Boolean)
PrepareMovableContinuousCollisionMotion()
SetContinuousCollisionFrameSegment(Gravitas.CollisionHandling.ContinuousCollisionMotionSegment2D)
AppendContinuousCollisionFrameSegment(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64)
AppendContinuousCollisionFrameSegment(FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
ValidateContinuousCollisionPosition(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64)
CanAppendContinuousCollisionFrameSegment(FixedMathSharp.Fixed64)
TryResolveContinuousCollisionMotionBound(FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&)
ResolveContinuousCollisionTrajectoryBounds(FixedMathSharp.Fixed64)
RemoveSupersededContinuousCollisionFrameSegments(FixedMathSharp.Fixed64,FixedMathSharp.Vector2d)
ResolveContinuousCollisionSegment(FixedMathSharp.Fixed64)
GetContinuousCollisionTrajectorySegmentIndex(FixedMathSharp.Fixed64)
GetContinuousCollisionTrajectorySegmentRange(FixedMathSharp.Fixed64,System.Int32&,System.Int32&)
InvalidateContinuousCollisionFrame()
ApplyContinuousCollisionHandoff(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,Gravitas.Colliders.LSCollider,Gravitas.Colliders.LSCollider2D)
ApplyContinuousCollisionHandoff(FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.Colliders.LSCollider,Gravitas.Colliders.LSCollider2D)
ApplyContinuousCollisionHandoffState(FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.Colliders.LSCollider,Gravitas.Colliders.LSCollider2D)
ApplyContinuousCollisionHandoffStateReserved(FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.Colliders.LSCollider,Gravitas.Colliders.LSCollider2D)
ValidateContinuousCollisionHandoffPosition(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
HasRemainingHandoffMotion(FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
CanApplyContinuousCollisionHandoffState(FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Vector2d&)
TryConsumeContinuousCollisionHandoff(System.Boolean,System.Boolean)
DiscardContinuousCollisionHandoff()
TryResolveContinuousCollision(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d&)
TryResolveContinuousCollision(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d&,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Boolean)
TryApplyContinuousCollisionDynamicResponse(FixedMathSharp.Vector2d,Gravitas.CollisionHandling.ContinuousCollisionTargetKind,Gravitas.Colliders.LSCollider2D,Gravitas.Colliders.LSCollider,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
TryApplyContinuousCollisionDynamicResponse(Gravitas.SolidBody2D,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
TryApplyContinuousCollisionMixed3DResponse(Gravitas.SolidBody,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
ApplyContinuousCollisionSourceResponse(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64)
UpdateContinuousCollisionFrameTrajectory(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64)
ResolveContinuousCollisionFrameFraction(FixedMathSharp.Fixed64)
ResolveContinuousCollisionRestitution(Gravitas.SolidBody2D,FixedMathSharp.Fixed64)
ResolveContinuousCollisionRestitution(Gravitas.SolidBody,FixedMathSharp.Fixed64)
RemoveClosingContinuousCollisionVelocity(FixedMathSharp.Vector2d)
ShouldUseContinuousCollision(Gravitas.ContinuousCollisionMode&)
ResolveContinuousCollisionMode()
ResolveContinuousCollisionProxyRadius()
ResolveMixedContinuousCollisionProxyRadius()
IsValidContinuousCollisionHit(Gravitas.Queries.Physics2DHit)
IsClosingContinuousCollisionHit(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d)
IsValidContinuousCollisionTarget(Gravitas.Colliders.LSCollider2D)
IsValidMixedContinuousCollisionHit(Gravitas.Colliders.LSCollider)
TryGetFirstContinuousCollisionHit(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Vector2d&,FixedMathSharp.Fixed64&,Gravitas.CollisionHandling.ContinuousCollisionTargetKind&,Gravitas.Colliders.LSCollider2D&,Gravitas.Colliders.LSCollider&)
TryGetFirstValidContinuousCollisionHit(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,System.Int32,Gravitas.Queries.Physics2DHit&)
AdmitStaticSurfaceContinuousCollisionHit(Gravitas.Queries.Physics2DHit&,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d)
TryGetFirstValidMixedContinuousCollisionHit(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,System.Int32,Gravitas.Queries.PhysicsMixedHit&)
UsesMixedStaticManifold(Gravitas.Colliders.LSCollider)
TryBuildStaticMixedContinuousCollisionManifold(Gravitas.Colliders.LSCollider,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64)
TryApplyStaticMixedContinuousCollisionResponse(Gravitas.Colliders.LSCollider,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
TryGetFirstDynamicContinuousCollisionHit(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.Queries.Physics2DHit&,FixedMathSharp.Fixed64&)
TryGetDynamicRelativeContinuousCollisionHit(Gravitas.SolidBody2D,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.Queries.Physics2DHit&,FixedMathSharp.Fixed64&)
TranslateContactAnchor(Gravitas.CollisionHandling.ContactAnchor2D,FixedMathSharp.Geometry.FixedSegment2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
TryGetFirstDynamicMixedContinuousCollisionHit(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,Gravitas.DynamicMixedIntervalHit&)
TryGetDynamicMixed3DContinuousCollisionHit(Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.DynamicMixedIntervalHit&)
IsEligibleDynamicContinuousCollisionTarget(Gravitas.SolidBody2D)
IsEligibleDynamicMixed3DTarget(Gravitas.SolidBody)
TryResolveKinematicContinuousCollision(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d&)
TryGetFirstKinematicStaticContinuousCollisionHit(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&)
TryApplyKinematicDynamicContinuousCollisionPushes(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Boolean&,FixedMathSharp.Fixed64&)
GatherKinematicDynamic2DContinuousCollisionHits(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
GatherKinematicDynamicMixed3DContinuousCollisionHits(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Boolean&,FixedMathSharp.Fixed64&)
ApplyKinematicContinuousCollisionHandoff(Gravitas.SolidBody2D,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
ApplyKinematicContinuousCollisionHandoff(Gravitas.SolidBody,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
get_HasRotationInvariantCollider()
HasNearbyRotationalContinuousCollisionTarget(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64)
IsRotatingContinuousCollisionTarget(Gravitas.SolidBody2D)
IsRotatingContinuousCollisionTarget(Gravitas.SolidBody)
ShouldUseRotationalContinuousCollisionArbiter(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Boolean)
TryResolveRotationalContinuousCollision(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d&,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&)
TryResolveRotationalContinuousCollision(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d&,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Boolean)
TryResolveKinematicRotationalContinuousCollision(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d&,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&)
GatherRotationalContinuousCollisionCandidates(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64)
TryFindEarliestRotationalContinuousCollision(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&,Gravitas.CollisionHandling.Contact2D&,System.Boolean&,FixedMathSharp.Fixed64&,Gravitas.Colliders.LSCollider2D&)
TryFindEarliestRotationalContinuousCollisionAgainstTarget(Gravitas.Colliders.LSCollider2D,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&,Gravitas.CollisionHandling.Contact2D&,System.Boolean&,FixedMathSharp.Fixed64&)
CanExcludeInvariantTangentialContact(Gravitas.Queries.Physics2DHit,FixedMathSharp.Vector2d)
IsRotationalIntervalSeparated(Gravitas.Colliders.LSCollider2D,FixedMathSharp.Fixed64)
TryGetCirclePairSeparationGap(Gravitas.Colliders.LSCollider2D,FixedMathSharp.Fixed64&)
TryGetCircleSeparationGap(Gravitas.Colliders.LSCircleCollider2D,Gravitas.Colliders.LSCollider2D,FixedMathSharp.Fixed64&)
SampleRotationalContinuousPairPose(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.SolidBody2D,FixedMathSharp.Fixed64,System.Boolean)
IsMovingRotationalContinuousCollisionTarget(Gravitas.SolidBody2D)
.ctor(Gravitas.IMatterAgent,Gravitas.Colliders.LSCollider2D)
ResolvedMixedSurfaceFamilies(Gravitas.StaticSurface)
SetMixedContinuousCollisionSamplePose(FixedMathSharp.Vector2d,FixedMathSharp.Fixed64)
ResolveMixedContinuousCollisionTrajectoryBounds(FixedMathSharp.Fixed64)
.ctor(FixedMathSharp.Fixed64,Gravitas.CollisionHandling.MixedContact,System.Boolean,System.Boolean,FixedMathSharp.Fixed64,Gravitas.Colliders.LSCollider,Gravitas.CollisionHandling.ContactManifold)
TryFindEarliestMixedRotationalContinuousCollision(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Boolean,Gravitas.SolidBody2D/RotationalMixed3DHit&)
GatherMixedRotationalContinuousCollisionCandidates(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64)
ConsiderMixedRotationalContinuousCollisionCandidate(Gravitas.Colliders.LSCollider,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Boolean&,FixedMathSharp.Fixed64&,System.Int32&,Gravitas.CollisionHandling.MixedContact&,System.Boolean&,FixedMathSharp.Fixed64&,Gravitas.Colliders.LSCollider&)
TryFindEarliestMixedRotationalContinuousCollisionAgainstTarget(Gravitas.Colliders.LSCollider,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Boolean,FixedMathSharp.Fixed64&,Gravitas.CollisionHandling.MixedContact&,System.Boolean&,FixedMathSharp.Fixed64&)
SampleMixedRotationalContinuousPairPose(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.SolidBody,System.Boolean)
TryGetExactSphereCircleTranslationalContact(Gravitas.SolidBody,Gravitas.CollisionHandling.ContinuousCollisionMotionSegment3D,FixedMathSharp.Geometry.FixedSegment,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Geometry.FixedSegment2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Boolean&,FixedMathSharp.Fixed64&,FixedMathSharp.Fixed64&,FixedMathSharp.Fixed64&,Gravitas.CollisionHandling.MixedContact&)
BuildMixedRotationalContinuousContact(Gravitas.Colliders.LSCollider,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.MixedContact&,System.Boolean&)
IsValidMixedRotationalContinuousCollisionTarget(Gravitas.Colliders.LSCollider)
IsMovingMixedRotationalContinuousCollisionTarget(Gravitas.SolidBody)
TryApplyMixedTranslationalManifoldResponse(Gravitas.SolidBody,FixedMathSharp.Vector2d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
TryApplyMixedRotationalContinuousCollisionResponse(Gravitas.Colliders.LSCollider,Gravitas.CollisionHandling.MixedContact,FixedMathSharp.Fixed64,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ContactManifold)
TryApplyRotationalContinuousCollisionResponse(Gravitas.SolidBody2D,Gravitas.CollisionHandling.Contact2D,FixedMathSharp.Fixed64,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
ResolveRotationalFrameFraction(FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
StopRotationalContinuousCollision(FixedMathSharp.Vector2d)
.ctor(Gravitas.IMatterAgent,Gravitas.Colliders.LSCollider2D)
get_FreezeAxes()
set_FreezeAxes(Gravitas.BodyFreezeAxes2D)
get_IsPositionFullyFrozen()
get_MotionType()
get_IsDynamic()
get_IsStatic()
get_IsKinematic()
SetMotionType(Gravitas.PhysicsMobilityType)
PrepareMotionTypeTransition(Gravitas.PhysicsMobilityType)
CommitMotionTypeTransition(Gravitas.PhysicsMobilityType)
ApplyLoadedMotionType(Gravitas.PhysicsMobilityType)
PreflightLoadedMotionType(Gravitas.PhysicsMobilityType)
ThrowIfRuntimeRegistrationMissing()
ReconcileMotionTypeRegistration(Gravitas.PhysicsMobilityType)
ClearMotionForMotionTypeTransition()
get_ContinuousCollisionMode()
set_ContinuousCollisionMode(Gravitas.ContinuousCollisionMode)
get_IsRotationFullyFrozen()
get_Mass()
set_Mass(FixedMathSharp.Fixed64)
InvalidateMassDependentSolverState()
RefreshPartitionAwakeState()
get_InverseMass()
get_CanTranslate()
get_CanRotate()
get_HasSolverMobility()
get_EffectiveInverseMass()
get_EffectiveInverseMomentOfInertia()
ProjectLinearMotion(FixedMathSharp.Vector2d)
ProjectLinearEndpoint(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d)
GetConstrainedInverseMass(FixedMathSharp.Vector2d)
get_MomentOfInertia()
get_InverseMomentOfInertia()
get_AngularVelocity()
get_AngularAcceleration()
get_AngularSpeed()
get_Position()
get_Rotation()
get_LinearVelocity()
get_LinearSpeed()
get_Gravity()
set_Gravity(FixedMathSharp.Vector2d)
get_GravityScale()
set_GravityScale(FixedMathSharp.Fixed64)
get_SleepEnabled()
set_SleepEnabled(System.Boolean)
get_SleepFrameThreshold()
set_SleepFrameThreshold(System.Int32)
get_SleepLinearSpeedThreshold()
set_SleepLinearSpeedThreshold(FixedMathSharp.Fixed64)
get_SleepAngularSpeedThreshold()
set_SleepAngularSpeedThreshold(FixedMathSharp.Fixed64)
get_IsSleeping()
get_IsAwakeForCollision()
Initialize(FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,Gravitas.PhysicsMobilityType)
GetWorldPoint(FixedMathSharp.Vector2d)
TryGetWorldPoint(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d&)
GetLocalPoint(FixedMathSharp.Vector2d)
TryGetLocalPoint(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d&)
ResetPosition(FixedMathSharp.Vector2d,FixedMathSharp.Fixed64)
Sleep()
Wake()
WakeFromCollision()
LateSimulate()
LateSimulate(System.Boolean,System.Boolean)
OnVisualize()
UpdateKinematicPositionAndRotation(System.Boolean)
CanonicalizeRotation(FixedMathSharp.Fixed64)
ReadHostRotation(FixedMathSharp.FixedTransform)
GetHostYaw(FixedMathSharp.Fixed64)
SetHostWorldPose(FixedMathSharp.FixedTransform,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64)
PublishAuthoritativePose()
Deactivate()
.cctor()
.ctor(Gravitas.IMatterAgent,Gravitas.Colliders.LSCollider2D)
get_GroundingMode()
get_GroundProbeMode()
set_GroundProbeMode(Gravitas.GroundProbeMode2D)
get_UseGravityDerivedGroundUpDirection()
set_UseGravityDerivedGroundUpDirection(System.Boolean)
get_GroundUpDirection()
set_GroundUpDirection(FixedMathSharp.Vector2d)
get_GroundProbeRadius()
set_GroundProbeRadius(FixedMathSharp.Fixed64)
get_GroundedDistanceRay()
set_GroundedDistanceRay(FixedMathSharp.Fixed64)
get_GroundMinNormalDot()
set_GroundMinNormalDot(FixedMathSharp.Fixed64)
get_IsGrounded()
set_IsGrounded(System.Boolean)
get_WasGrounded()
get_GroundNormal()
get_HasGroundPoint()
get_GroundPoint()
TryGetGroundPoint(FixedMathSharp.Vector2d&)
get_LastGroundedPosition()
UseManualGrounding(System.Boolean)
UseAutomaticGrounding(System.Boolean)
SetManualGrounding(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d)
ClearManualGrounding()
CheckGround()
BeginAutomaticGroundingRefresh()
TryAcceptContactGroundCandidate(Gravitas.Colliders.LSCollider2D,Gravitas.Colliders.LSCollider2D,Gravitas.CollisionHandling.ManifoldContact2D,System.Boolean)
CompleteAutomaticGroundingRefresh()
CheckGroundForSimulation()
RefreshAutomaticGroundSupportBeforeMotion()
HasCurrentGroundCollider()
WakeForContactChange()
RemoveIntoGroundComponent(FixedMathSharp.Vector2d)
get_CanUseAutomaticGrounding()
CheckGround(System.Boolean)
TryFindGroundHit(Gravitas.GroundProbeMode2D,FixedMathSharp.Fixed64,FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,Gravitas.Queries.Physics2DHit&)
IsValidGroundHit(Gravitas.Queries.Physics2DHit)
IsValidGroundCollider(Gravitas.Colliders.LSCollider2D)
ResolveGroundProbeMode()
ResolveGroundProbeRadius()
ValidateGroundMinNormalDot(FixedMathSharp.Fixed64)
IsValidGroundNormal(FixedMathSharp.Vector2d,FixedMathSharp.Fixed64&)
InvalidateAutomaticGroundSupport()
ResolveGroundUpDirection()
CaptureGroundedTransitionState()
CaptureGroundedStepState()
CompleteGroundedStepState()
ClearGrounding()
ClearGroundingForAutomaticRefresh()
SetGroundingState(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d,Gravitas.Colliders.LSCollider2D,System.Boolean)
ResetGroundingForInitialize(FixedMathSharp.Vector2d)
RevalidateAutomaticGroundingWrite(System.Int64,Gravitas.Colliders.ColliderLifetimeToken2D&,Gravitas.Colliders.ColliderLifetimeToken2D&)
ClearGroundContactCandidate()
CompareGroundContactCandidate(FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Int32,System.UInt64)
AddForce(FixedMathSharp.Vector2d)
AddTorque(FixedMathSharp.Fixed64)
AddLinearImpulse(FixedMathSharp.Vector2d)
AddAngularImpulse(FixedMathSharp.Fixed64)
SetPosition(FixedMathSharp.Vector2d)
SetRotation(FixedMathSharp.Fixed64)
RefreshStaticColliderAfterExplicitPoseChange()
PreflightStaticPoseChange()
RefreshPartitionMobility()
ApplyCollisionLinearVelocityDelta(FixedMathSharp.Vector2d)
ApplyCollisionAngularVelocityDelta(FixedMathSharp.Fixed64)
CanApplyCollisionVelocityDeltas(FixedMathSharp.Vector2d,FixedMathSharp.Fixed64)
PrepareCollisionPositionCorrection(FixedMathSharp.Vector2d)
ApplyCollisionPositionState(FixedMathSharp.Vector2d)
PublishCollisionPositionState()
AddPositionDisplacement(FixedMathSharp.Vector2d,FixedMathSharp.Vector2d)
get_CanSleep()
ApplyFreezeConstraintsToMotion()
UpdateSleepState()
get_CanCountTowardSleep()
get_ReadyForAutomaticSleep()
AdvanceSleepCounter()
RefreshLinearSpeed()
RefreshAngularSpeed()
RefreshMassPropertiesFromColliderShape()
RefreshMomentOfInertia(FixedMathSharp.Fixed64)
ContributeReplayHash(Chronicler.Hashing.ChronicleHashWriter&,Gravitas.GravitasReplayHashMode)
WriteContinuousCollisionTrajectory(Chronicler.Hashing.ChronicleHashWriter&)
RecordData(Chronicler.IChronicler)
ApplyLoadedState()
ApplyLoadedFreezeConstraintsToMotion()