< Summary

Information
Class: Gravitas.SolidBody
Assembly: Gravitas
File(s): File 1: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.CenterOfMass.cs
File 2: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.ColliderReconfiguration.cs
File 3: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.ContinuousCollision.cs
File 4: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.ContinuousCollision.Dynamic.cs
File 5: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.ContinuousCollision.Helpers.cs
File 6: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.ContinuousCollision.Hits.cs
File 7: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.ContinuousCollision.Kinematic.cs
File 8: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.ContinuousCollision.Rotational.cs
File 9: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.ContinuousCollision.Rotational.Mixed.cs
File 10: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.ContinuousCollision.Rotational.Response.cs
File 11: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.ContinuousCollision.Rotational.Search.cs
File 12: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.cs
File 13: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.Grounding.cs
File 14: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.MassProperties.cs
File 15: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.Motion.cs
File 16: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.ReplayHash.cs
File 17: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.Serialization.cs
File 18: /home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.Visualization.cs
Line coverage
100%
Covered lines: 5278
Uncovered lines: 0
Coverable lines: 5278
Total lines: 8874
Line coverage: 100%
Branch coverage
100%
Covered branches: 1909
Total branches: 1909
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%1212100%
File 2: TryFindColliderReconfigurationBlocker(...)100%1616100%
File 2: CommitColliderReconfiguration(...)100%66100%
File 2: CaptureSynchronizedStatePublication(...)100%22100%
File 2: CanPublishRootPosition(...)100%66100%
File 2: GetSupportedRuntimeShape(...)100%55100%
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%88100%
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%1818100%
File 3: InvalidateContinuousCollisionTrajectory()100%11100%
File 3: SetSingleContinuousCollisionSegment(...)100%11100%
File 3: AppendContinuousCollisionSegment(...)100%11100%
File 3: AppendContinuousCollisionSegment(...)100%44100%
File 3: ValidateContinuousCollisionPosition(...)100%11100%
File 3: ResolveCommittedLinearVelocity(...)100%22100%
File 3: CanAppendContinuousCollisionSegment(...)100%44100%
File 3: TryResolveContinuousCollisionMotionBound(...)100%88100%
File 3: ResolveContinuousCollisionTrajectoryBounds(...)100%22100%
File 3: RemoveSupersededContinuousCollisionSegments(...)100%88100%
File 3: ResolveContinuousCollisionSegment(...)100%11100%
File 3: GetContinuousCollisionTrajectorySegmentIndex(...)100%44100%
File 3: GetContinuousCollisionTrajectorySegmentRange(...)100%22100%
File 4: ApplyContinuousCollisionHandoff(...)100%22100%
File 4: ApplyContinuousCollisionHandoff(...)100%22100%
File 4: ApplyContinuousCollisionHandoffReserved(...)100%66100%
File 4: ValidateContinuousCollisionHandoffPosition(...)100%44100%
File 4: HasRemainingHandoffMotion(...)100%44100%
File 4: CanApplyContinuousCollisionHandoff(...)100%22100%
File 4: TryConsumeContinuousCollisionHandoff(...)100%11100%
File 4: TryConsumeQueuedContinuousCollisionHandoff(...)100%11100%
File 4: TryConsumeContinuousCollisionHandoff(...)100%2424100%
File 4: NotifyAuthoritativeMovement()100%22100%
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: TryApplyContinuousCollisionMixed2DResponse(...)100%2626100%
File 4: UpdateContinuousCollisionFrameTrajectory(...)100%11100%
File 4: ResolveContinuousCollisionFrameFraction(...)100%11100%
File 4: ResolveContinuousCollisionRestitution(...)100%22100%
File 4: ResolveContinuousCollisionRestitution(...)100%22100%
File 4: RemoveClosingContinuousCollisionVelocity(...)100%22100%
File 5: IsEligibleDynamicContinuousCollisionTarget(...)100%88100%
File 5: IsEligibleDynamicMixed2DTarget(...)100%88100%
File 5: ShouldUseContinuousCollision(...)100%22100%
File 5: ResolveContinuousCollisionMode()100%1010100%
File 5: ResolveContinuousCollisionProxyRadius()100%11100%
File 5: IsClosingContinuousCollisionHit(...)100%11100%
File 5: ResolveShapeExactContinuousClosingNormal(...)100%44100%
File 5: IsValidContinuousCollisionTarget(...)100%88100%
File 5: IsValidMixedContinuousCollisionHit(...)100%88100%
File 6: TryGetFirstContinuousCollisionHit(...)100%2222100%
File 6: TryGetFirstValidContinuousCollisionHit(...)100%2828100%
File 6: AdmitContinuousSurfaceTriangle(...)100%1010100%
File 6: QueryStaticContinuousCollisionHits(...)100%66100%
File 6: TryGetFirstValidMixedContinuousCollisionHit(...)100%2020100%
File 6: UsesMixedStaticManifold(...)100%88100%
File 6: TryBuildStaticMixedContinuousCollisionManifold(...)100%11100%
File 6: TryApplyStaticMixedContinuousCollisionResponse(...)100%44100%
File 6: TryRefineShapeExactContinuousCollisionHit(...)100%44100%
File 6: TryRefineContinuousCollisionAgainstTargetSphere(...)100%22100%
File 6: ApplyShapeExactContinuousContactSlop(...)100%22100%
File 6: PrepareExactConvexSourceSweep(...)100%44100%
File 6: IsExactConvexSourceSupported(...)100%1414100%
File 6: ContinuousCollisionHitComesBefore(...)100%22100%
File 6: TryGetFirstDynamicContinuousCollisionHit(...)100%88100%
File 6: TryGetDynamicRelativeContinuousCollisionHit(...)100%5252100%
File 6: TrySweepRelativeSourceSphere(...)100%22100%
File 6: TryGetFirstDynamicMixedContinuousCollisionHit(...)100%88100%
File 6: TryGetDynamicMixed2DContinuousCollisionHit(...)100%2828100%
File 7: TryResolveKinematicContinuousCollision(...)100%1818100%
File 7: TryGetFirstKinematicStaticContinuousCollisionHit(...)100%1010100%
File 7: TryApplyKinematicDynamicContinuousCollisionPushes(...)100%88100%
File 7: GatherKinematicDynamic3DContinuousCollisionHits(...)100%88100%
File 7: GatherKinematicDynamicMixed2DContinuousCollisionHits(...)100%1212100%
File 7: ApplyKinematicContinuousCollisionHandoff(...)100%44100%
File 7: ApplyKinematicContinuousCollisionHandoff(...)100%66100%
File 8: get_HasRotationInvariantCollider()100%22100%
File 8: TryResolveRotationalContinuousCollision(...)100%11100%
File 8: TryResolveRotationalContinuousCollision(...)100%8888100%
File 8: TryResolveKinematicRotationalContinuousCollision(...)100%8686100%
File 8: IsRotationalIntervalSeparated(...)100%22100%
File 8: TryGetSpherePairSeparationGap(...)100%44100%
File 8: TryGetSphereSeparationGap(...)100%1616100%
File 8: SampleDynamicRotationalContinuousPose(...)100%11100%
File 8: SampleKinematicRotationalContinuousPose(...)100%11100%
File 8: ResolveKinematicAngularDistanceRadians(...)100%11100%
File 8: TrySampleRotationalContinuousCollision(...)100%22100%
File 8: TryBuildContinuousCollisionManifold(...)100%22100%
File 8: TrySelectRotationalClosingContact(...)100%3030100%
File 8: ResolveSourceOutwardContactNormal(...)100%22100%
File 8: IsMovingRotationalContinuousCollisionTarget(...)100%1212100%
File 8: ResolveRotationalFrameFraction(...)100%11100%
File 8: OrderRotationalContinuousCollisionPair(...)100%22100%
File 8: IntegrateAngularRotation(...)100%11100%
File 8: IntegrateAngularRotation(...)100%11100%
File 9: .ctor(...)100%11100%
File 9: SetMixedContinuousCollisionSamplePose(...)100%11100%
File 9: .ctor(...)100%11100%
File 9: TryFindEarliestMixedRotationalContinuousCollision(...)100%2424100%
File 9: GatherMixedRotationalContinuousCollisionCandidates(...)100%22100%
File 9: ConsiderMixedRotationalContinuousCollisionCandidate(...)100%1212100%
File 9: TryGetInvariantMixedPlane(...)100%44100%
File 9: TryFindEarliestMixedRotationalContinuousCollisionAgainstTarget(...)100%6464100%
File 9: SampleMixedRotationalContinuousPairPose(...)100%44100%
File 9: TryGetExactSphereCircleTranslationalContact(...)100%2626100%
File 9: BuildMixedRotationalContinuousContact(...)100%1212100%
File 9: IsValidMixedRotationalContinuousCollisionTarget(...)100%66100%
File 9: IsMovingMixedRotationalContinuousCollisionTarget(...)100%1010100%
File 9: TryApplyMixedTranslationalManifoldResponse(...)100%22100%
File 9: TryApplyMixedRotationalContinuousCollisionResponse(...)100%4444100%
File 10: TryApplyRotationalContinuousCollisionResponse(...)100%6868100%
File 10: TryCalculateExactRotationalVelocityDeltas(...)100%11100%
File 10: ResolveContinuousCollisionInverseInertia(...)100%22100%
File 10: StopRotationalContinuousCollision(...)100%11100%
File 11: HasNearbyRotationalContinuousCollisionTarget(...)100%1010100%
File 11: IsRotatingContinuousCollisionTarget(...)100%66100%
File 11: IsRotatingContinuousCollisionTarget(...)100%44100%
File 11: GatherRotationalContinuousCollisionCandidates(...)100%1010100%
File 11: TryFindEarliestRotationalContinuousCollision(...)100%2828100%
File 11: CanExcludeInvariantTangentialContact(...)100%2828100%
File 11: HasInvariantContinuousProjection(...)100%1010100%
File 11: TryGetFullColliderSupportPoint(...)100%1010100%
File 11: TryGetLeafColliderSupportPoint(...)100%44100%
File 11: GetExactMeshSupportPoint(...)100%44100%
File 11: TryFindEarliestRotationalContinuousCollisionAgainstTarget(...)100%5050100%
File 11: SampleRotationalContinuousPairPose(...)100%44100%
File 12: .ctor(...)100%22100%
File 12: get_DynamicId()100%11100%
File 12: get_FreezeAxes()100%11100%
File 12: set_FreezeAxes(...)100%44100%
File 12: get_IsPositionFullyFrozen()100%11100%
File 12: get_MotionType()100%11100%
File 12: get_IsDynamic()100%11100%
File 12: get_IsStatic()100%11100%
File 12: get_IsKinematic()100%11100%
File 12: SetMotionType(...)100%22100%
File 12: PrepareMotionTypeTransition(...)100%44100%
File 12: CommitMotionTypeTransition(...)100%11100%
File 12: ApplyLoadedMotionType(...)100%44100%
File 12: PreflightLoadedMotionType(...)100%22100%
File 12: ThrowIfRuntimeRegistrationMissing()100%22100%
File 12: ReconcileMotionTypeRegistration(...)100%11100%
File 12: SetDynamicId(...)100%11100%
File 12: get_ContinuousCollisionMode()100%11100%
File 12: set_ContinuousCollisionMode(...)100%11100%
File 12: get_PositionTransform()100%11100%
File 12: get_RotationTransform()100%11100%
File 12: get_PositionChangePending()100%22100%
File 12: SetPosition2d(...)100%22100%
File 12: get_Position3d()100%11100%
File 12: set_Position3d(...)100%22100%
File 12: get_HeightPos()100%11100%
File 12: set_HeightPos(...)100%22100%
File 12: .cctor()100%11100%
File 12: get_VisualPosition()100%11100%
File 12: get_LastVisualPosition()100%11100%
File 12: get_RotationChangePending()100%22100%
File 12: get_Rotation()100%11100%
File 12: set_Rotation(...)100%22100%
File 12: get_Forward()100%11100%
File 12: get_Up()100%11100%
File 12: get_Right()100%11100%
File 12: get_VisualRotation()100%11100%
File 12: get_LastVisualRotation()100%11100%
File 12: get_IsRotationFullyFrozen()100%11100%
File 12: get_SettingVisuals()100%11100%
File 12: get_LinearVelocity()100%11100%
File 12: get_AngularVelocity()100%11100%
File 12: GetConstrainedInverseMass(...)100%88100%
File 12: ApplyConstrainedInverseInertia(...)100%44100%
File 12: GetConstrainedInverseInertiaTensor()100%11100%
File 12: get_GravityScale()100%11100%
File 12: set_GravityScale(...)100%11100%
File 12: get_IsAtRest()100%22100%
File 12: get_IsSleeping()100%11100%
File 12: get_SleepEnabled()100%11100%
File 12: set_SleepEnabled(...)100%44100%
File 12: get_SleepFrameThreshold()100%11100%
File 12: set_SleepFrameThreshold(...)100%11100%
File 12: get_SleepLinearSpeedThreshold()100%11100%
File 12: set_SleepLinearSpeedThreshold(...)100%11100%
File 12: get_SleepAngularSpeedThreshold()100%11100%
File 12: set_SleepAngularSpeedThreshold(...)100%11100%
File 12: get_IsAwakeForCollision()100%22100%
File 12: get_LinearSpeed()100%11100%
File 12: get_LinearAcceleration()100%11100%
File 12: get_AngularSpeed()100%11100%
File 12: get_AngularAcceleration()100%11100%
File 12: get_Weight()100%11100%
File 12: get_World()100%11100%
File 12: Initialize(...)100%66100%
File 12: LateSimulate()100%11100%
File 12: LateSimulate(...)100%2424100%
File 12: UpdateKinematicPositionAndRotation()100%1212100%
File 12: Sleep()100%44100%
File 12: Wake()100%22100%
File 12: WakeFromCollision()100%22100%
File 12: get_CanSleep()100%22100%
File 12: SetTransformWorldPose(...)100%22100%
File 12: Deactivate()100%22100%
File 12: UpdateRotation(...)100%22100%
File 12: GetWorldPoint(...)100%11100%
File 12: TryGetWorldPoint(...)100%22100%
File 12: GetLocalPoint(...)100%11100%
File 12: TryGetLocalPoint(...)100%22100%
File 12: ResetPosition(...)100%22100%
File 13: .ctor(...)100%11100%
File 13: get_GroundProbeMode()100%11100%
File 13: set_GroundProbeMode(...)100%44100%
File 13: get_GroundProbeRadius()100%11100%
File 13: set_GroundProbeRadius(...)100%44100%
File 13: get_GroundMinNormalDot()100%11100%
File 13: set_GroundMinNormalDot(...)100%44100%
File 13: get_GroundNormal()100%11100%
File 13: get_HitPlatform()100%11100%
File 13: get_HasHitPoint()100%11100%
File 13: get_HitPoint()100%44100%
File 13: TryGetHitPoint(...)100%11100%
File 13: get_IsGrounded()100%11100%
File 13: set_IsGrounded(...)100%44100%
File 13: get_WasGrounded()100%11100%
File 13: get_LastGroundedPosition()100%11100%
File 13: SkipGrounding(...)100%11100%
File 13: UseManualGrounding(...)100%22100%
File 13: UseAutomaticGrounding(...)100%44100%
File 13: SetManualGrounding(...)100%11100%
File 13: ClearManualGrounding()100%11100%
File 13: SkipGroundingCoroutine()100%11100%
File 13: CheckGround()100%11100%
File 13: ValidateGroundMinNormalDot(...)100%22100%
File 13: CheckGroundForSimulation()100%11100%
File 13: get_HasCurrentGroundSupport()100%44100%
File 13: WakeForContactChange()100%44100%
File 13: RefreshAutomaticGroundSupport()100%1616100%
File 13: HasCurrentGroundSupportDependency()100%1212100%
File 13: CaptureGroundedTransitionState()100%11100%
File 13: CaptureGroundedStepState()100%22100%
File 13: CompleteGroundedStepState()100%11100%
File 13: CheckGround(...)100%3636100%
File 13: TryFindGroundHit(...)100%66100%
File 13: TryFindGroundHitWithRay(...)100%44100%
File 13: TryFindGroundHitWithSweptSphere(...)100%66100%
File 13: IsValidGroundHit(...)100%66100%
File 13: IsValidGroundCollider(...)100%88100%
File 13: ResolveGroundProbeMode()100%1616100%
File 13: ResolveGroundProbeRadius()100%1212100%
File 13: ClearGrounding()100%44100%
File 13: ApplyGroundedHeightOrReset()100%44100%
File 13: SetGroundingState(...)100%22100%
File 13: SetGroundingState(...)100%1010100%
File 13: RefreshGroundNormalForce()100%44100%
File 13: ResetGroundCalculations()100%11100%
File 14: get_CanUseAngularInertia()100%22100%
File 14: get_Mass()100%11100%
File 14: set_Mass(...)100%44100%
File 14: InvalidateMassDependentSolverState()100%11100%
File 14: get_InverseMass()100%22100%
File 14: get_InverseInertiaTensor()100%11100%
File 14: get_CanTranslate()100%88100%
File 14: get_CanRotate()100%88100%
File 14: get_HasSolverMobility()100%22100%
File 14: get_EffectiveInverseMass()100%22100%
File 14: get_EffectiveInverseInertiaTensor()100%22100%
File 14: RefreshMassPropertiesFromColliderShape()100%22100%
File 14: UpdateInertiaTensorOrientation()100%11100%
File 14: CalculateInertiaTensorOrientation(...)100%44100%
File 14: RefreshInertiaTensor(...)100%44100%
File 15: ProjectLinearMotion(...)100%1010100%
File 15: ProjectLinearEndpoint(...)100%66100%
File 15: ProjectAngularMotion(...)100%1010100%
File 15: UpdateSleepState()100%22100%
File 15: get_CanCountTowardSleep()100%88100%
File 15: get_ReadyForAutomaticSleep()100%22100%
File 15: AdvanceSleepCounter()100%66100%
File 15: ClearMotionForSleep()100%11100%
File 15: ApplyFreezeConstraintsToMotion()100%11100%
File 15: RefreshPartitionAwakeState()100%22100%
File 15: RefreshPartitionMobility()100%66100%
File 15: AddTorque(...)100%22100%
File 15: AddForce(...)100%22100%
File 15: AddLinearImpulse(...)100%22100%
File 15: AddAngularImpulse(...)100%22100%
File 15: ApplyCollisionLinearVelocityDelta(...)100%44100%
File 15: ApplyCollisionAngularVelocityDelta(...)100%44100%
File 15: CanApplyCollisionVelocityDeltas(...)100%11100%
File 15: TryPrepareCollisionVelocityState(...)100%44100%
File 15: ApplyCollisionVelocityState(...)100%88100%
File 15: PrepareCollisionPositionCorrection(...)100%22100%
File 15: ApplyCollisionPositionState(...)100%22100%
File 15: PublishCollisionPositionState()100%44100%
File 15: AddPositionDisplacement(...)100%22100%
File 15: SetPosition(...)100%22100%
File 15: SetHeight(...)100%22100%
File 15: SetRotation(...)100%22100%
File 15: PrepareExplicitBodyPose(...)100%22100%
File 15: PublishExplicitBodyPose(...)100%22100%
File 15: PreflightStaticPoseChange()100%44100%
File 15: PreflightResetPoseChange()100%44100%
File 15: ProcessMovable()100%11100%
File 15: ApplyLinearForces()100%11100%
File 15: ApplyDrag(...)100%1010100%
File 15: ApplyGroundLinearFriction()100%66100%
File 15: UpdateLinearVelocity()100%44100%
File 15: RefreshLinearMotionState(...)100%44100%
File 15: ApplyAngularTorques()100%11100%
File 15: ApplyGroundAngularFriction(...)100%1010100%
File 15: ApplyAngularDamping(...)100%66100%
File 15: ApplyAngularDissipation(...)100%22100%
File 15: ApplyConstrainedAngularDissipation(...)100%44100%
File 15: ResolveConstrainedDissipativeVelocity(...)100%22100%
File 15: ApplyBoundedDissipation(...)100%44100%
File 15: ResolveDissipativeVelocity(...)100%66100%
File 15: UpdateAngularVelocity()100%11100%
File 15: ResolveAngularVelocityForStep(...)100%44100%
File 15: RefreshAngularMotionState(...)100%44100%
File 15: ResolveLimitedVelocity(...)100%44100%
File 15: ResolveGroundFrictionImpulse(...)100%22100%
File 15: NonKinematicUpdate(...)100%1010100%
File 15: PositionBasedOnForce()100%11100%
File 15: ApplyResolvedPosition(...)100%66100%
File 15: RotationBasedOnTorque(...)100%11100%
File 15: ApplyGyroscopicPrecession(...)100%44100%
File 16: ContributeReplayHash(...)100%88100%
File 16: WriteContinuousCollisionTrajectory(...)100%22100%
File 17: RecordData(...)100%1818100%
File 17: ApplyLoadedState()100%44100%
File 18: OnVisualize()100%1616100%
File 18: ResolveVisualRotationStep()100%22100%
File 18: SetPositionTransformWorldPosition(...)100%11100%
File 18: SetRotationTransformWorldRotation(...)100%11100%
File 18: PublishAuthoritativePose()100%11100%
File 18: CheckChangedValues()100%44100%
File 18: SetVisualPosition(...)100%11100%
File 18: SetVisualRotation(...)100%11100%
File 18: StoreVisualRotation(...)100%11100%

File(s)

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.CenterOfMass.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody.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 FixedMathSharp;
 10using Gravitas.CollisionHandling;
 11
 12namespace Gravitas;
 13
 14/// <content>Maintains the spatial center-of-mass offset and resolves it in world space.</content>
 15public partial class SolidBody
 16{
 17    private Vector3d _localCenterOfMassOffset;
 18    private bool _centerOfMassOffsetExplicit;
 19
 20    /// <summary>
 21    /// Gets or sets the authoritative body-local center-of-mass offset used by response and inertia.
 22    /// </summary>
 23    public Vector3d LocalCenterOfMassOffset
 24    {
 1733425        get => _localCenterOfMassOffset;
 26        set
 27        {
 2528            if (_localCenterOfMassOffset == value && _centerOfMassOffsetExplicit)
 129                return;
 30
 2431            _localCenterOfMassOffset = value;
 2432            _centerOfMassOffsetExplicit = true;
 2433            if (!Active)
 134                return;
 35
 2336            Wake();
 2337            RefreshInertiaTensor(Mass);
 2338        }
 39    }
 40
 41    /// <summary>
 42    /// Gets the authoritative world-space center of mass.
 43    /// </summary>
 44    /// <exception cref="InvalidOperationException">
 45    /// The center lies outside the representable coordinate domain. Use
 46    /// <see cref="TryGetWorldCenterOfMass(out Vector3d)"/> when that is possible.
 47    /// </exception>
 48    public Vector3d WorldCenterOfMass
 49    {
 50        get
 51        {
 15052            bool succeeded = TryGetWorldCenterOfMass(out Vector3d center);
 15053            ChronicleGuard.ThrowIfTrue(
 15054                !succeeded,
 15055                nameof(WorldCenterOfMass),
 15056                "The world center of mass is outside the representable coordinate domain.");
 14957            return center;
 58        }
 59    }
 60
 61    /// <summary>
 62    /// Attempts to materialize the authoritative world-space center of mass
 63    /// without saturation.
 64    /// </summary>
 65    public bool TryGetWorldCenterOfMass(out Vector3d center)
 15366        => Rotation.TryTransformPoint(
 15367            Position3d,
 15368            _localCenterOfMassOffset,
 15369            out center);
 70
 71    internal bool TryGetOffsetFromCenterOfMass(
 72        ContactAnchor anchor,
 73        out Vector3d offset)
 27860874        => anchor.TryGetOffsetFrom(GetCenterOfMassAnchor(), out offset);
 75
 76    [System.Runtime.CompilerServices.MethodImpl(
 77        System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
 78    internal ContactAnchor GetCenterOfMassAnchor() =>
 34220579        new(
 34220580            Position3d,
 34220581            Rotation,
 34220582            _localCenterOfMassOffset);
 83
 84    /// <summary>
 85    /// Clears an explicit center-of-mass override and derives the offset from the bound collider again.
 86    /// </summary>
 87    public void ResetCenterOfMassFromCollider()
 88    {
 189        _centerOfMassOffsetExplicit = false;
 190        RefreshMassPropertiesFromColliderShape();
 191    }
 92}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.ColliderReconfiguration.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody.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 exact, transactional 3D collider reconfiguration.</content>
 17public partial class SolidBody
 18{
 19    /// <summary>
 20    /// Attempts to replace this registered body's primitive geometry and root
 21    /// pose without replacing its collider, body, or host identity.
 22    /// </summary>
 23    /// <remarks>
 24    /// Sphere, capsule, and finite-cylinder definitions are supported. The
 25    /// definition must match the collider's existing family. Material, filters,
 26    /// hierarchy, events, rotation, motion, and runtime identity are retained.
 27    /// Exact touching is accepted; any physically eligible 3D collider with
 28    /// positive penetration, even below one raw unit, rejects the transaction.
 29    /// <paramref name="publishSynchronizedState"/> runs exactly once after an
 30    /// applied physical state is committed, or after an unchanged state is
 31    /// confirmed, and before separation callbacks. It does not run for a blocked
 32    /// transaction. Publisher or separation-callback failures do not roll back
 33    /// an accepted transaction; pair retirement continues, then one failure is
 34    /// returned directly or several are aggregated through
 35    /// <paramref name="notificationException"/>. This keeps the transaction
 36    /// result distinct from post-commit host notification failure while allowing
 37    /// a coordinating caller to prevent observers from seeing split state.
 38    /// </remarks>
 39    /// <param name="definition">The replacement geometry definition.</param>
 40    /// <param name="localOffset">The replacement unscaled collider offset.</param>
 41    /// <param name="position">The replacement authoritative body-root position.</param>
 42    /// <param name="blocker">The first stable-order collider with positive candidate penetration.</param>
 43    /// <param name="notificationException">
 44    /// A synchronized-state publisher or separation-callback failure observed
 45    /// after an accepted transaction, or <see langword="null"/>. One failure is
 46    /// returned directly; multiple failures are combined in stable order as an
 47    /// <see cref="AggregateException"/>.
 48    /// </param>
 49    /// <param name="publishSynchronizedState">
 50    /// Optional dependent-state publisher invoked after the physical result is
 51    /// committed or confirmed unchanged and before separation callbacks.
 52    /// </param>
 53    /// <returns>The exact transaction result.</returns>
 54    /// <exception cref="ArgumentException">
 55    /// The definition is default, unsupported, or does not match the existing
 56    /// collider family.
 57    /// </exception>
 58    /// <exception cref="InvalidOperationException">
 59    /// The body is not registered, the fixed-step transaction is active, or the
 60    /// candidate geometry or host pose is not representable.
 61    /// </exception>
 62    public ColliderReconfigurationStatus TryReconfigureCollider(
 63        ColliderShapeDefinition definition,
 64        Vector3d localOffset,
 65        Vector3d position,
 66        out LSCollider? blocker,
 67        out Exception? notificationException,
 68        Action? publishSynchronizedState = null)
 69    {
 20270        blocker = null;
 20271        notificationException = null;
 20272        definition.EnsureDefined();
 20173        ThrowIfRuntimeRegistrationMissing();
 20074        Context.ThrowIfFixedStepMutationNotAllowed();
 19975        ColliderType expectedShape = GetSupportedRuntimeShape(definition.Kind);
 19976        ChronicleGuard.ThrowIfArgument(
 19977            expectedShape == ColliderType.None,
 19978            nameof(definition),
 19979            "Collider reconfiguration supports only sphere, capsule, and finite-cylinder definitions.");
 19880        ChronicleGuard.ThrowIfArgument(
 19881            Collider.Shape != expectedShape,
 19882            nameof(definition),
 19883            "Collider reconfiguration cannot replace the existing collider family.");
 19784        ChronicleGuard.ThrowIfTrue(
 19785            !CanPublishRootPosition(position),
 19786            nameof(position),
 19787            "The host transform cannot represent the requested body-root position.");
 88
 19689        LSCollider candidate = definition.CreateRuntimeCollider();
 19690        candidate.PrepareDetachedBodyReconfigurationCandidate(
 19691            this,
 19692            localOffset,
 19693            position,
 19694            Rotation);
 95
 19596        if (Position3d == position
 19597            && Collider.LocalOffset == localOffset
 19598            && Collider.Radius == definition.Radius
 19599            && Collider.Size == definition.Size
 195100            && Collider.MatchesCommittedReconfigurationCandidate(candidate))
 101        {
 1102            SwiftList<Exception>? notificationExceptions = null;
 1103            CaptureSynchronizedStatePublication(
 1104                publishSynchronizedState,
 1105                ref notificationExceptions);
 1106            notificationException = CollisionNotificationExceptions.ToException(
 1107                notificationExceptions);
 1108            return ColliderReconfigurationStatus.Unchanged;
 109        }
 110
 194111        if (TryFindColliderReconfigurationBlocker(candidate, out blocker))
 64112            return ColliderReconfigurationStatus.Blocked;
 113
 130114        Collider.PrepareBodyReconfiguration(
 130115            definition,
 130116            localOffset,
 130117            position,
 130118            Rotation);
 130119        notificationException = CommitColliderReconfiguration(
 130120            definition,
 130121            localOffset,
 130122            position,
 130123            publishSynchronizedState);
 130124        return ColliderReconfigurationStatus.Applied;
 125    }
 126
 127    private bool TryFindColliderReconfigurationBlocker(
 128        LSCollider candidate,
 129        out LSCollider? blocker)
 130    {
 194131        int count = Context.Physics.ColliderCount;
 1034132        for (int i = 0; i < count; i++)
 133        {
 387134            LSCollider other = Context.Physics.GetColliderByServiceIndex(i);
 387135            if (ReferenceEquals(other, Collider)
 387136                || other.IsTrigger
 387137                || !Context.Physics.RequireCollisionPair(Collider, other)
 387138                || !candidate.Bounds.Intersects(other.Bounds))
 139            {
 140                continue;
 141            }
 142
 127143            LSCollider first = candidate;
 127144            LSCollider second = other;
 127145            if (second.Priority > first.Priority)
 102146                (first, second) = (second, first);
 147
 127148            CollisionType collisionType = ColliderSettings.GetCollisionType(
 127149                first.Shape,
 127150                second.Shape);
 127151            if (collisionType == CollisionType.None)
 152                continue;
 153
 127154            if (!CollisionDetection.DoesPostureCandidatePenetrate(candidate, other))
 155            {
 156                continue;
 157            }
 158
 64159            blocker = other;
 64160            return true;
 161        }
 162
 130163        blocker = null;
 130164        return false;
 165    }
 166
 167    private Exception? CommitColliderReconfiguration(
 168        ColliderShapeDefinition definition,
 169        Vector3d localOffset,
 170        Vector3d position,
 171        Action? publishSynchronizedState)
 172    {
 130173        Context.Constraints3D.ClearSolverCachesForBody(this);
 130174        InvalidateContinuousCollisionTrajectory();
 175
 130176        Position3d = position;
 130177        SetPositionTransformWorldPosition(position);
 130178        Collider.PublishPreparedBodyReconfiguration(definition, localOffset);
 130179        Collider.Simulate();
 130180        if (Context.Settings.RuntimeMode.RunsMixedContacts())
 2181            Context.MixedCollisions.Refresh3DColliderPartition(Collider);
 130182        Wake();
 183
 130184        var registration = new ColliderLifetimeToken(Collider);
 130185        SwiftList<Exception>? notificationExceptions = null;
 130186        CaptureSynchronizedStatePublication(
 130187            publishSynchronizedState,
 130188            ref notificationExceptions);
 130189        if (registration.IsCurrentLifetime)
 129190            Context.Physics.InvalidatePairsForColliderReconfiguration(
 129191                Collider,
 129192                ref notificationExceptions);
 130193        if (registration.IsCurrentLifetime)
 128194            Context.MixedCollisions.InvalidatePairsFor3DColliderReconfiguration(
 128195                Collider,
 128196                ref notificationExceptions);
 130197        return CollisionNotificationExceptions.ToException(notificationExceptions);
 198    }
 199
 200    private static void CaptureSynchronizedStatePublication(
 201        Action? publishSynchronizedState,
 202        ref SwiftList<Exception>? notificationExceptions)
 203    {
 131204        if (publishSynchronizedState == null)
 20205            return;
 206
 207        try
 208        {
 111209            publishSynchronizedState();
 110210        }
 1211        catch (Exception exception)
 212        {
 1213            CollisionNotificationExceptions.Capture(
 1214                ref notificationExceptions,
 1215                exception);
 1216        }
 111217    }
 218
 219    private bool CanPublishRootPosition(Vector3d position)
 220    {
 197221        FixedTransform? parent = PositionTransform.Parent;
 197222        if (parent == null)
 195223            return true;
 224
 2225        return parent.TryInverseTransformPoint(position, out Vector3d localPosition)
 2226            && parent.TryTransformPoint(localPosition, out Vector3d roundTrip)
 2227            && roundTrip.FuzzyEqualAbsolute(position, Fixed64.Epsilon);
 228    }
 229
 230    private static ColliderType GetSupportedRuntimeShape(
 231        ColliderShapeDefinitionKind kind) =>
 199232        kind switch
 199233        {
 69234            ColliderShapeDefinitionKind.Sphere => ColliderType.Sphere,
 68235            ColliderShapeDefinitionKind.Capsule => ColliderType.Capsule,
 61236            ColliderShapeDefinitionKind.Cylinder => ColliderType.Cylinder,
 1237            _ => ColliderType.None
 199238        };
 239}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.ContinuousCollision.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody.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;
 12using SwiftCollections.Query;
 13
 14namespace Gravitas;
 15
 16/// <content>Tracks spatial motion segments and samples the body trajectory for continuous collision detection.</content
 17public partial class SolidBody
 18{
 19    private LSCollider? _continuousCollisionHandoffIgnoredCollider3D;
 20    private LSCollider2D? _continuousCollisionHandoffIgnoredCollider2D;
 21
 22    internal Vector3d ContinuousCollisionFrameStart
 23    {
 24        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 7440225        get => ResolveLegacyContinuousCollisionFrameStart();
 26    }
 27
 28    internal Vector3d ContinuousCollisionFrameDisplacement
 29    {
 30        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 731        get => ResolveLegacyContinuousCollisionFrameDisplacement();
 32    }
 33
 34    internal FixedQuaternion ContinuousCollisionFrameRotation
 35    {
 36        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 737        get => _continuousCollisionTrajectory[0].StartRotation;
 38    }
 39
 40    internal Vector3d ContinuousCollisionFrameEnd
 41    {
 42        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 8092243        get => _continuousCollisionTrajectory[_continuousCollisionTrajectory.Count - 1].EndPosition;
 44    }
 45
 46    internal FixedQuaternion ContinuousCollisionFrameTargetRotation
 47    {
 48        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 656849        get => _continuousCollisionTrajectory[_continuousCollisionTrajectory.Count - 1].TargetRotation;
 50    }
 51
 52    internal int ContinuousCollisionTrajectoryCount =>
 43353        _continuousCollisionTrajectory.Count;
 54
 55    internal ContinuousCollisionMotionSegment3D GetContinuousCollisionTrajectorySegment(
 56        int index) =>
 25757        _continuousCollisionTrajectory[index];
 58
 59    internal bool HasContinuousCollisionMotion
 60    {
 61        get
 62        {
 1360863            for (int i = 0; i < _continuousCollisionTrajectory.Count; i++)
 64            {
 675965                ContinuousCollisionMotionSegment3D segment =
 675966                    _continuousCollisionTrajectory[i];
 675967                if (segment.Displacement.MagnitudeSquared > Fixed64.Epsilon
 675968                    || segment.AngularDistance > Fixed64.Epsilon)
 69                {
 671470                    return true;
 71                }
 72            }
 73
 4574            return false;
 75        }
 76    }
 77
 78    internal bool ShouldOwnContinuousCollisionMovingPair(bool otherHasRotationalMotion)
 79    {
 220580        if (!IsKinematic || !ShouldUseContinuousCollision(out ContinuousCollisionMode mode))
 200281            return false;
 82
 20383        EnsureContinuousCollisionFramePrepared(Context.LateSimulateToken);
 20384        if (!HasContinuousCollisionMotion)
 685            return false;
 19786        if (mode == ContinuousCollisionMode.Continuous)
 19287            return true;
 88
 589        Fixed64 proxyRadius = ResolveContinuousCollisionProxyRadius();
 590        Vector3d displacement = ContinuousCollisionFrameDisplacement;
 91
 592        Fixed64 angularDistance = ResolveKinematicAngularDistanceRadians(
 593            ContinuousCollisionFrameRotation,
 594            ContinuousCollisionFrameTargetRotation);
 595        bool requiresTargetSampling = angularDistance <= Fixed64.Epsilon
 596            && otherHasRotationalMotion;
 597        return requiresTargetSampling
 598            | angularDistance * proxyRadius > proxyRadius
 599            | !ContinuousCollisionMath.IsWithinProxyRadius(
 5100                displacement,
 5101                displacement.MagnitudeSquared,
 5102                proxyRadius);
 103    }
 104
 105    internal bool HasContinuousCollisionRotationalMotion
 106    {
 107        get
 108        {
 9888109            for (int i = 0; i < _continuousCollisionTrajectory.Count; i++)
 110            {
 2807111                if (_continuousCollisionTrajectory[i].AngularDistance > Fixed64.Epsilon)
 124112                    return true;
 113            }
 114
 2137115            return false;
 116        }
 117    }
 118
 119    internal Vector3d SampleContinuousCollisionPosition(Fixed64 frameFraction) =>
 7289120        _continuousCollisionTrajectory.Count == 0
 7289121            ? Position3d
 7289122            : ResolveContinuousCollisionSegment(frameFraction).SamplePosition(frameFraction);
 123
 124    internal FixedQuaternion SampleContinuousCollisionRotation(Fixed64 frameFraction) =>
 8030125        _continuousCollisionTrajectory.Count == 0
 8030126            ? Rotation
 8030127            : ResolveContinuousCollisionSegment(frameFraction).SampleRotation(
 8030128                frameFraction,
 8030129                Context.DeltaTime);
 130
 131    internal Vector3d SampleContinuousCollisionLinearVelocity(Fixed64 frameFraction)
 132    {
 2179133        if (_continuousCollisionTrajectory.Count == 0)
 32134            return ProjectLinearMotion(_linearVelocity);
 135
 2147136        ContinuousCollisionMotionSegment3D segment =
 2147137            ResolveContinuousCollisionSegment(frameFraction);
 2147138        Fixed64 duration = (segment.EndFraction - segment.StartFraction) * Context.DeltaTime;
 2147139        return ProjectLinearMotion(segment.Displacement / duration);
 140    }
 141
 142    internal Vector3d SampleContinuousCollisionAngularVelocity(Fixed64 frameFraction) =>
 2463143        _continuousCollisionTrajectory.Count == 0
 2463144            ? _angularVelocity
 2463145            : ResolveContinuousCollisionSegment(frameFraction).AngularVelocity;
 146
 147    private Vector3d ResolveLegacyContinuousCollisionFrameStart()
 148    {
 74402149        ContinuousCollisionMotionSegment3D segment =
 74402150            _continuousCollisionTrajectory[_continuousCollisionTrajectory.Count - 1];
 74402151        Vector3d frameDisplacement = ResolveLegacyContinuousCollisionFrameDisplacement();
 74402152        return segment.StartPosition - frameDisplacement * segment.StartFraction;
 153    }
 154
 155    private Vector3d ResolveLegacyContinuousCollisionFrameDisplacement()
 156    {
 74409157        ContinuousCollisionMotionSegment3D segment =
 74409158            _continuousCollisionTrajectory[_continuousCollisionTrajectory.Count - 1];
 74409159        Fixed64 remainingFraction = Fixed64.One - segment.StartFraction;
 74409160        return segment.Displacement / remainingFraction;
 161    }
 162
 163    internal bool EnsureContinuousCollisionFramePrepared(long token, bool refreshGroundSupport = false)
 164    {
 221589165        if (_continuousCollisionFrameToken == token)
 147150166            return !refreshGroundSupport || RefreshAutomaticGroundSupport();
 167
 168        // Publish pending authoring before inertia integration and CCD indexing;
 169        // rotational samples must not introduce a different geometry epoch.
 74439170        Collider.RebuildRuntimeShapeOnly(geometryOnly: true);
 171        // Grounding queries can invoke host callbacks; only body/service entry points opt in.
 74439172        if (refreshGroundSupport && !RefreshAutomaticGroundSupport())
 2173            return false;
 174
 74437175        Vector3d startPosition = Position3d;
 74437176        FixedQuaternion startRotation = Rotation;
 177        Vector3d displacement;
 178        Vector3d endPosition;
 179        FixedQuaternion targetRotation;
 180        Vector3d angularVelocity;
 181        Fixed64 angularDistance;
 182        ContinuousCollisionRotationPath3D rotationPath;
 74437183        if (IsKinematic)
 184        {
 6560185            _continuousCollisionAngularVelocityStepStart = Vector3d.Zero;
 6560186            Vector3d targetPosition = ProjectLinearEndpoint(
 6560187                startPosition,
 6560188                _positionTransform.WorldPosition);
 6560189            endPosition = targetPosition;
 6560190            if (ShouldUseContinuousCollision(out _))
 191            {
 6482192                displacement = ContinuousCollisionSweepRange.ValidateEndpoint(
 6482193                    startPosition,
 6482194                    targetPosition,
 6482195                    out _);
 196            }
 197            else
 198            {
 78199                displacement = targetPosition - startPosition;
 200            }
 201
 6559202            targetRotation = _rotationTransform.WorldRotation.Normalized;
 6559203            angularVelocity = FixedQuaternion.ToAngularVelocity(
 6559204                targetRotation,
 6559205                startRotation,
 6559206                Context.DeltaTime);
 6559207            angularDistance = ResolveKinematicAngularDistanceRadians(
 6559208                startRotation,
 6559209                targetRotation);
 6559210            rotationPath = ContinuousCollisionRotationPath3D.KinematicSlerp;
 211        }
 212        else
 213        {
 67877214            _continuousCollisionAngularVelocityStepStart = _angularVelocity;
 67877215            if (!_isSleeping)
 216            {
 65394217                if (CanTranslate)
 218                {
 65361219                    ApplyLinearForces();
 65361220                    UpdateLinearVelocity();
 221                }
 222
 65394223                if (CanRotate)
 224                {
 23251225                    ApplyAngularTorques();
 23251226                    UpdateAngularVelocity();
 227                }
 228            }
 229
 67874230            displacement = ProjectLinearMotion(_linearVelocity) * Context.DeltaTime;
 67874231            endPosition = AddPositionDisplacement(startPosition, displacement);
 67872232            angularVelocity = _angularVelocity;
 67872233            targetRotation = IntegrateAngularRotation(
 67872234                startRotation,
 67872235                angularVelocity,
 67872236                Context.DeltaTime);
 67872237            angularDistance = angularVelocity.Magnitude * Context.DeltaTime;
 67872238            rotationPath = ContinuousCollisionRotationPath3D.IntegratedAngularVelocity;
 239        }
 240
 74431241        _continuousCollisionFrameToken = token;
 74431242        SetSingleContinuousCollisionSegment(new ContinuousCollisionMotionSegment3D(
 74431243            Fixed64.Zero,
 74431244            Fixed64.One,
 74431245            startPosition,
 74431246            endPosition,
 74431247            displacement,
 74431248            startRotation,
 74431249            angularVelocity,
 74431250            targetRotation,
 74431251            angularDistance,
 74431252            rotationPath));
 74431253        return true;
 254    }
 255
 256    private void InvalidateContinuousCollisionTrajectory()
 257    {
 16886258        _continuousCollisionFrameToken = -1;
 16886259        _continuousCollisionAngularVelocityStepStart = Vector3d.Zero;
 16886260        _continuousCollisionTrajectory.FastClear();
 16886261    }
 262
 263    private void SetSingleContinuousCollisionSegment(
 264        ContinuousCollisionMotionSegment3D segment)
 265    {
 74431266        _continuousCollisionTrajectory.FastClear();
 74431267        _continuousCollisionTrajectory.Add(segment);
 74431268    }
 269
 270    private void AppendContinuousCollisionSegment(
 271        Vector3d positionAtElapsedTime,
 272        Vector3d velocity,
 273        Fixed64 elapsedTime)
 274    {
 456275        Fixed64 startFraction = FixedMath.Clamp01(elapsedTime / Context.DeltaTime);
 456276        FixedQuaternion startRotation = SampleContinuousCollisionRotation(startFraction);
 456277        AppendContinuousCollisionSegment(
 456278            positionAtElapsedTime,
 456279            startRotation,
 456280            velocity,
 456281            _angularVelocity,
 456282            elapsedTime);
 456283    }
 284
 285    private void AppendContinuousCollisionSegment(
 286        Vector3d positionAtElapsedTime,
 287        FixedQuaternion rotationAtElapsedTime,
 288        Vector3d linearVelocity,
 289        Vector3d angularVelocity,
 290        Fixed64 elapsedTime)
 291    {
 1066292        Fixed64 deltaTime = Context.DeltaTime;
 1066293        Fixed64 startFraction = FixedMath.Clamp01(elapsedTime / deltaTime);
 1066294        Fixed64 remainingTime = FixedMath.Max(Fixed64.Zero, deltaTime - elapsedTime);
 1066295        Vector3d displacement = ProjectLinearMotion(linearVelocity) * remainingTime;
 1066296        FixedQuaternion targetRotation = IntegrateAngularRotation(
 1066297            rotationAtElapsedTime,
 1066298            angularVelocity,
 1066299            remainingTime);
 1066300        Vector3d endPosition = AddPositionDisplacement(positionAtElapsedTime, displacement);
 1066301        _continuousCollisionFrameToken = Context.LateSimulateToken;
 1066302        RemoveSupersededContinuousCollisionSegments(
 1066303            startFraction,
 1066304            positionAtElapsedTime,
 1066305            rotationAtElapsedTime);
 1066306        if (startFraction >= Fixed64.One && _continuousCollisionTrajectory.Count > 0)
 3307            return;
 308
 1063309        _continuousCollisionTrajectory.Add(new ContinuousCollisionMotionSegment3D(
 1063310            startFraction,
 1063311            Fixed64.One,
 1063312            positionAtElapsedTime,
 1063313            endPosition,
 1063314            displacement,
 1063315            rotationAtElapsedTime,
 1063316            angularVelocity,
 1063317            targetRotation,
 1063318            angularVelocity.Magnitude * remainingTime,
 1063319            ContinuousCollisionRotationPath3D.IntegratedAngularVelocity));
 1063320    }
 321
 322    // Step durations are at most one; keep the existing product rounding before checking the final coordinate.
 323    internal void ValidateContinuousCollisionPosition(Vector3d position, Vector3d velocity, Fixed64 remainingTime) =>
 302324        _ = AddPositionDisplacement(position, ResolveCommittedLinearVelocity(velocity) * remainingTime);
 325
 326    private Vector3d ResolveCommittedLinearVelocity(Vector3d velocity)
 327    {
 565328        velocity = ProjectLinearMotion(velocity);
 329        // An unchanged collision velocity skips refresh, including newly changed environment limits.
 565330        return velocity == _linearVelocity
 565331            ? velocity
 565332            : ResolveLimitedVelocity(velocity, velocity.Magnitude, out _);
 333    }
 334
 335    internal bool CanAppendContinuousCollisionSegment(Fixed64 elapsedTime)
 336    {
 1273337        Fixed64 startFraction = FixedMath.Clamp01(elapsedTime / Context.DeltaTime);
 1273338        int retainedCount = _continuousCollisionTrajectory.Count;
 1975339        while (retainedCount > 0
 1975340            && _continuousCollisionTrajectory[retainedCount - 1].StartFraction >= startFraction)
 341        {
 702342            retainedCount--;
 343        }
 344
 1273345        return retainedCount <= Context.Settings.ContinuousCollisionMaxToiIterations;
 346    }
 347
 348    internal bool TryResolveContinuousCollisionMotionBound(
 349        Fixed64 lowerFraction,
 350        Fixed64 upperFraction,
 351        Fixed64 pivotRadius,
 352        out Fixed64 motionBound)
 353    {
 6832354        motionBound = Fixed64.Zero;
 27408355        for (int i = 0; i < _continuousCollisionTrajectory.Count; i++)
 356        {
 6873357            ContinuousCollisionMotionSegment3D segment = _continuousCollisionTrajectory[i];
 6873358            Fixed64 lower = FixedMath.Max(lowerFraction, segment.StartFraction);
 6873359            Fixed64 upper = FixedMath.Min(upperFraction, segment.EndFraction);
 6873360            if (upper <= lower)
 361                continue;
 362
 6836363            if (!segment.TryResolveMotionBound(lower, upper, pivotRadius, out Fixed64 segmentBound)
 6836364                || !Fixed64.TryAdd(motionBound, segmentBound, out motionBound))
 365            {
 1366                motionBound = default;
 1367                return false;
 368            }
 369        }
 370
 6831371        return true;
 372    }
 373
 374    internal FixedBoundVolume ResolveContinuousCollisionTrajectoryBounds(Fixed64 radius)
 375    {
 888376        Vector3d min = Position3d;
 888377        Vector3d max = Position3d;
 4878378        for (int i = 0; i < _continuousCollisionTrajectory.Count; i++)
 379        {
 1551380            ContinuousCollisionMotionSegment3D segment = _continuousCollisionTrajectory[i];
 1551381            min = Vector3d.Min(min, Vector3d.Min(segment.StartPosition, segment.EndPosition));
 1551382            max = Vector3d.Max(max, Vector3d.Max(segment.StartPosition, segment.EndPosition));
 383        }
 384
 888385        Vector3d extents = Vector3d.One * radius;
 888386        return new FixedBoundVolume(min - extents, max + extents);
 387    }
 388
 389    private void RemoveSupersededContinuousCollisionSegments(
 390        Fixed64 startFraction,
 391        Vector3d positionAtElapsedTime,
 392        FixedQuaternion rotationAtElapsedTime)
 393    {
 1606394        while (_continuousCollisionTrajectory.Count > 0
 1606395            && _continuousCollisionTrajectory[_continuousCollisionTrajectory.Count - 1]
 1606396                .StartFraction >= startFraction)
 397        {
 540398            _continuousCollisionTrajectory.RemoveAt(
 540399                _continuousCollisionTrajectory.Count - 1);
 400        }
 401
 1066402        if (_continuousCollisionTrajectory.Count == 0)
 529403            return;
 404
 537405        int predecessorIndex = _continuousCollisionTrajectory.Count - 1;
 537406        ContinuousCollisionMotionSegment3D predecessor =
 537407            _continuousCollisionTrajectory[predecessorIndex];
 537408        if (predecessor.EndFraction <= startFraction)
 22409            return;
 410
 515411        Fixed64 localFraction = (startFraction - predecessor.StartFraction)
 515412            / (predecessor.EndFraction - predecessor.StartFraction);
 515413        _continuousCollisionTrajectory[predecessorIndex] =
 515414            new ContinuousCollisionMotionSegment3D(
 515415                predecessor.StartFraction,
 515416                startFraction,
 515417                predecessor.StartPosition,
 515418                positionAtElapsedTime,
 515419                positionAtElapsedTime - predecessor.StartPosition,
 515420                predecessor.StartRotation,
 515421                predecessor.AngularVelocity,
 515422                rotationAtElapsedTime,
 515423                predecessor.AngularDistance * localFraction,
 515424                predecessor.RotationPath);
 515425    }
 426
 427    private ContinuousCollisionMotionSegment3D ResolveContinuousCollisionSegment(
 428        Fixed64 frameFraction) =>
 19830429        _continuousCollisionTrajectory[
 19830430            GetContinuousCollisionTrajectorySegmentIndex(frameFraction)];
 431
 432    internal int GetContinuousCollisionTrajectorySegmentIndex(
 433        Fixed64 frameFraction)
 434    {
 39888435        for (int i = _continuousCollisionTrajectory.Count - 1; i > 0; i--)
 436        {
 307437            ContinuousCollisionMotionSegment3D segment =
 307438                _continuousCollisionTrajectory[i];
 307439            if (frameFraction >= segment.StartFraction)
 252440                return i;
 441        }
 442
 19637443        return 0;
 444    }
 445
 446    internal void GetContinuousCollisionTrajectorySegmentRange(
 447        Fixed64 startFraction,
 448        out int startIndex,
 449        out int endExclusive)
 450    {
 214451        startIndex = startFraction <= Fixed64.Zero
 214452            ? 0
 214453            : GetContinuousCollisionTrajectorySegmentIndex(startFraction);
 214454        endExclusive = _continuousCollisionTrajectory.Count;
 214455    }
 456}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.ContinuousCollision.Dynamic.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody.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 FixedMathSharp;
 9using FixedMathSharp.Geometry;
 10using Gravitas.Colliders;
 11using Gravitas.CollisionHandling;
 12using Gravitas.Materials;
 13
 14namespace Gravitas;
 15
 16/// <content>Resolves dynamic spatial sweeps and applies spatial and mixed collision handoffs.</content>
 17public partial class SolidBody
 18{
 19    internal bool ApplyContinuousCollisionHandoff(
 20        Vector3d positionAtImpact,
 21        Vector3d velocityDelta,
 22        Fixed64 remainingTime,
 23        LSCollider? ignoredCollider3D = null,
 24        LSCollider2D? ignoredCollider2D = null)
 25    {
 4726        if (!Vector3d.TryAdd(_linearVelocity, ProjectLinearMotion(velocityDelta), out Vector3d postLinearVelocity))
 127            return false;
 28
 4629        return ApplyContinuousCollisionHandoff(
 4630            positionAtImpact,
 4631            Rotation,
 4632            postLinearVelocity,
 4633            _angularVelocity,
 4634            remainingTime,
 4635            ignoredCollider3D,
 4636            ignoredCollider2D);
 37    }
 38
 39    internal bool ApplyContinuousCollisionHandoff(
 40        Vector3d positionAtImpact,
 41        FixedQuaternion rotationAtImpact,
 42        Vector3d postLinearVelocity,
 43        Vector3d postAngularVelocity,
 44        Fixed64 remainingTime,
 45        LSCollider? ignoredCollider3D = null,
 46        LSCollider2D? ignoredCollider2D = null)
 47    {
 11448        if (!CanApplyContinuousCollisionHandoff(
 11449                positionAtImpact,
 11450                rotationAtImpact,
 11451                remainingTime,
 11452                out Vector3d resolvedPosition))
 353            return false;
 54
 11155        ValidateContinuousCollisionHandoffPosition(resolvedPosition, postLinearVelocity, postAngularVelocity, remainingT
 10756        _ = Context.Physics.TryReserveContinuousCollisionCandidateRefresh(this);
 57
 10758        ApplyContinuousCollisionHandoffReserved(
 10759            resolvedPosition,
 10760            rotationAtImpact,
 10761            postLinearVelocity,
 10762            postAngularVelocity,
 10763            remainingTime,
 10764            ignoredCollider3D,
 10765            ignoredCollider2D);
 10766        return true;
 67    }
 68
 69    internal void ApplyContinuousCollisionHandoffReserved(
 70        Vector3d resolvedPosition,
 71        FixedQuaternion rotationAtImpact,
 72        Vector3d postLinearVelocity,
 73        Vector3d postAngularVelocity,
 74        Fixed64 remainingTime,
 75        LSCollider? ignoredCollider3D = null,
 76        LSCollider2D? ignoredCollider2D = null)
 77    {
 25578        FixedQuaternion resolvedRotation = rotationAtImpact.Normalized;
 25579        Position3d = resolvedPosition;
 25580        Rotation = resolvedRotation;
 25581        Collider.PublishPreparedBodyPose();
 25582        UpdateInertiaTensorOrientation();
 25583        ApplyCollisionVelocityState(postLinearVelocity, postAngularVelocity);
 25584        bool hasRemainingMotion = HasRemainingHandoffMotion(_linearVelocity, _angularVelocity, remainingTime);
 25585        AppendContinuousCollisionSegment(
 25586            resolvedPosition,
 25587            Rotation,
 25588            hasRemainingMotion ? _linearVelocity : Vector3d.Zero,
 25589            hasRemainingMotion ? _angularVelocity : Vector3d.Zero,
 25590            Context.DeltaTime - remainingTime);
 25591        Context.Physics.RefreshContinuousCollisionCandidate(this);
 25592        if (!hasRemainingMotion)
 93        {
 2994            DiscardContinuousCollisionHandoff();
 2995            return;
 96        }
 97
 22698        _continuousCollisionHandoffToken = Context.LateSimulateToken;
 22699        _continuousCollisionHandoffRemainingTime = remainingTime;
 226100        _continuousCollisionHandoffIgnoredCollider3D = ignoredCollider3D;
 226101        _continuousCollisionHandoffIgnoredCollider2D = ignoredCollider2D;
 226102        _continuousCollisionHandoffPending = true;
 226103        Context.Physics.QueueContinuousCollisionHandoff(this);
 226104    }
 105
 106    internal void ValidateContinuousCollisionHandoffPosition(
 107        Vector3d position, Vector3d linearVelocity, Vector3d angularVelocity, Fixed64 remainingTime)
 108    {
 263109        linearVelocity = ResolveCommittedLinearVelocity(linearVelocity);
 263110        angularVelocity = ProjectAngularMotion(angularVelocity);
 263111        if (angularVelocity != _angularVelocity)
 53112            angularVelocity = ResolveLimitedVelocity(angularVelocity, angularVelocity.Magnitude, out _);
 263113        if (HasRemainingHandoffMotion(linearVelocity, angularVelocity, remainingTime))
 234114            _ = AddPositionDisplacement(position, linearVelocity * remainingTime);
 255115    }
 116
 117    private static bool HasRemainingHandoffMotion(Vector3d linearVelocity, Vector3d angularVelocity, Fixed64 remainingTi
 518118        remainingTime > Fixed64.Epsilon
 518119        && (linearVelocity.MagnitudeSquared > Fixed64.Epsilon || angularVelocity.MagnitudeSquared > Fixed64.Epsilon);
 120
 121    internal bool CanApplyContinuousCollisionHandoff(
 122        Vector3d positionAtImpact,
 123        FixedQuaternion rotationAtImpact,
 124        Fixed64 remainingTime,
 125        out Vector3d resolvedPosition)
 126    {
 281127        bool hasMobility = CanTranslate | CanRotate;
 281128        bool hasTrajectoryCapacity = CanAppendContinuousCollisionSegment(
 281129            Context.DeltaTime - remainingTime);
 281130        bool positionDeltaResolved = Vector3d.TrySubtract(
 281131            positionAtImpact,
 281132            Position3d,
 281133            out Vector3d positionDelta);
 281134        bool positionResolved = Vector3d.TryAdd(
 281135            Position3d,
 281136            ProjectLinearMotion(positionDelta),
 281137            out resolvedPosition);
 281138        if (!(hasMobility
 281139                & hasTrajectoryCapacity
 281140                & positionDeltaResolved
 281141                & positionResolved))
 142        {
 8143            return false;
 144        }
 145
 273146        return Collider.TryPrepareBodyPose(
 273147            resolvedPosition,
 273148            rotationAtImpact.Normalized);
 149    }
 150
 151    internal bool TryConsumeContinuousCollisionHandoff(bool updateSleepState, bool updateColliderState) =>
 74254152        TryConsumeContinuousCollisionHandoff(
 74254153            updateSleepState,
 74254154            updateColliderState,
 74254155            invokeMovementCallback: true,
 74254156            out _);
 157
 158    internal bool TryConsumeQueuedContinuousCollisionHandoff(
 159        bool updateSleepState,
 160        bool updateColliderState,
 161        out bool shouldNotifyMovement) =>
 92162        TryConsumeContinuousCollisionHandoff(
 92163            updateSleepState,
 92164            updateColliderState,
 92165            invokeMovementCallback: false,
 92166            out shouldNotifyMovement);
 167
 168    private bool TryConsumeContinuousCollisionHandoff(
 169        bool updateSleepState,
 170        bool updateColliderState,
 171        bool invokeMovementCallback,
 172        out bool shouldNotifyMovement)
 173    {
 74346174        shouldNotifyMovement = false;
 74346175        if (!_continuousCollisionHandoffPending || _continuousCollisionHandoffToken != Context.LateSimulateToken)
 74184176            return false;
 177
 162178        Fixed64 remainingTime = _continuousCollisionHandoffRemainingTime;
 162179        _continuousCollisionHandoffPending = false;
 162180        _continuousCollisionHandoffRemainingTime = Fixed64.Zero;
 162181        if ((!CanTranslate || _linearVelocity.MagnitudeSquared <= Fixed64.Epsilon)
 162182            && (!CanRotate || _angularVelocity.MagnitudeSquared <= Fixed64.Epsilon))
 183        {
 2184            _continuousCollisionHandoffIgnoredCollider3D = null;
 2185            _continuousCollisionHandoffIgnoredCollider2D = null;
 2186            if (updateColliderState)
 1187                Collider.Simulate();
 2188            return true;
 189        }
 190
 160191        Vector3d startPosition = Position3d;
 160192        Vector3d proposedPosition = AddPositionDisplacement(
 160193            startPosition, ProjectLinearMotion(_linearVelocity) * remainingTime);
 160194        FixedQuaternion startRotation = Rotation;
 160195        FixedQuaternion proposedRotation = IntegrateAngularRotation(
 160196            startRotation,
 160197            _angularVelocity,
 160198            remainingTime);
 160199        Fixed64 elapsedTime = FixedMath.Max(Fixed64.Zero, Context.DeltaTime - remainingTime);
 200        try
 201        {
 160202            if (!TryResolveRotationalContinuousCollision(
 160203                    startPosition,
 160204                    ref proposedPosition,
 160205                    startRotation,
 160206                    ref proposedRotation,
 160207                    remainingTime,
 160208                    elapsedTime,
 160209                    forceContinuous: true))
 210            {
 151211                TryResolveContinuousCollision(
 151212                    startPosition,
 151213                    ref proposedPosition,
 151214                    remainingTime,
 151215                    elapsedTime,
 151216                    forceContinuous: true);
 217            }
 160218        }
 219        finally
 220        {
 160221            _continuousCollisionHandoffIgnoredCollider3D = null;
 160222            _continuousCollisionHandoffIgnoredCollider2D = null;
 160223        }
 224
 160225        SetPosition2d(proposedPosition.ToVector2d());
 160226        HeightPos = proposedPosition.Y;
 160227        Rotation = proposedRotation;
 160228        UpdateInertiaTensorOrientation();
 229
 160230        CheckGroundForSimulation();
 160231        ApplyGroundedHeightOrReset();
 232
 160233        CheckChangedValues();
 234
 160235        if (updateColliderState)
 1236            Collider.Simulate();
 237        else
 159238            Collider.RebuildRuntimeShapeOnly(refreshMassProperties: false);
 239
 160240        if (updateSleepState)
 1241            UpdateSleepState();
 242
 160243        shouldNotifyMovement = PositionChangePending || RotationChangePending;
 160244        if (invokeMovementCallback && shouldNotifyMovement)
 72245            NotifyAuthoritativeMovement();
 246
 160247        return true;
 248    }
 249
 156250    internal void NotifyAuthoritativeMovement() => OnMoved?.Invoke();
 251
 252    internal void DiscardContinuousCollisionHandoff()
 253    {
 960254        _continuousCollisionHandoffPending = false;
 960255        _continuousCollisionHandoffRemainingTime = Fixed64.Zero;
 960256        _continuousCollisionHandoffIgnoredCollider3D = null;
 960257        _continuousCollisionHandoffIgnoredCollider2D = null;
 960258    }
 259
 260    private bool TryResolveContinuousCollision(Vector3d startPosition, ref Vector3d proposedPosition) =>
 65077261        TryResolveContinuousCollision(
 65077262            startPosition,
 65077263            ref proposedPosition,
 65077264            Context.DeltaTime,
 65077265            Fixed64.Zero,
 65077266            forceContinuous: false);
 267
 268    private bool TryResolveContinuousCollision(
 269        Vector3d startPosition,
 270        ref Vector3d proposedPosition,
 271        Fixed64 initialRemainingTime,
 272        Fixed64 initialElapsedTime,
 273        bool forceContinuous)
 274    {
 65228275        LastContinuousCollisionToiIterationCount = 0;
 65228276        LastContinuousCollisionToiIterationLimitReached = false;
 277
 65228278        ContinuousCollisionMode mode = ContinuousCollisionMode.Continuous;
 65228279        if (!forceContinuous && !ShouldUseContinuousCollision(out mode))
 64148280            return false;
 281
 1080282        Vector3d requestedDisplacement = _linearVelocity * initialRemainingTime;
 1080283        Vector3d displacement = ContinuousCollisionSweepRange.ValidateEndpoint(
 1080284            startPosition,
 1080285            proposedPosition,
 1080286            requestedDisplacement,
 1080287            out _);
 288
 1079289        Fixed64 displacementMagnitudeSquared = displacement.MagnitudeSquared;
 1079290        if (displacementMagnitudeSquared <= Fixed64.Epsilon)
 136291            return false;
 292
 943293        Fixed64 proxyRadius = ResolveContinuousCollisionProxyRadius();
 943294        if (proxyRadius <= Fixed64.Epsilon
 943295            || (mode == ContinuousCollisionMode.Auto
 943296                && ContinuousCollisionMath.IsWithinProxyRadius(displacement, displacementMagnitudeSquared, proxyRadius))
 297        {
 2298            return false;
 299        }
 300
 941301        bool resolved = false;
 941302        int conservativeMixedRefinementCount = 0;
 941303        Vector3d currentPosition = startPosition;
 941304        Fixed64 remainingTime = initialRemainingTime;
 941305        Fixed64 elapsedTime = initialElapsedTime;
 941306        int maxToiIterations = Context.Settings.ContinuousCollisionMaxToiIterations;
 941307        LSCollider? originalIgnoredCollider3D = _continuousCollisionHandoffIgnoredCollider3D;
 941308        LSCollider2D? originalIgnoredCollider2D = _continuousCollisionHandoffIgnoredCollider2D;
 309        try
 310        {
 1421311            while (LastContinuousCollisionToiIterationCount < maxToiIterations)
 312            {
 1421313                Vector3d requestedSegmentDisplacement = ProjectLinearMotion(_linearVelocity) * remainingTime;
 1421314                Vector3d requestedSegmentEnd = AddPositionDisplacement(currentPosition, requestedSegmentDisplacement);
 1421315                Vector3d segmentDisplacement = ContinuousCollisionSweepRange.ValidateEndpoint(
 1421316                    currentPosition,
 1421317                    requestedSegmentEnd,
 1421318                    requestedSegmentDisplacement,
 1421319                    out Fixed64 segmentLength);
 1421320                Vector3d segmentEnd = requestedSegmentEnd;
 321
 1421322                Fixed64 elapsedFraction = elapsedTime / Context.DeltaTime;
 1421323                if (!TryGetFirstContinuousCollisionHit(
 1421324                        currentPosition,
 1421325                        segmentEnd,
 1421326                        proxyRadius,
 1421327                        elapsedFraction,
 1421328                        out Vector3d hitNormal,
 1421329                        out Fixed64 hitDistance,
 1421330                        out ContinuousCollisionTargetKind targetKind,
 1421331                        out LSCollider? target3D,
 1421332                        out LSCollider2D? target2D))
 333                {
 774334                    currentPosition = segmentEnd;
 774335                    break;
 336                }
 337
 647338                Fixed64 hitTime = FixedMath.Clamp01(hitDistance / segmentLength);
 647339                currentPosition = new FixedSegment(currentPosition, segmentEnd)
 647340                    .GetPointAtDistance(hitDistance, segmentLength);
 647341                Vector3d previousVelocity = _linearVelocity;
 647342                Fixed64 consumedTime = remainingTime * hitTime;
 647343                Fixed64 remainingAfterHit = remainingTime - consumedTime;
 647344                Fixed64 hitElapsedTime = elapsedTime + consumedTime;
 647345                if (targetKind
 647346                    == ContinuousCollisionTargetKind.UnresolvedMixed)
 347                {
 39348                    resolved = true;
 39349                    if (hitTime > Fixed64.Epsilon
 39350                        && conservativeMixedRefinementCount
 39351                            < ContinuousCollisionMath.RotationalIntervalMaxDepth)
 352                    {
 36353                        conservativeMixedRefinementCount++;
 36354                        remainingTime = remainingAfterHit;
 36355                        elapsedTime = hitElapsedTime;
 36356                        if (remainingTime > Fixed64.Epsilon)
 357                            continue;
 358                    }
 359
 3360                    LastContinuousCollisionToiIterationCount++;
 3361                    LastContinuousCollisionToiIterationLimitReached =
 3362                        remainingAfterHit > Fixed64.Epsilon;
 3363                    _ = Context.Physics
 3364                        .TryReserveContinuousCollisionCandidateRefresh(this);
 3365                    UpdateContinuousCollisionFrameTrajectory(
 3366                        currentPosition,
 3367                        Vector3d.Zero,
 3368                        hitElapsedTime);
 3369                    Context.Physics
 3370                        .RefreshContinuousCollisionCandidate(this);
 371
 3372                    if (LastContinuousCollisionToiIterationLimitReached)
 3373                        Context.Physics.ReportContinuousCollisionIterationLimit();
 374
 3375                    break;
 376                }
 377
 608378                if (!CanAppendContinuousCollisionSegment(hitElapsedTime))
 379                {
 3380                    ContinuousCollisionMotionSegment3D activeSegment =
 3381                        ResolveContinuousCollisionSegment(
 3382                            ResolveContinuousCollisionFrameFraction(hitElapsedTime));
 3383                    currentPosition = activeSegment.StartPosition;
 3384                    elapsedTime = activeSegment.StartFraction * Context.DeltaTime;
 3385                    RemoveClosingContinuousCollisionVelocity(hitNormal);
 3386                    UpdateContinuousCollisionFrameTrajectory(
 3387                        currentPosition,
 3388                        _linearVelocity,
 3389                        elapsedTime);
 3390                    Context.Physics.RefreshContinuousCollisionCandidate(this);
 3391                    LastContinuousCollisionToiIterationCount++;
 3392                    LastContinuousCollisionToiIterationLimitReached = true;
 3393                    Context.Physics.ReportContinuousCollisionIterationLimit();
 3394                    resolved = true;
 3395                    break;
 396                }
 397
 605398                bool appliedResponse = TryApplyContinuousCollisionDynamicResponse(
 605399                        hitNormal,
 605400                        targetKind,
 605401                        target3D,
 605402                        target2D,
 605403                        currentPosition,
 605404                        hitElapsedTime,
 605405                        remainingAfterHit);
 605406                if (!appliedResponse)
 407                {
 391408                    RemoveClosingContinuousCollisionVelocity(hitNormal);
 391409                    UpdateContinuousCollisionFrameTrajectory(currentPosition, _linearVelocity, hitElapsedTime);
 410                }
 214411                else if (targetKind == ContinuousCollisionTargetKind.Dynamic3D)
 412                {
 45413                    _continuousCollisionHandoffIgnoredCollider3D = target3D;
 45414                    _continuousCollisionHandoffIgnoredCollider2D = null;
 415                }
 169416                else if (targetKind == ContinuousCollisionTargetKind.Dynamic2D)
 417                {
 12418                    _continuousCollisionHandoffIgnoredCollider3D = null;
 12419                    _continuousCollisionHandoffIgnoredCollider2D = target2D;
 420                }
 421
 605422                LastContinuousCollisionToiIterationCount++;
 605423                resolved = true;
 424
 605425                remainingTime = remainingAfterHit;
 605426                elapsedTime = hitElapsedTime;
 605427                if (remainingTime <= Fixed64.Epsilon || _linearVelocity.MagnitudeSquared <= Fixed64.Epsilon)
 428                    break;
 429
 449430                if (LastContinuousCollisionToiIterationCount >= maxToiIterations)
 431                {
 5432                    LastContinuousCollisionToiIterationLimitReached = true;
 5433                    break;
 434                }
 435
 444436                if (hitTime <= Fixed64.Epsilon && previousVelocity == _linearVelocity)
 437                    break;
 438            }
 439
 941440            proposedPosition = currentPosition;
 941441            return resolved;
 442        }
 443        finally
 444        {
 941445            _continuousCollisionHandoffIgnoredCollider3D = originalIgnoredCollider3D;
 941446            _continuousCollisionHandoffIgnoredCollider2D = originalIgnoredCollider2D;
 941447        }
 941448    }
 449
 450    private bool TryApplyContinuousCollisionDynamicResponse(
 451        Vector3d normalForSource,
 452        ContinuousCollisionTargetKind targetKind,
 453        LSCollider? target3D,
 454        LSCollider2D? target2D,
 455        Vector3d sourcePositionAtImpact,
 456        Fixed64 hitElapsedTime,
 457        Fixed64 remainingTime)
 458    {
 605459        if (targetKind == ContinuousCollisionTargetKind.Static2D)
 161460            return TryApplyStaticMixedContinuousCollisionResponse(target2D!, sourcePositionAtImpact,
 161461                hitElapsedTime, remainingTime);
 462
 444463        if (targetKind == ContinuousCollisionTargetKind.Dynamic3D)
 464        {
 46465            SolidBody targetBody = target3D!.Body!;
 46466            Fixed64 frameFraction = ResolveContinuousCollisionFrameFraction(hitElapsedTime);
 46467            Vector3d targetPositionAtImpact = targetBody.SampleContinuousCollisionPosition(
 46468                frameFraction);
 46469            return TryApplyContinuousCollisionDynamicResponse(
 46470                targetBody,
 46471                normalForSource,
 46472                sourcePositionAtImpact,
 46473                targetPositionAtImpact,
 46474                hitElapsedTime,
 46475                remainingTime);
 476        }
 477
 398478        if (targetKind == ContinuousCollisionTargetKind.Dynamic2D)
 479        {
 12480            SolidBody2D targetBody = target2D!.Body!;
 12481            Fixed64 frameFraction = ResolveContinuousCollisionFrameFraction(hitElapsedTime);
 12482            Vector2d targetPositionAtImpact = targetBody.SampleContinuousCollisionPosition(
 12483                frameFraction);
 12484            return TryApplyContinuousCollisionMixed2DResponse(
 12485                targetBody,
 12486                normalForSource,
 12487                sourcePositionAtImpact,
 12488                targetPositionAtImpact,
 12489                hitElapsedTime,
 12490                remainingTime);
 491        }
 492
 386493        return false;
 494    }
 495
 496    internal bool TryApplyContinuousCollisionDynamicResponse(
 497        SolidBody target,
 498        Vector3d normalForSource,
 499        Vector3d sourcePositionAtImpact,
 500        Vector3d targetPositionAtImpact,
 501        Fixed64 hitElapsedTime,
 502        Fixed64 remainingTime)
 503    {
 56504        if (!ContinuousCollisionImpulsePolicy.TryResolveImpactNormal(normalForSource, out Vector3d normal))
 1505            return false;
 506
 55507        Fixed64 constrainedInverseMassA = GetConstrainedInverseMass(normal);
 55508        Fixed64 constrainedInverseMassB = target.GetConstrainedInverseMass(normal);
 55509        Fixed64 frameFraction = ResolveContinuousCollisionFrameFraction(hitElapsedTime);
 55510        Vector3d targetLinearVelocity = target.SampleContinuousCollisionLinearVelocity(frameFraction);
 55511        bool inverseMassResolved = Fixed64.TryAdd(
 55512            constrainedInverseMassA,
 55513            constrainedInverseMassB,
 55514            out Fixed64 inverseMass);
 55515        bool relativeVelocityResolved = Vector3d.TrySubtract(
 55516            _linearVelocity,
 55517            targetLinearVelocity,
 55518            out Vector3d relativeVelocity);
 55519        if (!(inverseMassResolved & relativeVelocityResolved))
 520        {
 1521            return false;
 522        }
 523
 54524        Fixed64 normalVelocity = Vector3d.Dot(relativeVelocity, normal);
 54525        if (normalVelocity >= -Fixed64.Epsilon)
 1526            return false;
 527
 53528        Fixed64 restitution = ResolveContinuousCollisionRestitution(target, -normalVelocity);
 53529        bool responseFactorResolved = Fixed64.TryAdd(
 53530            Fixed64.One,
 53531            restitution,
 53532            out Fixed64 responseFactor);
 53533        bool closingSpeedResolved = Fixed64.TrySubtract(
 53534            Fixed64.Zero,
 53535            normalVelocity,
 53536            out Fixed64 closingSpeed);
 53537        bool responseSpeedResolved = Fixed64.TryMultiplyDivide(
 53538            closingSpeed,
 53539            responseFactor,
 53540            Fixed64.One,
 53541            out Fixed64 responseSpeed);
 53542        if (!(responseFactorResolved & closingSpeedResolved & responseSpeedResolved))
 543        {
 1544            return false;
 545        }
 546
 52547        Vector3d sourceResponseNormal = ProjectLinearMotion(normal);
 52548        Vector3d targetResponseNormal = target.ProjectLinearMotion(-normal);
 52549        bool sourceDeltaResolved = ContinuousCollisionImpulsePolicy.TryResolveVelocityDelta(
 52550            sourceResponseNormal,
 52551            responseSpeed,
 52552            EffectiveInverseMass,
 52553            inverseMass,
 52554            out Vector3d sourceVelocityDelta);
 52555        bool targetDeltaResolved = ContinuousCollisionImpulsePolicy.TryResolveVelocityDelta(
 52556            targetResponseNormal,
 52557            responseSpeed,
 52558            target.EffectiveInverseMass,
 52559            inverseMass,
 52560            out Vector3d targetVelocityDelta);
 52561        if (!(sourceDeltaResolved & targetDeltaResolved))
 562        {
 1563            return false;
 564        }
 565
 51566        bool sourceVelocityResolved = Vector3d.TryAdd(
 51567            _linearVelocity,
 51568            sourceVelocityDelta,
 51569            out Vector3d sourcePostLinearVelocity);
 51570        bool sourceTrajectoryAvailable = CanAppendContinuousCollisionSegment(hitElapsedTime);
 51571        if (!(sourceVelocityResolved & sourceTrajectoryAvailable))
 572        {
 1573            return false;
 574        }
 575
 50576        Vector3d targetResolvedPosition = default;
 50577        Vector3d targetPostLinearVelocity = targetLinearVelocity;
 50578        FixedQuaternion targetRotationAtImpact = target.SampleContinuousCollisionRotation(frameFraction);
 50579        Vector3d targetAngularVelocity = target.SampleContinuousCollisionAngularVelocity(frameFraction);
 50580        bool targetStateAvailable = true;
 50581        if (target.CanTranslate)
 582        {
 46583            bool targetVelocityResolved = Vector3d.TryAdd(
 46584                targetLinearVelocity,
 46585                targetVelocityDelta,
 46586                out targetPostLinearVelocity);
 46587            bool targetTrajectoryAvailable = target.CanApplyContinuousCollisionHandoff(
 46588                targetPositionAtImpact,
 46589                targetRotationAtImpact,
 46590                remainingTime,
 46591                out targetResolvedPosition);
 46592            targetStateAvailable = targetVelocityResolved & targetTrajectoryAvailable;
 593        }
 594
 50595        if (!targetStateAvailable)
 596        {
 1597            return false;
 598        }
 599
 49600        ValidateContinuousCollisionPosition(sourcePositionAtImpact, sourcePostLinearVelocity, Context.DeltaTime - hitEla
 49601        if (target.CanTranslate)
 45602            target.ValidateContinuousCollisionHandoffPosition(targetResolvedPosition, targetPostLinearVelocity, targetAn
 603
 47604        _ = target.CanTranslate
 47605            ? Context.Physics.TryReserveContinuousCollisionCandidateRefresh(this, target)
 47606            : Context.Physics.TryReserveContinuousCollisionCandidateRefresh(this);
 607
 47608        ApplyCollisionVelocityState(sourcePostLinearVelocity, _angularVelocity);
 47609        UpdateContinuousCollisionFrameTrajectory(sourcePositionAtImpact, _linearVelocity, hitElapsedTime);
 47610        Context.Physics.RefreshContinuousCollisionCandidate(this);
 47611        if (target.CanTranslate)
 612        {
 43613            target.ApplyContinuousCollisionHandoffReserved(
 43614                targetResolvedPosition,
 43615                targetRotationAtImpact,
 43616                targetPostLinearVelocity,
 43617                targetAngularVelocity,
 43618                remainingTime,
 43619                ignoredCollider3D: Collider);
 620        }
 621
 47622        return true;
 623    }
 624
 625    internal bool TryApplyContinuousCollisionMixed2DResponse(
 626        SolidBody2D target,
 627        Vector3d normalForSource,
 628        Vector3d sourcePositionAtImpact,
 629        Vector2d targetPositionAtImpact,
 630        Fixed64 hitElapsedTime,
 631        Fixed64 remainingTime)
 632    {
 22633        if (Collider is LSCompoundCollider or LSMeshCollider || target.Collider is LSCompoundCollider2D)
 7634            return TryApplyMixedTranslationalManifoldResponse(target, sourcePositionAtImpact,
 7635                targetPositionAtImpact, hitElapsedTime, remainingTime);
 15636        bool normalResolved = ContinuousCollisionImpulsePolicy.TryResolveImpactNormal(
 15637            normalForSource,
 15638            out Vector3d normal);
 15639        Vector2d planarNormal = normal.ToVector2d();
 15640        bool targetParticipates =
 15641            planarNormal != Vector2d.Zero && target.CanTranslate;
 15642        Fixed64 constrainedInverseMassA = GetConstrainedInverseMass(normal);
 15643        Fixed64 constrainedInverseMassB = targetParticipates
 15644            ? target.GetConstrainedInverseMass(planarNormal)
 15645                * planarNormal.MagnitudeSquared
 15646            : Fixed64.Zero;
 15647        Fixed64 frameFraction = ResolveContinuousCollisionFrameFraction(hitElapsedTime);
 15648        Vector2d targetLinearVelocity = target.SampleContinuousCollisionLinearVelocity(frameFraction);
 15649        Vector3d targetVelocity = targetLinearVelocity
 15650            .ToVector3d(Fixed64.Zero);
 15651        bool inverseMassResolved = Fixed64.TryAdd(
 15652            constrainedInverseMassA,
 15653            constrainedInverseMassB,
 15654            out Fixed64 inverseMass);
 15655        bool relativeVelocityResolved = Vector3d.TrySubtract(
 15656            _linearVelocity,
 15657            targetVelocity,
 15658            out Vector3d relativeVelocity);
 15659        Fixed64 normalVelocity = Vector3d.Dot(relativeVelocity, normal);
 15660        Fixed64 restitution = ResolveContinuousCollisionRestitution(target, -normalVelocity);
 15661        bool responseFactorResolved = Fixed64.TryAdd(
 15662            Fixed64.One,
 15663            restitution,
 15664            out Fixed64 responseFactor);
 15665        bool closingSpeedResolved = Fixed64.TrySubtract(
 15666            Fixed64.Zero,
 15667            normalVelocity,
 15668            out Fixed64 closingSpeed);
 15669        bool responseSpeedResolved = Fixed64.TryMultiplyDivide(
 15670            closingSpeed,
 15671            responseFactor,
 15672            Fixed64.One,
 15673            out Fixed64 responseSpeed);
 15674        Vector3d sourceResponseNormal = ProjectLinearMotion(normal);
 15675        Vector2d targetResponseNormal = targetParticipates
 15676            ? target.ProjectLinearMotion(-planarNormal)
 15677            : Vector2d.Zero;
 15678        bool sourceDeltaResolved = ContinuousCollisionImpulsePolicy.TryResolveVelocityDelta(
 15679            sourceResponseNormal,
 15680            responseSpeed,
 15681            EffectiveInverseMass,
 15682            inverseMass,
 15683            out Vector3d sourceVelocityDelta);
 15684        Vector2d targetVelocityDelta = Vector2d.Zero;
 15685        bool targetDeltaResolved = !targetParticipates
 15686            || ContinuousCollisionImpulsePolicy.TryResolveVelocityDelta(
 15687                targetResponseNormal,
 15688                responseSpeed,
 15689                target.EffectiveInverseMass,
 15690                inverseMass,
 15691                out targetVelocityDelta);
 15692        bool sourceVelocityResolved = Vector3d.TryAdd(
 15693            _linearVelocity,
 15694            sourceVelocityDelta,
 15695            out Vector3d sourcePostLinearVelocity);
 15696        bool sourceTrajectoryAvailable = CanAppendContinuousCollisionSegment(hitElapsedTime);
 15697        Vector2d targetResolvedPosition = default;
 15698        Vector2d targetPostLinearVelocity = targetLinearVelocity;
 15699        Fixed64 targetRotationAtImpact = target.SampleContinuousCollisionRotation(frameFraction);
 15700        Fixed64 targetAngularVelocity = target.SampleContinuousCollisionAngularVelocity(frameFraction);
 15701        bool targetStateAvailable = true;
 15702        if (targetParticipates)
 703        {
 12704            bool targetVelocityResolved = Vector2d.TryAdd(
 12705                targetLinearVelocity,
 12706                targetVelocityDelta,
 12707                out targetPostLinearVelocity);
 12708            bool targetTrajectoryAvailable = target.CanApplyContinuousCollisionHandoffState(
 12709                targetPositionAtImpact,
 12710                targetRotationAtImpact,
 12711                remainingTime,
 12712                out targetResolvedPosition);
 12713            targetStateAvailable = targetVelocityResolved & targetTrajectoryAvailable;
 714        }
 715
 15716        bool responseAdmissible = normalResolved
 15717            & inverseMassResolved
 15718            & relativeVelocityResolved
 15719            & normalVelocity < -Fixed64.Epsilon
 15720            & responseFactorResolved
 15721            & closingSpeedResolved
 15722            & responseSpeedResolved
 15723            & sourceDeltaResolved
 15724            & targetDeltaResolved
 15725            & sourceVelocityResolved
 15726            & sourceTrajectoryAvailable
 15727            & targetStateAvailable;
 15728        if (!responseAdmissible)
 729        {
 1730            return false;
 731        }
 732
 14733        ValidateContinuousCollisionPosition(sourcePositionAtImpact, sourcePostLinearVelocity, Context.DeltaTime - hitEla
 14734        if (targetParticipates)
 12735            target.ValidateContinuousCollisionHandoffPosition(targetResolvedPosition, targetPostLinearVelocity, targetAn
 736
 12737        _ = Context.Physics.TryReserveContinuousCollisionCandidateRefresh(this);
 12738        if (targetParticipates)
 10739            _ = Context.Physics2D.TryReserveContinuousCollisionCandidateRefresh(target);
 740
 12741        ApplyCollisionVelocityState(sourcePostLinearVelocity, _angularVelocity);
 12742        UpdateContinuousCollisionFrameTrajectory(sourcePositionAtImpact, _linearVelocity, hitElapsedTime);
 12743        Context.Physics.RefreshContinuousCollisionCandidate(this);
 12744        if (targetParticipates)
 745        {
 10746            target.ApplyContinuousCollisionHandoffStateReserved(
 10747                targetResolvedPosition,
 10748                targetRotationAtImpact,
 10749                targetPostLinearVelocity,
 10750                targetAngularVelocity,
 10751                remainingTime,
 10752                ignoredCollider3D: Collider);
 753        }
 754
 12755        return true;
 756    }
 757
 758    private void UpdateContinuousCollisionFrameTrajectory(
 759        Vector3d positionAtElapsedTime,
 760        Vector3d velocity,
 761        Fixed64 elapsedTime)
 762    {
 456763        AppendContinuousCollisionSegment(
 456764            positionAtElapsedTime,
 456765            velocity,
 456766            elapsedTime);
 456767    }
 768
 769    private Fixed64 ResolveContinuousCollisionFrameFraction(Fixed64 hitElapsedTime)
 770    {
 138771        return FixedMath.Clamp01(hitElapsedTime / Context.DeltaTime);
 772    }
 773
 774    private Fixed64 ResolveContinuousCollisionRestitution(SolidBody target, Fixed64 closingSpeed)
 775    {
 63776        if (closingSpeed <= Context.Settings.RestitutionVelocityThreshold)
 2777            return Fixed64.Zero;
 778
 61779        return PhysicsMaterial.CombineRestitution(Collider.Material, target.Collider.Material);
 780    }
 781
 782    private Fixed64 ResolveContinuousCollisionRestitution(SolidBody2D target, Fixed64 closingSpeed)
 783    {
 22784        if (closingSpeed <= Context.Settings.RestitutionVelocityThreshold)
 4785            return Fixed64.Zero;
 786
 18787        return PhysicsMaterial.CombineRestitution(Collider.Material, target.Collider.Material);
 788    }
 789
 790    private void RemoveClosingContinuousCollisionVelocity(Vector3d normal)
 791    {
 432792        Fixed64 closingSpeed = Vector3d.Dot(_linearVelocity, normal);
 432793        if (closingSpeed >= Fixed64.Zero)
 34794            return;
 795
 398796        Vector3d lastVelocity = _linearVelocity;
 398797        _linearVelocity -= normal * closingSpeed;
 398798        RefreshLinearMotionState(lastVelocity);
 398799        Context.Diagnostics.EmitLinearVelocityDelta(this, lastVelocity, _linearVelocity);
 398800    }
 801
 802}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.ContinuousCollision.Helpers.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody.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 spatial sweep modes and proxy radii and filters eligible continuous collision hits.</content>
 17public partial class SolidBody
 18{
 19    private bool IsEligibleDynamicContinuousCollisionTarget(SolidBody target) =>
 794920        (IsKinematic || !target.ShouldOwnContinuousCollisionMovingPair(
 794921            HasContinuousCollisionRotationalMotion))
 794922        && !ContinuousCollisionCandidateOrdering.IsIgnoredTarget(
 794923            target.Collider,
 794924            _continuousCollisionHandoffIgnoredCollider3D)
 794925        && ContinuousCollisionTargetPolicy.AllowsIndexed3DTarget(
 794926            ReferenceEquals(target, this),
 794927            target.Active,
 794928            target.IsDynamic,
 794929            target.IsKinematic,
 794930            target.IsKinematic && target.HasContinuousCollisionMotion,
 794931            target.Collider.IsTrigger,
 794932            Context.Physics.RequireCollisionPair(Collider, target.Collider));
 33
 34    private bool IsEligibleDynamicMixed2DTarget(SolidBody2D target) =>
 13935        (IsKinematic || !target.ShouldOwnContinuousCollisionMovingPair(
 13936            HasContinuousCollisionRotationalMotion))
 13937        && !ContinuousCollisionCandidateOrdering.IsIgnoredTarget(
 13938            target.Collider,
 13939            _continuousCollisionHandoffIgnoredCollider2D)
 13940        && ContinuousCollisionTargetPolicy.AllowsMixedIndexedTarget(
 13941            target.Active,
 13942            target.IsDynamic,
 13943            target.IsKinematic,
 13944            target.IsKinematic && target.HasContinuousCollisionMotion,
 13945            target.Collider.IsTrigger,
 13946            Context.MixedCollisions.RequireCollisionPair(Collider, target.Collider));
 47
 48    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 49    private bool ShouldUseContinuousCollision(out ContinuousCollisionMode mode)
 50    {
 16544651        mode = ResolveContinuousCollisionMode();
 16544652        return mode == ContinuousCollisionMode.Continuous || mode == ContinuousCollisionMode.Auto;
 53    }
 54
 55    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 56    private ContinuousCollisionMode ResolveContinuousCollisionMode()
 57    {
 16544658        ContinuousCollisionMode mode = _continuousCollisionMode;
 16544659        if (mode != ContinuousCollisionMode.Inherit)
 2895060            return mode;
 61
 13649662        SolidBody? parentBody = Collider.TopParent3D?.Body;
 13649663        if (parentBody != null && parentBody._continuousCollisionMode != ContinuousCollisionMode.Inherit)
 664            return parentBody._continuousCollisionMode;
 65
 13649066        mode = Context.Settings.DefaultContinuousCollisionMode;
 13649067        return mode == ContinuousCollisionMode.Inherit
 13649068            ? ContinuousCollisionMode.Discrete
 13649069            : mode;
 70    }
 71
 72    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 9870973    internal Fixed64 ResolveContinuousCollisionProxyRadius() => Collider.CanonicalPivotProxyRadius;
 74
 75    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 76    private static bool IsClosingContinuousCollisionHit(Vector3d displacement, Vector3d normal) =>
 743677        Vector3d.Dot(displacement, normal) < -Fixed64.Epsilon;
 78
 79    private static Vector3d ResolveShapeExactContinuousClosingNormal(
 80        Physics3DHit hit)
 81    {
 82        // Query normals oppose the sweep; start-contact admission instead
 83        // needs the target's outward surface normal.
 1884        if (hit.Distance > ShapeExactContinuousContactSlop
 1885            || !hit.TryGetPoint(out Vector3d startContactPoint))
 86        {
 287            return hit.Normal;
 88        }
 89
 1690        Vector3d surfaceNormal = hit.Collider!.GetNormalAtPoint(
 1691            startContactPoint);
 1692        return surfaceNormal;
 93    }
 94
 95    private bool IsValidContinuousCollisionTarget(LSCollider hitCollider)
 96    {
 702197        SolidBody? hitBody = hitCollider.Body;
 702198        if (hitBody?.IsKinematic == true)
 99        {
 10100            hitBody.EnsureContinuousCollisionFramePrepared(Context.LateSimulateToken);
 10101            if (hitBody.HasContinuousCollisionMotion)
 4102                return false;
 103        }
 104
 7017105        return ContinuousCollisionTargetPolicy.AllowsStaticOrKinematic3DTarget(
 7017106            ReferenceEquals(hitCollider, Collider),
 7017107            ContinuousCollisionCandidateOrdering.IsIgnoredTarget(hitCollider, _continuousCollisionHandoffIgnoredCollider
 7017108            hitCollider.IsTrigger,
 7017109            Context.Physics.RequireCollisionPair(Collider, hitCollider),
 7017110            hitCollider.IsStatic,
 7017111            hitBody != null && hitBody.IsKinematic);
 112    }
 113
 114    private bool IsValidMixedContinuousCollisionHit(LSCollider2D hitCollider)
 115    {
 173116        SolidBody2D? hitBody = hitCollider.Body;
 173117        if (hitBody?.IsKinematic == true)
 118        {
 2119            hitBody.EnsureContinuousCollisionFramePrepared(Context.LateSimulateToken);
 2120            if (hitBody.HasContinuousCollisionMotion)
 2121                return false;
 122        }
 123
 171124        return ContinuousCollisionTargetPolicy.AllowsMixedStaticOrKinematicTarget(
 171125            ContinuousCollisionCandidateOrdering.IsIgnoredTarget(hitCollider, _continuousCollisionHandoffIgnoredCollider
 171126            hitCollider.IsTrigger,
 171127            Context.MixedCollisions.RequireCollisionPair(Collider, hitCollider),
 171128            hitCollider.IsStatic,
 171129            hitBody != null && hitBody.IsKinematic);
 130    }
 131
 132}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.ContinuousCollision.Hits.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody.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 spatial sweep hits and refines contact geometry for stationary, moving, and mixed targets.</content
 19public partial class SolidBody
 20{
 21    private ContinuousSurfaceTriangleAdmission? _admitContinuousSurfaceTriangle;
 22    private Vector3d _continuousSurfaceStart, _continuousSurfaceEnd, _continuousSurfaceBodyStart;
 23
 24    private bool TryGetFirstContinuousCollisionHit(
 25        Vector3d startPosition,
 26        Vector3d proposedPosition,
 27        Fixed64 proxyRadius,
 28        Fixed64 elapsedFrameFraction,
 29        out Vector3d normal,
 30        out Fixed64 distance,
 31        out ContinuousCollisionTargetKind targetKind,
 32        out LSCollider? target3D,
 33        out LSCollider2D? target2D)
 34    {
 142135        Vector3d originalPosition = Position3d;
 142136        bool originalPositionMutated = _positionMutated;
 37        try
 38        {
 142139            Position3d = startPosition;
 142140            Collider.RebuildRuntimeShapeOnly(refreshMassProperties: false);
 41
 142142            int hitCount = QueryStaticContinuousCollisionHits(
 142143                startPosition,
 142144                proposedPosition,
 142145                proxyRadius,
 142146                out bool staticHitsAreShapeExact);
 142147            int mixedHitCount = Context.Settings.RuntimeMode.RunsMixedContacts()
 142148                ? Context.QueryMixed.SweepSphereAgainstStatic2DAll(
 142149                    startPosition,
 142150                    proposedPosition,
 142151                    proxyRadius,
 142152                    PhysicsLayerMask.All,
 142153                    _continuousMixedCollisionHits,
 142154                    Collider,
 142155                    includeTriggers: false,
 142156                    cacheTargetPartitions: true,
 142157                    requireClosingContact: true)
 142158                : 0;
 59
 142160            bool found3D = TryGetFirstValidContinuousCollisionHit(
 142161                startPosition,
 142162                proposedPosition,
 142163                hitCount,
 142164                staticHitsAreShapeExact,
 142165                out Physics3DHit hit3D);
 142166            ContinuousCollisionTargetKind hit3DKind = found3D
 142167                ? ContinuousCollisionTargetKind.Static3D
 142168                : ContinuousCollisionTargetKind.None;
 142169            bool foundDynamic3D = TryGetFirstDynamicContinuousCollisionHit(
 142170                startPosition,
 142171                proposedPosition,
 142172                proxyRadius,
 142173                elapsedFrameFraction,
 142174                out Physics3DHit dynamicHit3D,
 142175                out Fixed64 dynamicClosingSpeed3D);
 142176            if (ContinuousCollisionCandidateOrdering.ShouldReplaceHit(dynamicHit3D, dynamicClosingSpeed3D, foundDynamic3
 77            {
 4978                hit3D = dynamicHit3D;
 4979                found3D = true;
 4980                hit3DKind = ContinuousCollisionTargetKind.Dynamic3D;
 81            }
 82
 142183            bool foundMixed = TryGetFirstValidMixedContinuousCollisionHit(startPosition, proposedPosition, mixedHitCount
 142184            ContinuousCollisionTargetKind hitMixedKind = foundMixed
 142185                ? ContinuousCollisionTargetKind.Static2D
 142186                : ContinuousCollisionTargetKind.None;
 142187            Fixed64 hitMixedDistance = foundMixed
 142188                ? hitMixed.Distance
 142189                : Fixed64.Zero;
 142190            bool foundDynamicMixed = TryGetFirstDynamicMixedContinuousCollisionHit(
 142191                startPosition,
 142192                proposedPosition,
 142193                proxyRadius,
 142194                elapsedFrameFraction,
 142195                out DynamicMixedIntervalHit dynamicMixed);
 142196            if (DynamicMixedIntervalHit.ShouldReplaceStatic(
 142197                    dynamicMixed,
 142198                    foundDynamicMixed,
 142199                    hitMixed,
 1421100                    foundMixed))
 101            {
 52102                hitMixed = dynamicMixed.ExactHit;
 52103                hitMixedDistance = dynamicMixed.SafeDistance;
 52104                foundMixed = true;
 52105                hitMixedKind = dynamicMixed.Status
 52106                    == IntervalSearchStatus.ExactHit
 52107                    ? ContinuousCollisionTargetKind.Dynamic2D
 52108                    : ContinuousCollisionTargetKind.UnresolvedMixed;
 109            }
 110
 1421111            if (DynamicMixedIntervalHit.ShouldSelect3D(
 1421112                    found3D,
 1421113                    hit3D.Distance,
 1421114                    foundMixed,
 1421115                    hitMixedDistance))
 116            {
 434117                normal = hit3D.Normal;
 434118                distance = hit3D.Distance;
 434119                targetKind = hit3DKind;
 434120                target3D = hit3D.Collider;
 434121                target2D = null;
 434122                return true;
 123            }
 124
 987125            if (foundMixed)
 126            {
 213127                normal = hitMixedKind == ContinuousCollisionTargetKind.UnresolvedMixed
 213128                    ? Vector3d.Zero
 213129                    : hitMixed.NormalFor3DSource;
 213130                distance = hitMixedDistance;
 213131                targetKind = hitMixedKind;
 213132                target3D = null;
 213133                target2D = hitMixedKind == ContinuousCollisionTargetKind.UnresolvedMixed
 213134                    ? null
 213135                    : hitMixed.Collider2D;
 213136                return true;
 137            }
 774138        }
 139        finally
 140        {
 1421141            Position3d = originalPosition;
 1421142            Collider.RebuildRuntimeShapeOnly(refreshMassProperties: false);
 1421143            _positionMutated = originalPositionMutated;
 1421144        }
 145
 774146        normal = Vector3d.Zero;
 774147        distance = Fixed64.Zero;
 774148        targetKind = ContinuousCollisionTargetKind.None;
 774149        target3D = null;
 774150        target2D = null;
 774151        return false;
 647152    }
 153
 154    private bool TryGetFirstValidContinuousCollisionHit(
 155        Vector3d startPosition,
 156        Vector3d proposedPosition,
 157        int hitCount,
 158        bool hitsAreShapeExact,
 159        out Physics3DHit hit)
 160    {
 7718161        Vector3d displacement = proposedPosition - startPosition;
 7718162        Vector3d direction = displacement.Normalized;
 7718163        bool found = false;
 7718164        Physics3DHit best = default;
 29238165        for (int i = 0; i < hitCount; i++)
 166        {
 6901167            Physics3DHit candidate = _continuousCollisionHits[i];
 6901168            if (!IsValidContinuousCollisionTarget(candidate.Collider!))
 169                continue;
 170
 171            Physics3DHit refined;
 172            // Other convex sources already apply native admission in their
 173            // exact candidate query; sphere candidates use the proxy collector.
 6896174            bool surfaceHit = Collider is LSSphereCollider
 6896175                && candidate.Collider is LSMeshCollider { StaticSurface: not null };
 6896176            if (surfaceHit)
 177            {
 339178                _continuousSurfaceBodyStart = startPosition;
 339179                _continuousSurfaceStart = Collider.Center;
 339180                _continuousSurfaceEnd = Collider.Center + displacement;
 339181                var admission = _admitContinuousSurfaceTriangle ??= AdmitContinuousSurfaceTriangle;
 339182                _shapeExactContinuousSweepWorker.Prepare(_continuousSurfaceStart, _continuousSurfaceEnd, Collider.Scaled
 339183                if (!_shapeExactContinuousSweepWorker.TrySweepMeshForContinuousCollision((LSMeshCollider)candidate.Colli
 339184                    admission, out refined)) continue;
 160185                refined = ApplyShapeExactContinuousContactSlop(refined);
 186            }
 6557187            else if (hitsAreShapeExact)
 136188                refined = ApplyShapeExactContinuousContactSlop(candidate);
 6421189            else if (TryRefineShapeExactContinuousCollisionHit(candidate, displacement, direction, out Physics3DHit exac
 1190                refined = exactHit;
 6420191            else if (exactSupported)
 192                continue;
 193            else
 6419194                refined = candidate;
 195
 6716196            Vector3d closingNormal = hitsAreShapeExact ? candidate.Normal
 6716197                : refined.Normal;
 198
 6716199            if (!IsClosingContinuousCollisionHit(displacement, closingNormal))
 200                continue;
 201
 6629202            if (found && !ContinuousCollisionHitComesBefore(refined, best))
 203                continue;
 204
 6592205            best = refined;
 6592206            found = true;
 207        }
 208
 7718209        hit = best;
 7718210        return found;
 211    }
 212
 213    private bool AdmitContinuousSurfaceTriangle(int triangle, ref Physics3DHit hit, WideDirection3d? exactNormal)
 214    {
 515215        var target = (LSMeshCollider)hit.Collider!;
 515216        Vector3d displacement = _continuousSurfaceEnd - _continuousSurfaceStart;
 515217        Vector3d center = new FixedSegment(_continuousSurfaceStart, _continuousSurfaceEnd)
 515218            .GetPointAtDistance(hit.Distance, displacement.Magnitude);
 515219        Vector3d original = Position3d;
 515220        bool positionMutated = _positionMutated;
 221        try
 222        {
 515223            Position3d = _continuousSurfaceBodyStart + (center - _continuousSurfaceStart);
 515224            Collider.RebuildRuntimeShapeOnly(refreshMassProperties: false);
 225            // Filled joined contact owners remove authored diagonals before a
 226            // triangle's conservative sweep witness can select response.
 515227            if (TryBuildContinuousCollisionManifold(target))
 228            {
 474229                OrderRotationalContinuousCollisionPair(target, out _, out _, out bool sourceIsA);
 1710230                foreach (ManifoldContact row in _rotationalContinuousCollisionManifold)
 231                {
 497232                    Vector3d normal = ResolveSourceOutwardContactNormal(target, row);
 497233                    if (!IsClosingContinuousCollisionHit(displacement, normal)) continue;
 232234                    hit = new(target, sourceIsA ? row.AnchorB : row.AnchorA, normal, hit.Distance, hit.Direction);
 232235                    return true;
 236                }
 242237                return false;
 238            }
 41239            return IsClosingContinuousCollisionHit(displacement, hit.Normal)
 41240                && target.StaticSurface!.AdmitContinuousWitness(Collider, null, center, target, triangle, hit.Anchor, ex
 241        }
 242        finally
 243        {
 1030244            Position3d = original; Collider.RebuildRuntimeShapeOnly(refreshMassProperties: false);
 515245            _positionMutated = positionMutated;
 515246        }
 515247    }
 248    private int QueryStaticContinuousCollisionHits(
 249        Vector3d startPosition,
 250        Vector3d proposedPosition,
 251        Fixed64 proxyRadius,
 252        out bool hitsAreShapeExact)
 253    {
 7718254        Vector3d displacement = proposedPosition - startPosition;
 7718255        if (Collider is not LSSphereCollider && IsExactConvexSourceSupported(Collider))
 256        {
 575257            hitsAreShapeExact = true;
 575258            _continuousSurfaceBodyStart = startPosition;
 575259            _continuousSurfaceStart = Collider.Center;
 575260            _continuousSurfaceEnd = Collider.Center + displacement;
 575261            return Context.Query3D.SweepExactSourceAgainstStaticAll(
 575262                Collider,
 575263                displacement,
 575264                PhysicsLayerMask.All,
 575265                _continuousCollisionHits,
 575266                Collider,
 575267                includeTriggers: false,
 575268                surfaceAdmission: _admitContinuousSurfaceTriangle ??= AdmitContinuousSurfaceTriangle);
 269        }
 270
 7143271        hitsAreShapeExact = false;
 7143272        return Context.Query3D.SweepSphereAgainstStaticAll(
 7143273            startPosition,
 7143274            proposedPosition,
 7143275            proxyRadius,
 7143276            PhysicsLayerMask.All,
 7143277            _continuousCollisionHits,
 7143278            Collider,
 7143279            includeTriggers: false);
 280    }
 281
 282    private bool TryGetFirstValidMixedContinuousCollisionHit(
 283        Vector3d startPosition,
 284        Vector3d proposedPosition,
 285        int hitCount,
 286        out PhysicsMixedHit hit)
 287    {
 7718288        Vector3d displacement = proposedPosition - startPosition;
 15454289        for (int i = 0; i < hitCount; i++)
 290        {
 173291            PhysicsMixedHit candidate = _continuousMixedCollisionHits[i];
 173292            if (!IsValidMixedContinuousCollisionHit(candidate.Collider2D!))
 293                continue;
 170294            bool exactSphere = candidate.Collider2D!.StaticSurface != null && GravitasQueryMixedService.TryResolveExactS
 170295                candidate.Collider2D!, displacement, out _, out _, out _);
 296
 170297            if (UsesMixedStaticManifold(candidate.Collider2D!))
 298            {
 166299                Vector3d position = new FixedSegment(startPosition, proposedPosition)
 166300                    .GetPointAtDistance(candidate.Distance, displacement.Magnitude);
 166301                if (TryBuildStaticMixedContinuousCollisionManifold(candidate.Collider2D!, position, Rotation))
 302                {
 160303                    ContactAnchor center = ContactAnchor.FromWorldPoint(Vector3d.Zero);
 160304                    if (!MixedContact.TrySelectClosingContact(_rotationalMixedEarliestManifold,
 160305                            center, displacement, Vector3d.Zero, center, Vector2d.Zero, Fixed64.Zero,
 160306                            out MixedContact closing))
 307                        continue;
 158308                    candidate = new PhysicsMixedHit(Collider, candidate.Collider2D, closing.Anchor3D,
 158309                        closing.Anchor2D, closing.Normal3DTo2D, candidate.ReducerKind,
 158310                        candidate.Distance, candidate.Direction3D);
 311                }
 6312                else if (exactSphere && candidate.Collider2D!.StaticSurface is { } surface)
 313                {
 5314                    Vector3d original = Position3d;
 315                    try
 316                    {
 5317                        SetMixedContinuousCollisionSamplePose(position, Rotation);
 5318                        if (!surface.AdmitMixedContinuousWitness(Collider, Collider.Center, candidate.Collider2D,
 9319                            candidate.Anchor2D)) continue;
 1320                    }
 10321                    finally { SetMixedContinuousCollisionSamplePose(original, Rotation); }
 322                }
 323            }
 164324            if (!IsClosingContinuousCollisionHit(displacement, candidate.NormalFor3DSource))
 325                continue;
 326
 164327            hit = candidate;
 164328            return true;
 329        }
 330
 7554331        hit = default;
 7554332        return false;
 333    }
 334
 335    private bool UsesMixedStaticManifold(LSCollider2D target)
 331336        => target.StaticSurface != null || target is LSCompoundCollider2D || Collider is LSCompoundCollider or LSMeshCol
 337
 338    private bool TryBuildStaticMixedContinuousCollisionManifold(LSCollider2D target,
 339        Vector3d position, FixedQuaternion rotation)
 340    {
 166341        Vector3d originalPosition = Position3d;
 166342        FixedQuaternion originalRotation = Rotation;
 343        try
 344        {
 166345            SetMixedContinuousCollisionSamplePose(position, rotation);
 346            // Translational moving-target searches overwrite Known only; the
 347            // mutually exclusive rotational path owns this same accepted buffer.
 166348            return CollisionDetectionMixed.TryBuildDiscreteManifold(Collider, target, _rotationalMixedEarliestManifold);
 349        }
 350        finally
 351        {
 166352            SetMixedContinuousCollisionSamplePose(originalPosition, originalRotation);
 166353        }
 166354    }
 355
 356    private bool TryApplyStaticMixedContinuousCollisionResponse(LSCollider2D target,
 357        Vector3d position, Fixed64 elapsedTime, Fixed64 remainingTime)
 358    {
 161359        if (!UsesMixedStaticManifold(target)
 161360            || !_rotationalMixedEarliestManifold.HasContact)
 4361            return false;
 157362        return TryApplyMixedRotationalContinuousCollisionResponse(target,
 157363            MixedContact.FromManifoldContact(_rotationalMixedEarliestManifold.PrimaryContact),
 157364            Fixed64.Zero, position, Vector3d.Zero, Rotation, Rotation, elapsedTime, remainingTime,
 157365            IsKinematic, _rotationalMixedEarliestManifold);
 366    }
 367
 368    private bool TryRefineShapeExactContinuousCollisionHit(
 369        Physics3DHit candidate,
 370        Vector3d displacement,
 371        Vector3d direction,
 372        out Physics3DHit refined,
 373        out bool exactSupported)
 374    {
 6421375        refined = default;
 6421376        exactSupported = false;
 377
 6421378        if (Collider is LSSphereCollider)
 6417379            return false;
 380
 4381        if (candidate.Collider is not LSSphereCollider targetSphere)
 2382            return false;
 383
 2384        return TryRefineContinuousCollisionAgainstTargetSphere(targetSphere, displacement, direction, out refined, out e
 385    }
 386
 387    private bool TryRefineContinuousCollisionAgainstTargetSphere(
 388        LSSphereCollider targetSphere,
 389        Vector3d displacement,
 390        Vector3d direction,
 391        out Physics3DHit refined,
 392        out bool exactSupported)
 393    {
 36394        exactSupported = true;
 36395        refined = default;
 396
 36397        Vector3d reverseStart = targetSphere.Center;
 36398        Vector3d reverseEnd = targetSphere.Center - displacement;
 36399        _shapeExactContinuousSweepWorker.Prepare(reverseStart, reverseEnd, targetSphere.ScaledRadius);
 36400        if (!_shapeExactContinuousSweepWorker.TrySweep(Collider, out Vector3d reverseCenterAtImpact, out Fixed64 distanc
 32401            return false;
 402
 4403        _ = ContinuousCollisionContactPolicy.TryResolveSweptSphereContact(
 4404            Collider,
 4405            reverseCenterAtImpact,
 4406            -direction,
 4407            out _,
 4408            out Vector3d sourceSurfaceNormal);
 409
 4410        Vector3d normal = -sourceSurfaceNormal;
 4411        refined = new Physics3DHit(
 4412            targetSphere,
 4413            new ContactAnchor(
 4414                targetSphere.Center,
 4415                normal * targetSphere.ScaledRadius),
 4416            normal,
 4417            distance,
 4418            direction);
 4419        return true;
 420    }
 421
 422    private static Physics3DHit ApplyShapeExactContinuousContactSlop(Physics3DHit hit)
 423    {
 299424        Fixed64 distance = hit.Distance > ShapeExactContinuousContactSlop
 299425            ? hit.Distance - ShapeExactContinuousContactSlop
 299426            : Fixed64.Zero;
 299427        return new Physics3DHit(hit.Collider, hit.Anchor, hit.Normal, distance, hit.Direction);
 428    }
 429
 430    private void PrepareExactConvexSourceSweep(Vector3d displacement)
 431    {
 7432        switch (Collider)
 433        {
 434            case LSMeshCollider mesh:
 1435                _shapeExactContinuousConvexSweepWorker.PrepareConvexMeshSource(mesh, displacement);
 1436                return;
 437            case LSCompoundCollider compound:
 1438                _shapeExactContinuousConvexSweepWorker.PrepareCompoundSource(compound, displacement);
 1439                return;
 440            default:
 5441                _shapeExactContinuousConvexSweepWorker.PreparePrimitiveSource(Collider, displacement);
 5442                return;
 443        }
 444    }
 445
 446    private static bool IsExactConvexSourceSupported(LSCollider collider)
 447    {
 615448        return collider switch
 615449        {
 270450            LSCapsuleCollider => true,
 284451            LSCuboidCollider => true,
 9452            LSCylinderCollider => true,
 2453            LSConeCollider => true,
 14454            LSMeshCollider { Mode: MeshColliderMode.Convex } => true,
 10455            LSCompoundCollider => true,
 26456            _ => false
 615457        };
 458    }
 459
 460    private static bool ContinuousCollisionHitComesBefore(Physics3DHit left, Physics3DHit right)
 461    {
 37462        int distanceCompare = left.Distance.CompareTo(right.Distance);
 37463        if (distanceCompare != 0)
 32464            return distanceCompare < 0;
 465
 5466        return left.Collider!.Id < right.Collider!.Id;
 467    }
 468
 469    private bool TryGetFirstDynamicContinuousCollisionHit(
 470        Vector3d startPosition,
 471        Vector3d proposedPosition,
 472        Fixed64 proxyRadius,
 473        Fixed64 elapsedFrameFraction,
 474        out Physics3DHit hit,
 475        out Fixed64 closingSpeed)
 476    {
 1421477        hit = default;
 1421478        closingSpeed = Fixed64.Zero;
 479
 1421480        Vector3d sourceDisplacement = proposedPosition - startPosition;
 1421481        Fixed64 sourceLength = sourceDisplacement.Magnitude;
 1421482        bool found = false;
 1421483        Physics3DHit best = default;
 1421484        Fixed64 bestClosingSpeed = Fixed64.Zero;
 1421485        long token = Context.LateSimulateToken;
 1421486        SwiftList<int> candidateIds = Context.Physics.QueryContinuousCollisionCandidates(
 1421487            DynamicCcdCandidateIndex.CreateSweptSphereBounds(startPosition, sourceDisplacement, proxyRadius));
 6116488        for (int candidateIndex = 0; candidateIndex < candidateIds.Count; candidateIndex++)
 489        {
 1637490            int dynamicId = candidateIds[candidateIndex];
 1637491            SolidBody target = Context.Physics.GetContinuousCollisionCandidate(dynamicId);
 1637492            if (!IsEligibleDynamicContinuousCollisionTarget(target))
 493            {
 494                continue;
 495            }
 496
 104497            target.EnsureContinuousCollisionFramePrepared(token);
 104498            if (!TryGetDynamicRelativeContinuousCollisionHit(
 104499                    target,
 104500                    startPosition,
 104501                    sourceDisplacement,
 104502                    proxyRadius,
 104503                    sourceLength,
 104504                    elapsedFrameFraction,
 104505                    out Physics3DHit candidate,
 104506                    out Fixed64 candidateClosingSpeed))
 507                continue;
 508
 60509            if (!ContinuousCollisionCandidateOrdering.ShouldReplaceHit(candidate, candidateClosingSpeed, true, found, be
 510                continue;
 511
 52512            best = candidate;
 52513            bestClosingSpeed = candidateClosingSpeed;
 52514            found = true;
 515        }
 516
 1421517        hit = best;
 1421518        closingSpeed = bestClosingSpeed;
 1421519        return found;
 520    }
 521
 522    internal bool TryGetDynamicRelativeContinuousCollisionHit(
 523        SolidBody target,
 524        Vector3d sourceStart,
 525        Vector3d sourceDisplacement,
 526        Fixed64 sourceRadius,
 527        Fixed64 sourceLength,
 528        Fixed64 queryStartFraction,
 529        out Physics3DHit hit,
 530        out Fixed64 closingSpeed)
 531    {
 121532        hit = default;
 121533        closingSpeed = Fixed64.Zero;
 121534        Vector3d sourceEnd = sourceStart + sourceDisplacement;
 121535        Vector3d sourceDirection = sourceDisplacement.Normalized;
 121536        Fixed64 targetRadius = target.ResolveContinuousCollisionProxyRadius();
 121537        bool exactSupported = Collider is LSSphereCollider
 121538            || target.Collider is LSSphereCollider
 121539            || IsExactConvexSourceSupported(Collider);
 121540        bool found = false;
 121541        Physics3DHit best = default;
 121542        Fixed64 bestClosingSpeed = Fixed64.Zero;
 543
 121544        Vector3d originalSourcePosition = Position3d;
 121545        FixedQuaternion originalSourceRotation = Rotation;
 121546        bool originalSourcePositionMutated = _positionMutated;
 121547        bool originalSourceRotationMutated = _rotationMutated;
 121548        Vector3d originalTargetPosition = target.Position3d;
 121549        FixedQuaternion originalTargetRotation = target.Rotation;
 121550        bool originalTargetPositionMutated = target._positionMutated;
 121551        bool originalTargetRotationMutated = target._rotationMutated;
 552
 553        try
 554        {
 121555            target.GetContinuousCollisionTrajectorySegmentRange(
 121556                queryStartFraction,
 121557                out int segmentStartIndex,
 121558                out int segmentEndExclusive);
 121559            for (int segmentIndex = segmentStartIndex;
 175560                segmentIndex < segmentEndExclusive;
 54561                segmentIndex++)
 562            {
 128563                ContinuousCollisionMotionSegment3D segment =
 128564                    target.GetContinuousCollisionTrajectorySegment(segmentIndex);
 128565                ContinuousCollisionMath.ClipTranslationalTrajectoryInterval(
 128566                    queryStartFraction,
 128567                    segment.StartFraction,
 128568                    segment.EndFraction,
 128569                    out Fixed64 overlapStart,
 128570                    out Fixed64 overlapEnd,
 128571                    out Fixed64 sourceStartTime,
 128572                    out Fixed64 sourceEndTime);
 573
 128574                Vector3d sourceSegmentStart = Vector3d.Lerp(
 128575                    sourceStart,
 128576                    sourceEnd,
 128577                    sourceStartTime);
 128578                Vector3d sourceSegmentEnd = Vector3d.Lerp(
 128579                    sourceStart,
 128580                    sourceEnd,
 128581                    sourceEndTime);
 128582                Vector3d sourceSegmentDisplacement =
 128583                    ContinuousCollisionSweepRange.ValidateEndpoint(
 128584                        sourceSegmentStart,
 128585                        sourceSegmentEnd,
 128586                        out Fixed64 sourceSegmentLength);
 128587                Vector3d targetStart = segment.SamplePosition(overlapStart);
 128588                Vector3d targetEnd = segment.SamplePosition(overlapEnd);
 128589                Vector3d targetDisplacement =
 128590                    ContinuousCollisionSweepRange.ValidateEndpoint(
 128591                        targetStart,
 128592                        targetEnd,
 128593                        out _);
 128594                LSSphereCollider? radialSourceSphere = Collider as LSSphereCollider;
 128595                LSSphereCollider? radialTargetSphere = target.Collider as LSSphereCollider;
 128596                bool radialPair = radialSourceSphere is not null
 128597                    && radialTargetSphere is not null;
 128598                Fixed64 radialEntryDistance = default;
 128599                Vector3d relativeDisplacement = default;
 128600                Fixed64 relativeLength = default;
 128601                Vector3d normal = default;
 128602                if (!radialPair
 128603                    && !ContinuousCollisionMath.TryGetRelativeSphereOverlapDistanceInterval(
 128604                            sourceSegmentStart,
 128605                            sourceSegmentDisplacement,
 128606                            sourceRadius,
 128607                            targetStart,
 128608                            targetDisplacement,
 128609                            targetRadius,
 128610                            out radialEntryDistance,
 128611                            out _,
 128612                            out relativeDisplacement,
 128613                            out relativeLength,
 128614                            out normal,
 128615                            out _))
 616                {
 617                    continue;
 618                }
 619
 128620                Position3d = sourceSegmentStart;
 128621                target.Position3d = targetStart;
 128622                target.Rotation = target.SampleContinuousCollisionRotation(
 128623                    overlapStart);
 128624                Collider.RebuildRuntimeShapeOnly(refreshMassProperties: false);
 128625                target.Collider.RebuildRuntimeShapeOnly(refreshMassProperties: false);
 626
 128627                Physics3DHit relativeHit = default;
 128628                Vector3d closingNormal = normal;
 128629                bool foundExact = false;
 128630                if (radialPair)
 631                {
 79632                    foundExact = ContinuousCollisionMath
 79633                        .TryGetRelativeSphereOverlapDistanceInterval(
 79634                            radialSourceSphere!.Center,
 79635                            sourceSegmentDisplacement,
 79636                            radialSourceSphere.ScaledRadius,
 79637                            radialTargetSphere!.Center,
 79638                            targetDisplacement,
 79639                            radialTargetSphere.ScaledRadius,
 79640                            out radialEntryDistance,
 79641                            out _,
 79642                            out relativeDisplacement,
 79643                            out relativeLength,
 79644                            out normal,
 79645                            out _);
 646                }
 647
 128648                Vector3d relativeDirection = relativeDisplacement.Normalized;
 128649                if (radialPair && foundExact)
 650                {
 64651                    relativeHit = new Physics3DHit(
 64652                        radialTargetSphere!,
 64653                        new ContactAnchor(
 64654                            radialTargetSphere!.Center,
 64655                            normal * radialTargetSphere.ScaledRadius),
 64656                        normal,
 64657                        radialEntryDistance,
 64658                        relativeDirection);
 64659                    closingNormal = relativeHit.Normal;
 660                }
 64661                else if (!radialPair && Collider is LSSphereCollider sourceSphere)
 662                {
 6663                    foundExact = TrySweepRelativeSourceSphere(
 6664                        sourceSphere,
 6665                        target.Collider,
 6666                        relativeDisplacement,
 6667                        relativeDirection,
 6668                        out relativeHit);
 6669                    if (foundExact)
 5670                        closingNormal = relativeHit.Normal;
 671                }
 58672                else if (!radialPair && target.Collider is LSSphereCollider targetSphere)
 673                {
 34674                    foundExact = TryRefineContinuousCollisionAgainstTargetSphere(
 34675                        targetSphere,
 34676                        relativeDisplacement,
 34677                        relativeDirection,
 34678                        out relativeHit,
 34679                        out _);
 34680                    if (foundExact)
 3681                        closingNormal = relativeHit.Normal;
 682                }
 24683                else if (IsExactConvexSourceSupported(Collider))
 684                {
 7685                    PrepareExactConvexSourceSweep(relativeDisplacement);
 7686                    foundExact = _shapeExactContinuousConvexSweepWorker
 7687                        .TrySweepPreparedSource(
 7688                            target.Collider,
 7689                            out Physics3DHit shapeExactConvexHit,
 7690                            requireClosingInitialContact: true);
 7691                    if (foundExact)
 692                    {
 3693                        relativeHit = ApplyShapeExactContinuousContactSlop(
 3694                            shapeExactConvexHit);
 3695                        closingNormal = shapeExactConvexHit.Normal;
 696                    }
 697                }
 698
 128699                if (exactSupported && !foundExact)
 700                    continue;
 701
 77702                Fixed64 contactDistance = radialEntryDistance;
 77703                if (foundExact)
 704                {
 75705                    contactDistance = relativeHit.Distance;
 75706                    normal = relativeHit.Normal;
 707                }
 708
 77709                Fixed64 successorStart = segmentIndex + 1
 77710                    < target.ContinuousCollisionTrajectoryCount
 77711                    ? target.GetContinuousCollisionTrajectorySegment(segmentIndex + 1)
 77712                        .StartFraction
 77713                    : Fixed64.Zero;
 77714                if (ContinuousCollisionMath.IsSupersededTranslationalBoundaryHit(
 77715                        contactDistance >= relativeLength,
 77716                        overlapEnd,
 77717                        segmentIndex,
 77718                        target.ContinuousCollisionTrajectoryCount,
 77719                        successorStart))
 720                {
 721                    continue;
 722                }
 723
 76724                Fixed64 localClosingSpeed =
 76725                    -Vector3d.Dot(relativeDisplacement, closingNormal);
 76726                if (!ContinuousCollisionMath.TryNormalizeTranslationalClosingSpeed(
 76727                        localClosingSpeed,
 76728                        sourceEndTime - sourceStartTime,
 76729                        out Fixed64 candidateClosingSpeed))
 730                    continue;
 731
 74732                Vector3d sourceCenter = new FixedSegment(
 74733                    sourceSegmentStart,
 74734                    sourceSegmentEnd).GetPointAtDistance(
 74735                        contactDistance,
 74736                        relativeLength);
 74737                Vector3d targetCenter = new FixedSegment(
 74738                    targetStart,
 74739                    targetEnd).GetPointAtDistance(
 74740                        contactDistance,
 74741                        relativeLength);
 742                ContactAnchor anchor;
 74743                if (foundExact
 74744                    && relativeHit.Anchor.TryGetOffsetFrom(
 74745                        target.Collider.Center,
 74746                        out Vector3d targetOffset))
 747                {
 72748                    anchor = new ContactAnchor(targetCenter, targetOffset);
 749                }
 750                else
 751                {
 2752                    anchor = ContactAnchor.FromWorldPoint(
 2753                        ContinuousCollisionMath.ResolveContactPointOnTarget(
 2754                            sourceCenter,
 2755                            targetCenter,
 2756                            normal,
 2757                            targetRadius));
 758                }
 74759                if (!Fixed64.TryMultiplyDivide(
 74760                        sourceSegmentLength,
 74761                        contactDistance,
 74762                        relativeLength,
 74763                        out Fixed64 localSourceDistance)
 74764                    || !Fixed64.TryMultiplyAdd(
 74765                        sourceLength,
 74766                        sourceStartTime,
 74767                        localSourceDistance,
 74768                        out Fixed64 sourceDistance))
 769                {
 770                    continue;
 771                }
 74772                var candidate = new Physics3DHit(
 74773                    target.Collider,
 74774                    anchor,
 74775                    normal,
 74776                    sourceDistance,
 74777                    sourceDirection);
 74778                best = candidate;
 74779                bestClosingSpeed = candidateClosingSpeed;
 74780                found = true;
 74781                break;
 782            }
 47783        }
 784        finally
 785        {
 121786            Position3d = originalSourcePosition;
 121787            Rotation = originalSourceRotation;
 121788            target.Position3d = originalTargetPosition;
 121789            target.Rotation = originalTargetRotation;
 121790            target.Collider.RebuildRuntimeShapeOnly(refreshMassProperties: false);
 121791            Collider.RebuildRuntimeShapeOnly(refreshMassProperties: false);
 121792            target._positionMutated = originalTargetPositionMutated;
 121793            target._rotationMutated = originalTargetRotationMutated;
 121794            _positionMutated = originalSourcePositionMutated;
 121795            _rotationMutated = originalSourceRotationMutated;
 121796        }
 797
 121798        hit = best;
 121799        closingSpeed = bestClosingSpeed;
 121800        return found;
 801    }
 802
 803    private bool TrySweepRelativeSourceSphere(
 804        LSSphereCollider sourceSphere,
 805        LSCollider target,
 806        Vector3d relativeDisplacement,
 807        Vector3d relativeDirection,
 808        out Physics3DHit hit)
 809    {
 6810        hit = default;
 6811        _shapeExactContinuousSweepWorker.Prepare(
 6812            sourceSphere.Center,
 6813            sourceSphere.Center + relativeDisplacement,
 6814            sourceSphere.ScaledRadius);
 6815        if (!_shapeExactContinuousSweepWorker.TrySweep(target, out Vector3d sphereCenterAtImpact, out Fixed64 distance))
 1816            return false;
 817
 5818        _ = ContinuousCollisionContactPolicy.TryResolveSweptSphereContact(
 5819            target,
 5820            sphereCenterAtImpact,
 5821            relativeDirection,
 5822            out ContactAnchor anchor,
 5823            out Vector3d normal);
 824
 5825        hit = new Physics3DHit(target, anchor, normal, distance, relativeDirection);
 5826        return true;
 827    }
 828
 829    private bool TryGetFirstDynamicMixedContinuousCollisionHit(
 830        Vector3d startPosition,
 831        Vector3d proposedPosition,
 832        Fixed64 proxyRadius,
 833        Fixed64 elapsedFrameFraction,
 834        out DynamicMixedIntervalHit hit)
 835    {
 1421836        hit = default;
 837
 1421838        if (!Context.Settings.RuntimeMode.RunsMixedContacts())
 969839            return false;
 840
 452841        Vector3d sourceDisplacement = proposedPosition - startPosition;
 452842        Fixed64 sourceLength = sourceDisplacement.Magnitude;
 452843        Vector3d sourceDirection = sourceDisplacement.Normalized;
 452844        bool found = false;
 452845        DynamicMixedIntervalHit best = default;
 452846        long token = Context.LateSimulateToken;
 452847        SwiftList<int> candidateIds = Context.Physics2D.QueryMixedContinuousCollisionCandidates(
 452848            DynamicCcdCandidateIndex.CreateSweptSphereBounds(startPosition, sourceDisplacement, proxyRadius));
 1152849        for (int candidateIndex = 0; candidateIndex < candidateIds.Count; candidateIndex++)
 850        {
 124851            int dynamicId = candidateIds[candidateIndex];
 124852            SolidBody2D target = Context.Physics2D.GetContinuousCollisionCandidate(dynamicId);
 124853            if (!IsEligibleDynamicMixed2DTarget(target))
 854            {
 855                continue;
 856            }
 857
 55858            target.EnsureContinuousCollisionFramePrepared(token);
 55859            Fixed64 targetRadius =
 55860                target.ResolveMixedContinuousCollisionProxyRadius();
 55861            IntervalSearchStatus status =
 55862                TryGetDynamicMixed2DContinuousCollisionHit(
 55863                    target,
 55864                    startPosition,
 55865                    sourceDisplacement,
 55866                    proxyRadius,
 55867                    targetRadius,
 55868                    sourceLength,
 55869                    elapsedFrameFraction,
 55870                    out DynamicMixedIntervalHit candidate);
 55871            if (status
 55872                == IntervalSearchStatus.CertifiedNoHit)
 873            {
 874                continue;
 875            }
 876
 53877            best = DynamicMixedIntervalHit.Select(
 53878                candidate,
 53879                best,
 53880                ref found);
 881        }
 882
 452883        hit = best;
 452884        return found;
 885    }
 886
 887    private IntervalSearchStatus
 888        TryGetDynamicMixed2DContinuousCollisionHit(
 889        SolidBody2D target,
 890        Vector3d sourceStart,
 891        Vector3d sourceDisplacement,
 892        Fixed64 sourceRadius,
 893        Fixed64 targetRadius,
 894        Fixed64 sourceLength,
 895        Fixed64 queryStartFraction,
 896        out DynamicMixedIntervalHit hit)
 897    {
 79898        hit = default;
 79899        Vector3d sourceEnd = sourceStart + sourceDisplacement;
 79900        Vector3d sourceDirection = sourceDisplacement.Normalized;
 79901        target.GetContinuousCollisionTrajectorySegmentRange(
 79902            queryStartFraction,
 79903            out int segmentStartIndex,
 79904            out int segmentEndExclusive);
 79905        for (int segmentIndex = segmentStartIndex;
 90906            segmentIndex < segmentEndExclusive;
 11907            segmentIndex++)
 908        {
 82909            ContinuousCollisionMotionSegment2D segment =
 82910                target.GetContinuousCollisionTrajectorySegment(segmentIndex);
 82911            ContinuousCollisionMath.ClipTranslationalTrajectoryInterval(
 82912                queryStartFraction,
 82913                segment.StartFraction,
 82914                segment.EndFraction,
 82915                out Fixed64 overlapStart,
 82916                out Fixed64 overlapEnd,
 82917                out Fixed64 sourceStartTime,
 82918                out Fixed64 sourceEndTime);
 919
 82920            Vector3d sourceSegmentStart = Vector3d.Lerp(
 82921                sourceStart,
 82922                sourceEnd,
 82923                sourceStartTime);
 82924            Vector3d sourceSegmentEnd = Vector3d.Lerp(
 82925                sourceStart,
 82926                sourceEnd,
 82927                sourceEndTime);
 82928            Vector3d sourceSegmentDisplacement =
 82929                ContinuousCollisionSweepRange.ValidateEndpoint(
 82930                    sourceSegmentStart,
 82931                    sourceSegmentEnd,
 82932                    out Fixed64 sourceSegmentLength);
 82933            Vector2d targetStart2D = segment.SamplePosition(overlapStart);
 82934            Vector2d targetEnd2D = segment.SamplePosition(overlapEnd);
 82935            Vector2d targetDisplacement2D =
 82936                ContinuousCollisionSweepRange.ValidateEndpoint(
 82937                    targetStart2D,
 82938                    targetEnd2D,
 82939                    out Fixed64 targetSegmentLength);
 82940            Vector3d targetStart = new(
 82941                targetStart2D.X,
 82942                target.Collider.MixedSlabCenterY,
 82943                targetStart2D.Y);
 82944            Vector3d targetEnd = new(
 82945                targetEnd2D.X,
 82946                target.Collider.MixedSlabCenterY,
 82947                targetEnd2D.Y);
 82948            Vector3d targetDisplacement = new(
 82949                targetDisplacement2D.X,
 82950                Fixed64.Zero,
 82951                targetDisplacement2D.Y);
 82952            if (!ContinuousCollisionMath.TryGetRelativeSphereOverlapDistanceInterval(
 82953                    sourceSegmentStart,
 82954                    sourceSegmentDisplacement,
 82955                    sourceRadius,
 82956                    targetStart,
 82957                    targetDisplacement,
 82958                    targetRadius,
 82959                    out Fixed64 entryDistance,
 82960                    out Fixed64 exitDistance,
 82961                    out Vector3d relativeDisplacement,
 82962                    out Fixed64 relativeLength,
 82963                    out _,
 82964                    out _))
 965            {
 966                continue;
 967            }
 968
 79969            Fixed64 successorStart = segmentIndex + 1
 79970                < target.ContinuousCollisionTrajectoryCount
 79971                ? target.GetContinuousCollisionTrajectorySegment(segmentIndex + 1)
 79972                    .StartFraction
 79973                : Fixed64.Zero;
 79974            if (ContinuousCollisionMath.IsSupersededTranslationalBoundaryHit(
 79975                    entryDistance >= relativeLength,
 79976                    overlapEnd,
 79977                    segmentIndex,
 79978                    target.ContinuousCollisionTrajectoryCount,
 79979                    successorStart))
 980            {
 981                continue;
 982            }
 983
 79984            FixedQuaternion sourceStartRotation =
 79985                SampleContinuousCollisionRotation(overlapStart);
 79986            FixedQuaternion sourceEndRotation =
 79987                SampleContinuousCollisionRotation(overlapEnd);
 79988            Fixed64 angularDistance =
 79989                ResolveKinematicAngularDistanceRadians(
 79990                    sourceStartRotation,
 79991                    sourceEndRotation);
 79992            Fixed64 intervalElapsedTime =
 79993                overlapStart * Context.DeltaTime;
 79994            Fixed64 intervalDuration =
 79995                (overlapEnd - overlapStart) * Context.DeltaTime;
 79996            bool foundExactContact =
 79997                TryGetExactSphereCircleTranslationalContact(
 79998                    target,
 79999                    segment,
 791000                    new FixedSegment2d(targetStart2D, targetEnd2D),
 791001                    targetDisplacement2D,
 791002                    targetSegmentLength,
 791003                    new FixedSegment(sourceSegmentStart, sourceSegmentEnd),
 791004                    sourceSegmentDisplacement,
 791005                    sourceSegmentLength,
 791006                    sourceStartRotation,
 791007                    angularDistance,
 791008                    out bool requiresRotationalFallback,
 791009                    out Fixed64 exactRelativeDistance,
 791010                    out Fixed64 sourceContactDistance,
 791011                    out Fixed64 exactLocalClosingSpeed,
 791012                    out MixedContact exactContact);
 791013            if (foundExactContact)
 1014            {
 281015                if (ContinuousCollisionMath.IsSupersededTranslationalBoundaryHit(
 281016                        exactRelativeDistance >= relativeLength,
 281017                        overlapEnd,
 281018                        segmentIndex,
 281019                        target.ContinuousCollisionTrajectoryCount,
 281020                        successorStart))
 1021                {
 1022                    continue;
 1023                }
 1024
 261025                if (!ContinuousCollisionMath.TryNormalizeTranslationalClosingSpeed(
 261026                        exactLocalClosingSpeed,
 261027                        sourceEndTime - sourceStartTime,
 261028                        out Fixed64 exactClosingSpeed))
 1029                {
 1030                    continue;
 1031                }
 1032
 261033                if (!Fixed64.TryMultiplyAdd(
 261034                        sourceLength,
 261035                        sourceStartTime,
 261036                        sourceContactDistance,
 261037                        out Fixed64 exactSourceDistance))
 1038                {
 1039                    continue;
 1040                }
 1041
 261042                var analyticHit = new PhysicsMixedHit(
 261043                    null,
 261044                    target.Collider,
 261045                    exactContact.Anchor3D,
 261046                    exactContact.Anchor2D,
 261047                    exactContact.Normal3DTo2D,
 261048                    PhysicsQueryReducerKind.Exact,
 261049                    exactSourceDistance,
 261050                    sourceDirection);
 261051                hit = new DynamicMixedIntervalHit(
 261052                    IntervalSearchStatus.ExactHit,
 261053                    analyticHit,
 261054                    analyticHit.Distance,
 261055                    exactClosingSpeed,
 261056                    target.Collider.Id);
 261057                return IntervalSearchStatus.ExactHit;
 1058            }
 511059            if (!requiresRotationalFallback)
 1060                continue;
 1061
 471062            Fixed64 entryTime = entryDistance / relativeLength;
 471063            Fixed64 exitTime = exitDistance / relativeLength;
 1064
 471065            bool searchFound =
 471066                TryFindEarliestMixedRotationalContinuousCollisionAgainstTarget(
 471067                    target.Collider,
 471068                    sourceSegmentStart,
 471069                    sourceSegmentDisplacement,
 471070                    sourceStartRotation,
 471071                    sourceEndRotation,
 471072                    angularDistance,
 471073                    sourceRadius,
 471074                    intervalElapsedTime,
 471075                    intervalDuration,
 471076                    IsKinematic,
 471077                    entryTime,
 471078                    exitTime,
 471079                    needsResponseContact: true,
 471080                    out Fixed64 safeTime,
 471081                    out MixedContact contact,
 471082                    out bool hasContact,
 471083                    out Fixed64 contactTime);
 471084            if (!searchFound)
 1085            {
 1086                continue;
 1087            }
 1088
 451089            bool closingResolved = Vector3d.TryDot(
 451090                relativeDisplacement,
 451091                contact.Normal3DTo2D,
 451092                out Fixed64 contactClosingSpeed);
 451093            bool hasOnlyTranslationalMotion =
 451094                angularDistance <= Fixed64.Epsilon
 451095                & segment.AngularDistance <= Fixed64.Epsilon;
 451096            bool useContactClosingSpeed = hasContact & hasOnlyTranslationalMotion;
 451097            if (useContactClosingSpeed
 451098                & (!closingResolved
 451099                    | contactClosingSpeed <= Fixed64.Epsilon))
 1100            {
 1101                continue;
 1102            }
 451103            Fixed64 candidateClosingSpeed =
 451104                (useContactClosingSpeed
 451105                    ? contactClosingSpeed
 451106                    : relativeLength)
 451107                / (sourceEndTime - sourceStartTime);
 1108
 451109            Fixed64 safeSourceTime = FixedMath.Lerp(
 451110                sourceStartTime,
 451111                sourceEndTime,
 451112                safeTime);
 451113            bool exactHit = hasContact & contactTime == safeTime;
 451114            PhysicsMixedHit exact = exactHit
 451115                ? new PhysicsMixedHit(
 451116                    null,
 451117                    target.Collider,
 451118                    contact.Anchor3D,
 451119                    contact.Anchor2D,
 451120                    contact.Normal3DTo2D,
 451121                    PhysicsQueryReducerKind.ConservativeFallback,
 451122                    sourceLength * safeSourceTime,
 451123                    sourceDirection)
 451124                : default;
 451125            IntervalSearchStatus status = exactHit
 451126                ? IntervalSearchStatus.ExactHit
 451127                : IntervalSearchStatus.Unresolved;
 451128            hit = new DynamicMixedIntervalHit(
 451129                status,
 451130                exact,
 451131                sourceLength * safeSourceTime,
 451132                candidateClosingSpeed,
 451133                target.Collider.Id);
 451134            return status;
 1135        }
 1136
 81137        return IntervalSearchStatus.CertifiedNoHit;
 1138    }
 1139
 1140}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.ContinuousCollision.Kinematic.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody.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 spatial motion against static geometry and transfers pushes to dynamic targets.</content>
 18public partial class SolidBody
 19{
 20    private bool TryResolveKinematicContinuousCollision(Vector3d startPosition, ref Vector3d proposedPosition)
 21    {
 635222        if (!ShouldUseContinuousCollision(out ContinuousCollisionMode mode))
 4723            return false;
 24
 630525        Vector3d displacement = ContinuousCollisionSweepRange.ValidateEndpoint(
 630526            startPosition,
 630527            proposedPosition,
 630528            out Fixed64 sourceLength);
 630529        Fixed64 displacementMagnitudeSquared = displacement.MagnitudeSquared;
 630530        if (displacementMagnitudeSquared <= Fixed64.Epsilon)
 531            return false;
 32
 630033        Fixed64 proxyRadius = ResolveContinuousCollisionProxyRadius();
 630034        if (proxyRadius <= Fixed64.Epsilon
 630035            || (mode == ContinuousCollisionMode.Auto
 630036                && ContinuousCollisionMath.IsWithinProxyRadius(displacement, displacementMagnitudeSquared, proxyRadius))
 37        {
 338            return false;
 39        }
 40
 629741        bool foundStatic = TryGetFirstKinematicStaticContinuousCollisionHit(
 629742            startPosition,
 629743            proposedPosition,
 629744            proxyRadius,
 629745            out Fixed64 staticHitDistance);
 629746        Fixed64 dynamicPushLimit = foundStatic ? staticHitDistance : sourceLength;
 629747        bool pushedDynamic = TryApplyKinematicDynamicContinuousCollisionPushes(
 629748            startPosition,
 629749            proposedPosition,
 629750            proxyRadius,
 629751            dynamicPushLimit,
 629752            sourceLength,
 629753            out bool hasUnresolvedMixedLimit,
 629754            out Fixed64 unresolvedMixedDistance);
 629755        staticHitDistance = FixedMath.Min(
 629756            dynamicPushLimit,
 629757            unresolvedMixedDistance);
 629758        foundStatic |= hasUnresolvedMixedLimit;
 629759        if (!foundStatic && !pushedDynamic)
 7560            return false;
 61
 622262        LastContinuousCollisionToiIterationCount++;
 622263        if (foundStatic)
 621164            proposedPosition = new FixedSegment(startPosition, proposedPosition)
 621165                .GetPointAtDistance(staticHitDistance, sourceLength);
 66
 622267        return true;
 68    }
 69
 70    private bool TryGetFirstKinematicStaticContinuousCollisionHit(
 71        Vector3d startPosition,
 72        Vector3d proposedPosition,
 73        Fixed64 proxyRadius,
 74        out Fixed64 distance)
 75    {
 629776        Vector3d originalPosition = Position3d;
 629777        bool originalPositionMutated = _positionMutated;
 78        try
 79        {
 629780            Position3d = startPosition;
 629781            Collider.RebuildRuntimeShapeOnly(refreshMassProperties: false);
 82
 629783            int hitCount = QueryStaticContinuousCollisionHits(
 629784                startPosition,
 629785                proposedPosition,
 629786                proxyRadius,
 629787                out bool staticHitsAreShapeExact);
 629788            int mixedHitCount = Context.Settings.RuntimeMode.RunsMixedContacts()
 629789                ? Context.QueryMixed.SweepSphereAgainstStatic2DAll(
 629790                    startPosition,
 629791                    proposedPosition,
 629792                    proxyRadius,
 629793                    PhysicsLayerMask.All,
 629794                    _continuousMixedCollisionHits,
 629795                    Collider,
 629796                    includeTriggers: false,
 629797                    cacheTargetPartitions: true,
 629798                    requireClosingContact: true)
 629799                : 0;
 100
 6297101            bool found3D = TryGetFirstValidContinuousCollisionHit(
 6297102                startPosition,
 6297103                proposedPosition,
 6297104                hitCount,
 6297105                staticHitsAreShapeExact,
 6297106                out Physics3DHit hit3D);
 6297107            bool foundMixed = TryGetFirstValidMixedContinuousCollisionHit(startPosition, proposedPosition, mixedHitCount
 6297108            if (found3D
 6297109                && (!foundMixed
 6297110                    || !ContinuousCollisionCandidateOrdering.Is2DHitFirst(
 6297111                        hitMixed.Distance,
 6297112                        hit3D.Distance)))
 113            {
 6206114                distance = hit3D.Distance;
 6206115                return true;
 116            }
 117
 91118            if (foundMixed)
 119            {
 2120                distance = hitMixed.Distance;
 2121                return true;
 122            }
 89123        }
 124        finally
 125        {
 6297126            Position3d = originalPosition;
 6297127            Collider.RebuildRuntimeShapeOnly(refreshMassProperties: false);
 6297128            _positionMutated = originalPositionMutated;
 6297129        }
 130
 89131        distance = Fixed64.Zero;
 89132        return false;
 6208133    }
 134
 135    private bool TryApplyKinematicDynamicContinuousCollisionPushes(
 136        Vector3d startPosition,
 137        Vector3d proposedPosition,
 138        Fixed64 proxyRadius,
 139        Fixed64 maxDistance,
 140        Fixed64 sourceLength,
 141        out bool hasUnresolvedMixedLimit,
 142        out Fixed64 unresolvedMixedDistance)
 143    {
 6297144        hasUnresolvedMixedLimit = false;
 6297145        unresolvedMixedDistance = maxDistance;
 6297146        _continuousCollisionHits.FastClear();
 6297147        _continuousMixedCollisionHits.FastClear();
 6297148        GatherKinematicDynamic3DContinuousCollisionHits(
 6297149            startPosition,
 6297150            proposedPosition,
 6297151            proxyRadius,
 6297152            maxDistance,
 6297153            sourceLength);
 6297154        GatherKinematicDynamicMixed2DContinuousCollisionHits(
 6297155            startPosition,
 6297156            proposedPosition,
 6297157            proxyRadius,
 6297158            maxDistance,
 6297159            sourceLength,
 6297160            ref hasUnresolvedMixedLimit,
 6297161            ref unresolvedMixedDistance);
 162
 6297163        Physics3DHitSorter.SortByDistance(_continuousCollisionHits);
 6297164        PhysicsMixedHitSorter.SortByDistance(_continuousMixedCollisionHits);
 6297165        Vector3d sourceDisplacement = proposedPosition - startPosition;
 6297166        int hit3DIndex = 0;
 6297167        int hit2DIndex = 0;
 6297168        bool pushed = false;
 6314169        while (hit3DIndex < _continuousCollisionHits.Count
 6314170            || hit2DIndex < _continuousMixedCollisionHits.Count)
 171        {
 17172            bool take2D = hit3DIndex >= _continuousCollisionHits.Count
 17173                || (hit2DIndex < _continuousMixedCollisionHits.Count
 17174                    && _continuousMixedCollisionHits[hit2DIndex].Distance
 17175                        <= _continuousCollisionHits[hit3DIndex].Distance);
 17176            if (take2D)
 177            {
 7178                PhysicsMixedHit hit = _continuousMixedCollisionHits[hit2DIndex++];
 7179                pushed |= ApplyKinematicContinuousCollisionHandoff(
 7180                    hit.Body2D!,
 7181                    sourceDisplacement,
 7182                    hit.NormalFor3DSource,
 7183                    hit.Distance,
 7184                    sourceLength);
 185
 7186                continue;
 187            }
 188
 10189            Physics3DHit hit3D = _continuousCollisionHits[hit3DIndex++];
 10190            pushed |= ApplyKinematicContinuousCollisionHandoff(
 10191                hit3D.Body!,
 10192                sourceDisplacement,
 10193                hit3D.Normal,
 10194                hit3D.Distance,
 10195                sourceLength);
 196        }
 197
 6297198        return pushed;
 199    }
 200
 201    private void GatherKinematicDynamic3DContinuousCollisionHits(
 202        Vector3d startPosition,
 203        Vector3d proposedPosition,
 204        Fixed64 proxyRadius,
 205        Fixed64 maxDistance,
 206        Fixed64 sourceLength)
 207    {
 6297208        Vector3d sourceDisplacement = proposedPosition - startPosition;
 6297209        long token = Context.LateSimulateToken;
 6297210        SwiftList<int> candidateIds = Context.Physics.QueryContinuousCollisionCandidates(
 6297211            DynamicCcdCandidateIndex.CreateSweptSphereBounds(startPosition, sourceDisplacement, proxyRadius));
 25218212        for (int candidateIndex = 0; candidateIndex < candidateIds.Count; candidateIndex++)
 213        {
 6312214            int dynamicId = candidateIds[candidateIndex];
 6312215            SolidBody target = Context.Physics.GetContinuousCollisionCandidate(dynamicId);
 6312216            if (!IsEligibleDynamicContinuousCollisionTarget(target))
 217            {
 218                continue;
 219            }
 220
 14221            target.EnsureContinuousCollisionFramePrepared(token);
 14222            if (!TryGetDynamicRelativeContinuousCollisionHit(
 14223                    target,
 14224                    startPosition,
 14225                    sourceDisplacement,
 14226                    proxyRadius,
 14227                    sourceLength,
 14228                    Fixed64.Zero,
 14229                    out Physics3DHit hit,
 14230                    out _))
 231                continue;
 232
 11233            if (hit.Distance > maxDistance)
 234                continue;
 235
 10236            _continuousCollisionHits.Add(hit);
 237        }
 6297238    }
 239
 240    private void GatherKinematicDynamicMixed2DContinuousCollisionHits(
 241        Vector3d startPosition,
 242        Vector3d proposedPosition,
 243        Fixed64 proxyRadius,
 244        Fixed64 maxDistance,
 245        Fixed64 sourceLength,
 246        ref bool hasUnresolvedMixedLimit,
 247        ref Fixed64 unresolvedMixedDistance)
 248    {
 6297249        if (!Context.Settings.RuntimeMode.RunsMixedContacts())
 6278250            return;
 251
 19252        Vector3d sourceDisplacement = proposedPosition - startPosition;
 19253        long token = Context.LateSimulateToken;
 19254        SwiftList<int> candidateIds = Context.Physics2D.QueryMixedContinuousCollisionCandidates(
 19255            DynamicCcdCandidateIndex.CreateSweptSphereBounds(startPosition, sourceDisplacement, proxyRadius));
 68256        for (int candidateIndex = 0; candidateIndex < candidateIds.Count; candidateIndex++)
 257        {
 15258            int dynamicId = candidateIds[candidateIndex];
 15259            SolidBody2D target = Context.Physics2D.GetContinuousCollisionCandidate(dynamicId);
 15260            if (!IsEligibleDynamicMixed2DTarget(target))
 261            {
 262                continue;
 263            }
 264
 14265            target.EnsureContinuousCollisionFramePrepared(token);
 14266            Fixed64 targetRadius =
 14267                target.ResolveMixedContinuousCollisionProxyRadius();
 14268            IntervalSearchStatus status =
 14269                TryGetDynamicMixed2DContinuousCollisionHit(
 14270                    target,
 14271                    startPosition,
 14272                    sourceDisplacement,
 14273                    proxyRadius,
 14274                    targetRadius,
 14275                    sourceLength,
 14276                    Fixed64.Zero,
 14277                    out DynamicMixedIntervalHit candidate);
 14278            if (status
 14279                    == IntervalSearchStatus.CertifiedNoHit
 14280                || candidate.SafeDistance > maxDistance)
 281            {
 282                continue;
 283            }
 284
 10285            if (status
 10286                == IntervalSearchStatus.Unresolved)
 287            {
 3288                hasUnresolvedMixedLimit = true;
 3289                unresolvedMixedDistance = FixedMath.Min(
 3290                    unresolvedMixedDistance,
 3291                    candidate.SafeDistance);
 3292                continue;
 293            }
 294
 7295            _continuousMixedCollisionHits.Add(candidate.ExactHit);
 296        }
 19297    }
 298
 299    internal bool ApplyKinematicContinuousCollisionHandoff(
 300        SolidBody target,
 301        Vector3d sourceDisplacement,
 302        Vector3d normalForSource,
 303        Fixed64 hitDistance,
 304        Fixed64 sourceLength)
 305    {
 11306        _ = ContinuousCollisionImpulsePolicy.TryResolveSourceNormal(normalForSource, sourceDisplacement, out Vector3d no
 307
 11308        Fixed64 deltaTime = Context.DeltaTime;
 11309        Fixed64 constrainedInverseMass = target.GetConstrainedInverseMass(normal);
 11310        if (constrainedInverseMass <= Fixed64.Zero)
 1311            return false;
 312
 10313        Fixed64 hitTime = FixedMath.Clamp01(hitDistance / sourceLength);
 10314        Vector3d sourceVelocity = SampleContinuousCollisionLinearVelocity(hitTime);
 10315        Vector3d targetVelocity = target.SampleContinuousCollisionLinearVelocity(hitTime);
 10316        bool relativeVelocityResolved = Vector3d.TrySubtract(
 10317            sourceVelocity,
 10318            targetVelocity,
 10319            out Vector3d relativeVelocity);
 10320        Fixed64 normalVelocity = Vector3d.Dot(relativeVelocity, normal);
 10321        Fixed64 restitution = ResolveContinuousCollisionRestitution(target, -normalVelocity);
 10322        bool responseFactorResolved = Fixed64.TryAdd(
 10323            Fixed64.One,
 10324            restitution,
 10325            out Fixed64 responseFactor);
 10326        bool closingSpeedResolved = Fixed64.TrySubtract(
 10327            Fixed64.Zero,
 10328            normalVelocity,
 10329            out Fixed64 closingSpeed);
 10330        bool responseSpeedResolved = Fixed64.TryMultiplyDivide(
 10331            closingSpeed,
 10332            responseFactor,
 10333            Fixed64.One,
 10334            out Fixed64 responseSpeed);
 10335        Vector3d targetResponseNormal = target.ProjectLinearMotion(-normal);
 10336        bool targetDeltaResolved = ContinuousCollisionImpulsePolicy.TryResolveVelocityDelta(
 10337            targetResponseNormal,
 10338            responseSpeed,
 10339            target.EffectiveInverseMass,
 10340            constrainedInverseMass,
 10341            out Vector3d targetVelocityDelta);
 10342        bool targetVelocityResolved = Vector3d.TryAdd(
 10343            targetVelocity,
 10344            target.ProjectLinearMotion(targetVelocityDelta),
 10345            out Vector3d targetPostVelocity);
 10346        if (!(relativeVelocityResolved
 10347            & normalVelocity < -Fixed64.Epsilon
 10348            & responseFactorResolved
 10349            & closingSpeedResolved
 10350            & responseSpeedResolved
 10351            & targetDeltaResolved
 10352            & targetVelocityResolved))
 353        {
 1354            return false;
 355        }
 356
 9357        return target.ApplyContinuousCollisionHandoff(
 9358            target.SampleContinuousCollisionPosition(hitTime),
 9359            target.SampleContinuousCollisionRotation(hitTime),
 9360            targetPostVelocity,
 9361            target.SampleContinuousCollisionAngularVelocity(hitTime),
 9362            deltaTime * (Fixed64.One - hitTime),
 9363            ignoredCollider3D: Collider);
 364    }
 365
 366    internal bool ApplyKinematicContinuousCollisionHandoff(
 367        SolidBody2D target,
 368        Vector3d sourceDisplacement,
 369        Vector3d normalForSource,
 370        Fixed64 hitDistance,
 371        Fixed64 sourceLength)
 372    {
 9373        if (!ContinuousCollisionImpulsePolicy.TryResolveImpactNormal(
 9374                normalForSource,
 9375                out Vector3d normal))
 376        {
 1377            return false;
 378        }
 379
 8380        Fixed64 deltaTime = Context.DeltaTime;
 8381        Fixed64 inverseMass = target.EffectiveInverseMass;
 382
 8383        Fixed64 hitTime = FixedMath.Clamp01(hitDistance / sourceLength);
 8384        Vector3d sourceVelocity = SampleContinuousCollisionLinearVelocity(hitTime);
 8385        Vector2d targetLinearVelocity = target.SampleContinuousCollisionLinearVelocity(hitTime);
 8386        Vector3d targetVelocity = targetLinearVelocity.ToVector3d(Fixed64.Zero);
 8387        bool relativeVelocityResolved = Vector3d.TrySubtract(
 8388            sourceVelocity,
 8389            targetVelocity,
 8390            out Vector3d relativeVelocity);
 391
 8392        Vector2d planarNormal = normal.ToVector2d();
 8393        Fixed64 constrainedInverseMass = target.GetConstrainedInverseMass(planarNormal) * planarNormal.MagnitudeSquared;
 8394        if (constrainedInverseMass <= Fixed64.Zero)
 1395            return false;
 396
 7397        Fixed64 normalVelocity = Vector3d.Dot(relativeVelocity, normal);
 7398        Fixed64 restitution = ResolveContinuousCollisionRestitution(target, -normalVelocity);
 7399        bool responseFactorResolved = Fixed64.TryAdd(
 7400            Fixed64.One,
 7401            restitution,
 7402            out Fixed64 responseFactor);
 7403        bool closingSpeedResolved = Fixed64.TrySubtract(
 7404            Fixed64.Zero,
 7405            normalVelocity,
 7406            out Fixed64 closingSpeed);
 7407        bool responseSpeedResolved = Fixed64.TryMultiplyDivide(
 7408            closingSpeed,
 7409            responseFactor,
 7410            Fixed64.One,
 7411            out Fixed64 responseSpeed);
 7412        Vector2d targetResponseNormal = target.ProjectLinearMotion(-planarNormal);
 7413        bool targetDeltaResolved = ContinuousCollisionImpulsePolicy.TryResolveVelocityDelta(
 7414            targetResponseNormal,
 7415            responseSpeed,
 7416            inverseMass,
 7417            constrainedInverseMass,
 7418            out Vector2d targetVelocityDelta);
 7419        bool targetVelocityResolved = Vector2d.TryAdd(
 7420            targetLinearVelocity,
 7421            target.ProjectLinearMotion(targetVelocityDelta),
 7422            out Vector2d targetPostVelocity);
 7423        if (!(relativeVelocityResolved
 7424            & normalVelocity < -Fixed64.Epsilon
 7425            & responseFactorResolved
 7426            & closingSpeedResolved
 7427            & responseSpeedResolved
 7428            & targetDeltaResolved
 7429            & targetVelocityResolved))
 430        {
 2431            return false;
 432        }
 433
 5434        return target.ApplyContinuousCollisionHandoffState(
 5435            target.SampleContinuousCollisionPosition(hitTime),
 5436            target.SampleContinuousCollisionRotation(hitTime),
 5437            targetPostVelocity,
 5438            target.SampleContinuousCollisionAngularVelocity(hitTime),
 5439            deltaTime * (Fixed64.One - hitTime),
 5440            ignoredCollider3D: Collider);
 441    }
 442
 443}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.ContinuousCollision.Rotational.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody.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 FixedMathSharp;
 9using FixedMathSharp.Geometry;
 10using Gravitas.Colliders;
 11using Gravitas.CollisionHandling;
 12
 13namespace Gravitas;
 14
 15/// <content>Samples rotational contact manifolds and resolves dynamic and kinematic spatial motion at contact.</content
 16public partial class SolidBody
 17{
 18    /// <summary>Gets whether body rotation leaves this collider's occupied geometry unchanged.</summary>
 19    internal bool HasRotationInvariantCollider =>
 926720        Collider is LSSphereCollider && Collider.LocalOffset == Vector3d.Zero;
 21
 22    private bool TryResolveRotationalContinuousCollision(
 23        Vector3d startPosition,
 24        ref Vector3d proposedPosition,
 25        FixedQuaternion startRotation,
 26        ref FixedQuaternion proposedRotation) =>
 7406927        TryResolveRotationalContinuousCollision(
 7406928            startPosition,
 7406929            ref proposedPosition,
 7406930            startRotation,
 7406931            ref proposedRotation,
 7406932            Context.DeltaTime,
 7406933            Fixed64.Zero,
 7406934            forceContinuous: false);
 35
 36    private bool TryResolveRotationalContinuousCollision(
 37        Vector3d startPosition,
 38        ref Vector3d proposedPosition,
 39        FixedQuaternion startRotation,
 40        ref FixedQuaternion proposedRotation,
 41        Fixed64 initialRemainingTime,
 42        Fixed64 initialElapsedTime,
 43        bool forceContinuous)
 44    {
 7422945        ContinuousCollisionMode mode = ContinuousCollisionMode.Continuous;
 7422946        if (!forceContinuous && !ShouldUseContinuousCollision(out mode))
 7299947            return false;
 48
 123049        Fixed64 pivotRadius = ResolveContinuousCollisionProxyRadius();
 123050        Vector3d initialDisplacement = proposedPosition - startPosition;
 51        // A centered sphere turns without sweeping new geometry. Keep target
 52        // rotation admission below: another body's rotation can still hit it.
 123053        Fixed64 angularDistance = HasRotationInvariantCollider
 123054            ? Fixed64.Zero
 123055            : _angularVelocity.Magnitude * initialRemainingTime;
 123056        bool targetRequiresRotationalSampling = angularDistance <= Fixed64.Epsilon
 123057            && HasNearbyRotationalContinuousCollisionTarget(
 123058                startPosition,
 123059                initialDisplacement,
 123060                pivotRadius);
 123061        if (angularDistance <= Fixed64.Epsilon && !targetRequiresRotationalSampling)
 108062            return false;
 63
 15064        Fixed64 angularArcLength = angularDistance * pivotRadius;
 15065        if (pivotRadius <= Fixed64.Epsilon
 15066            || (!targetRequiresRotationalSampling
 15067                && angularArcLength <= Fixed64.Epsilon)
 15068            || (!targetRequiresRotationalSampling
 15069                && mode == ContinuousCollisionMode.Auto
 15070                && angularArcLength <= pivotRadius
 15071                && ContinuousCollisionMath.IsWithinProxyRadius(
 15072                    initialDisplacement,
 15073                    initialDisplacement.MagnitudeSquared,
 15074                    pivotRadius)))
 75        {
 676            return false;
 77        }
 78
 14479        Vector3d originalPosition = Position3d;
 14480        FixedQuaternion originalRotation = Rotation;
 14481        bool originalPositionMutated = _positionMutated;
 14482        bool originalRotationMutated = _rotationMutated;
 14483        LSCollider2D? originalIgnoredMixedTarget = _continuousCollisionHandoffIgnoredCollider2D;
 84        try
 85        {
 14486            Vector3d currentPosition = startPosition;
 14487            FixedQuaternion currentRotation = startRotation;
 14488            Fixed64 remainingTime = initialRemainingTime;
 14489            Fixed64 elapsedTime = initialElapsedTime;
 14490            Vector3d motionSegmentStartPosition = startPosition;
 14491            FixedQuaternion motionSegmentStartRotation = startRotation;
 14492            Fixed64 motionSegmentStartElapsedTime = initialElapsedTime;
 14493            Vector3d motionSegmentLinearVelocity = ProjectLinearMotion(_linearVelocity);
 14494            Vector3d motionSegmentAngularVelocity = _angularVelocity;
 14495            LSCollider? ignoredTarget = _continuousCollisionHandoffIgnoredCollider3D;
 14496            int maxIterations = Context.Settings.ContinuousCollisionMaxToiIterations;
 14497            int conservativeRefinementCount = 0;
 14498            bool reuseCandidates = false;
 14499            int arbiterIterationLimit = maxIterations
 144100                + ContinuousCollisionMath.RotationalIntervalMaxDepth;
 460101            for (int iteration = 0; iteration < arbiterIterationLimit; iteration++)
 102            {
 230103                Fixed64 segmentEndElapsedTime = elapsedTime + remainingTime;
 230104                Fixed64 segmentElapsedTime = segmentEndElapsedTime - motionSegmentStartElapsedTime;
 230105                Vector3d segmentEnd = AddPositionDisplacement(
 230106                    motionSegmentStartPosition, ProjectLinearMotion(motionSegmentLinearVelocity) * segmentElapsedTime);
 230107                Vector3d displacement = segmentEnd - currentPosition;
 230108                FixedQuaternion targetRotation = IntegrateAngularRotation(
 230109                    motionSegmentStartRotation,
 230110                    motionSegmentAngularVelocity,
 230111                    segmentElapsedTime);
 230112                angularDistance = HasRotationInvariantCollider
 230113                    ? Fixed64.Zero
 230114                    : motionSegmentAngularVelocity.Magnitude * remainingTime;
 230115                if (!reuseCandidates)
 226116                    GatherRotationalContinuousCollisionCandidates(
 226117                        currentPosition,
 226118                        segmentEnd,
 226119                        displacement,
 226120                        pivotRadius);
 230121                bool foundSameDimension = TryFindEarliestRotationalContinuousCollision(
 230122                        currentPosition,
 230123                        displacement,
 230124                        currentRotation,
 230125                        targetRotation,
 230126                        angularDistance,
 230127                        pivotRadius,
 230128                        elapsedTime,
 230129                        remainingTime,
 230130                        isKinematic: false,
 230131                        ignoredTarget,
 230132                        out Fixed64 safeTime,
 230133                        out ManifoldContact contact,
 230134                        out bool hasContact,
 230135                        out Fixed64 contactTime,
 230136                        out LSCollider? target);
 230137                bool foundMixed = TryFindEarliestMixedRotationalContinuousCollision(
 230138                    currentPosition,
 230139                    displacement,
 230140                    currentRotation,
 230141                    targetRotation,
 230142                    angularDistance,
 230143                    pivotRadius,
 230144                    elapsedTime,
 230145                    remainingTime,
 230146                    sourceIsKinematic: false,
 230147                    reuseCandidates,
 230148                    out RotationalMixed2DHit mixedHit);
 230149                reuseCandidates = false;
 150                // Both searches initialize their hit structures, so evaluating
 151                // the tie policy eagerly avoids encoding an artificial
 152                // short-circuit state in this deterministic arbitration rule.
 230153                bool useMixed = foundMixed
 230154                    & (!foundSameDimension | mixedHit.SafeTime <= safeTime);
 230155                if (!foundSameDimension && !foundMixed)
 156                {
 65157                    currentPosition = segmentEnd;
 65158                    currentRotation = targetRotation;
 65159                    break;
 160                }
 161
 165162                Fixed64 conservativeTime = useMixed ? mixedHit.SafeTime : safeTime;
 165163                Fixed64 witnessedTime = useMixed ? mixedHit.ContactTime : contactTime;
 165164                bool responseWitnessIsEarliest = witnessedTime == conservativeTime;
 165165                Fixed64 witnessedConsumedTime = remainingTime * witnessedTime;
 165166                bool appliedResponse = responseWitnessIsEarliest && (useMixed
 165167                    ? mixedHit.HasClosingContact
 165168                        && TryApplyMixedRotationalContinuousCollisionResponse(
 165169                            mixedHit.Target,
 165170                            mixedHit.Contact,
 165171                            mixedHit.ContactTime,
 165172                            currentPosition,
 165173                            displacement,
 165174                            currentRotation,
 165175                            targetRotation,
 165176                            elapsedTime,
 165177                            remainingTime,
 165178                            sourceIsKinematic: false,
 165179                            manifold: mixedHit.Manifold)
 165180                    : hasContact
 165181                        && target!.Body is SolidBody targetBody
 165182                        && IsMovingRotationalContinuousCollisionTarget(targetBody)
 165183                        && TryApplyRotationalContinuousCollisionResponse(
 165184                            targetBody,
 165185                            contact,
 165186                            contactTime,
 165187                            currentPosition,
 165188                            displacement,
 165189                            currentRotation,
 165190                            elapsedTime,
 165191                            remainingTime,
 165192                            sourceIsKinematic: false));
 193                // Stable trajectories and candidate ownership make a retained
 194                // witness rediscoverable or terminally clamped within this arbiter.
 165195                bool deferUnresolvedWitness = !responseWitnessIsEarliest
 165196                    && witnessedTime > conservativeTime
 165197                    && conservativeTime > Fixed64.Epsilon
 165198                    && conservativeRefinementCount
 165199                        < ContinuousCollisionMath.RotationalIntervalMaxDepth;
 165200                Fixed64 consumedTime = appliedResponse
 165201                    ? witnessedConsumedTime
 165202                    : remainingTime * conservativeTime;
 165203                Fixed64 impactElapsedTime = elapsedTime + consumedTime;
 165204                Fixed64 impactSegmentElapsedTime = impactElapsedTime
 165205                    - motionSegmentStartElapsedTime;
 165206                Vector3d impactPosition = motionSegmentStartPosition
 165207                    + motionSegmentLinearVelocity * impactSegmentElapsedTime;
 165208                FixedQuaternion impactRotation = IntegrateAngularRotation(
 165209                    motionSegmentStartRotation,
 165210                    motionSegmentAngularVelocity,
 165211                    impactSegmentElapsedTime);
 165212                if (appliedResponse)
 213                {
 71214                    elapsedTime = impactElapsedTime;
 71215                    remainingTime -= consumedTime;
 71216                    currentPosition = impactPosition;
 71217                    currentRotation = impactRotation;
 71218                    motionSegmentStartPosition = currentPosition;
 71219                    motionSegmentStartRotation = currentRotation;
 71220                    motionSegmentStartElapsedTime = elapsedTime;
 71221                    motionSegmentLinearVelocity = ProjectLinearMotion(_linearVelocity);
 71222                    motionSegmentAngularVelocity = _angularVelocity;
 71223                    LastContinuousCollisionToiIterationCount++;
 71224                    if (useMixed)
 225                    {
 60226                        _continuousCollisionHandoffIgnoredCollider2D = mixedHit.Target;
 60227                        ignoredTarget = null;
 228                    }
 229                    else
 230                    {
 11231                        ignoredTarget = target;
 11232                        _continuousCollisionHandoffIgnoredCollider2D = null;
 233                    }
 71234                    Fixed64 remainingMotionSquared = FixedMath.Max(
 71235                        _linearVelocity.MagnitudeSquared,
 71236                        _angularVelocity.MagnitudeSquared);
 71237                    if (remainingTime <= Fixed64.Epsilon
 71238                        || remainingMotionSquared <= Fixed64.Epsilon)
 239                    {
 240                        break;
 241                    }
 242
 71243                    if (LastContinuousCollisionToiIterationCount >= maxIterations)
 244                    {
 4245                        LastContinuousCollisionToiIterationLimitReached = true;
 4246                        break;
 247                    }
 248
 249                    continue;
 250                }
 251
 94252                currentPosition = impactPosition;
 94253                currentRotation = impactRotation;
 94254                if (deferUnresolvedWitness)
 255                {
 19256                    conservativeRefinementCount++;
 257                    // Pure rotation keeps the candidate proxy unchanged; responses regather.
 19258                    reuseCandidates = displacement == Vector3d.Zero;
 19259                    elapsedTime = impactElapsedTime;
 19260                    remainingTime -= consumedTime;
 19261                    continue;
 262                }
 263
 75264                LastContinuousCollisionToiIterationCount++;
 75265                bool unresolvedInterval = !responseWitnessIsEarliest
 75266                    || !(useMixed ? mixedHit.HasContact : hasContact)
 75267                    || (useMixed && !mixedHit.HasClosingContact);
 75268                if (unresolvedInterval)
 269                {
 270                    // A later witness cannot supply a response at this frontier.
 271                    // Retain momentum while the stationary tail bounds this frame.
 37272                    LastContinuousCollisionToiIterationLimitReached =
 37273                        remainingTime - consumedTime > Fixed64.Epsilon;
 37274                    if (LastContinuousCollisionToiIterationLimitReached)
 37275                        Context.Physics.ReportContinuousCollisionIterationLimit();
 276                }
 277                else
 278                {
 38279                    StopRotationalContinuousCollision(
 38280                        useMixed
 38281                            ? -mixedHit.Contact.Normal3DTo2D
 38282                            : ResolveSourceOutwardContactNormal(target!, contact));
 283                }
 75284                if (CanAppendContinuousCollisionSegment(impactElapsedTime))
 285                {
 74286                    _ = Context.Physics.TryReserveContinuousCollisionCandidateRefresh(this);
 74287                    AppendContinuousCollisionSegment(
 74288                        currentPosition,
 74289                        currentRotation,
 74290                        Vector3d.Zero,
 74291                        Vector3d.Zero,
 74292                        impactElapsedTime);
 74293                    Context.Physics.RefreshContinuousCollisionCandidate(this);
 294                }
 295                else
 296                {
 1297                    LastContinuousCollisionToiIterationLimitReached = true;
 298                }
 1299                break;
 300            }
 301
 144302            proposedPosition = currentPosition;
 144303            proposedRotation = currentRotation;
 144304            return true;
 305        }
 306        finally
 307        {
 144308            Position3d = originalPosition;
 144309            Rotation = originalRotation;
 144310            Collider.RebuildRuntimeShapeOnly(refreshMassProperties: false);
 144311            _positionMutated = originalPositionMutated;
 144312            _rotationMutated = originalRotationMutated;
 144313            _continuousCollisionHandoffIgnoredCollider2D = originalIgnoredMixedTarget;
 144314        }
 144315    }
 316
 317    private bool TryResolveKinematicRotationalContinuousCollision(
 318        Vector3d startPosition,
 319        ref Vector3d proposedPosition,
 320        FixedQuaternion startRotation,
 321        ref FixedQuaternion proposedRotation)
 322    {
 6508323        if (!ShouldUseContinuousCollision(out ContinuousCollisionMode mode))
 47324            return false;
 325
 6461326        Fixed64 pivotRadius = ResolveContinuousCollisionProxyRadius();
 6461327        Vector3d displacement = proposedPosition - startPosition;
 328        // Preserve the host's orientation, but do not treat an invariant sphere
 329        // as a rotating obstacle against its own tangential support contacts.
 6461330        Fixed64 angularDistance = HasRotationInvariantCollider
 6461331            ? Fixed64.Zero
 6461332            : ResolveKinematicAngularDistanceRadians(startRotation, proposedRotation);
 6461333        bool targetRequiresRotationalSampling = angularDistance <= Fixed64.Epsilon
 6461334            && HasNearbyRotationalContinuousCollisionTarget(
 6461335                startPosition,
 6461336                displacement,
 6461337                pivotRadius);
 6461338        if (angularDistance <= Fixed64.Epsilon && !targetRequiresRotationalSampling)
 339        {
 6302340            return false;
 341        }
 342
 159343        Fixed64 angularArcLength = angularDistance * pivotRadius;
 159344        if (pivotRadius <= Fixed64.Epsilon
 159345            || (!targetRequiresRotationalSampling
 159346                && angularArcLength <= Fixed64.Epsilon)
 159347            || (!targetRequiresRotationalSampling
 159348                && mode == ContinuousCollisionMode.Auto
 159349                && angularArcLength <= pivotRadius
 159350                && ContinuousCollisionMath.IsWithinProxyRadius(
 159351                    displacement,
 159352                    displacement.MagnitudeSquared,
 159353                    pivotRadius)))
 354        {
 3355            return false;
 356        }
 357
 156358        FixedQuaternion targetRotation = proposedRotation;
 156359        Vector3d originalPosition = Position3d;
 156360        FixedQuaternion originalRotation = Rotation;
 156361        bool originalPositionMutated = _positionMutated;
 156362        bool originalRotationMutated = _rotationMutated;
 156363        LSCollider2D? originalIgnoredMixedTarget = _continuousCollisionHandoffIgnoredCollider2D;
 364        try
 365        {
 156366            Vector3d currentPosition = startPosition;
 156367            FixedQuaternion currentRotation = startRotation;
 156368            Fixed64 elapsedTime = Fixed64.Zero;
 156369            Fixed64 remainingTime = Context.DeltaTime;
 156370            LSCollider? ignoredTarget = null;
 156371            int maxIterations = Context.Settings.ContinuousCollisionMaxToiIterations;
 156372            int conservativeRefinementCount = 0;
 156373            bool reuseCandidates = false;
 156374            int arbiterIterationLimit = maxIterations
 156375                + ContinuousCollisionMath.RotationalIntervalMaxDepth;
 684376            for (int iteration = 0; iteration < arbiterIterationLimit; iteration++)
 377            {
 342378                Vector3d segmentDisplacement = (proposedPosition - currentPosition);
 342379                FixedQuaternion segmentTargetRotation = targetRotation;
 342380                angularDistance = HasRotationInvariantCollider
 342381                    ? Fixed64.Zero
 342382                    : ResolveKinematicAngularDistanceRadians(currentRotation, segmentTargetRotation);
 342383                if (!reuseCandidates)
 240384                    GatherRotationalContinuousCollisionCandidates(
 240385                        currentPosition,
 240386                        proposedPosition,
 240387                        segmentDisplacement,
 240388                        pivotRadius);
 342389                bool foundSameDimension = TryFindEarliestRotationalContinuousCollision(
 342390                        currentPosition,
 342391                        segmentDisplacement,
 342392                        currentRotation,
 342393                        segmentTargetRotation,
 342394                        angularDistance,
 342395                        pivotRadius,
 342396                        elapsedTime,
 342397                        remainingTime,
 342398                        isKinematic: true,
 342399                        ignoredTarget,
 342400                        out Fixed64 safeTime,
 342401                        out ManifoldContact contact,
 342402                        out bool hasContact,
 342403                        out Fixed64 contactTime,
 342404                        out LSCollider? target);
 342405                bool foundMixed = TryFindEarliestMixedRotationalContinuousCollision(
 342406                    currentPosition,
 342407                    segmentDisplacement,
 342408                    currentRotation,
 342409                    segmentTargetRotation,
 342410                    angularDistance,
 342411                    pivotRadius,
 342412                    elapsedTime,
 342413                    remainingTime,
 342414                    sourceIsKinematic: true,
 342415                    reuseCandidates,
 342416                    out RotationalMixed2DHit mixedHit);
 342417                reuseCandidates = false;
 342418                bool useMixed = foundMixed
 342419                    && (!foundSameDimension || mixedHit.SafeTime <= safeTime);
 342420                if (!foundSameDimension && !foundMixed)
 421                {
 111422                    currentPosition = proposedPosition;
 111423                    currentRotation = targetRotation;
 111424                    break;
 425                }
 426
 231427                Fixed64 conservativeTime = useMixed ? mixedHit.SafeTime : safeTime;
 231428                Fixed64 witnessedTime = useMixed ? mixedHit.ContactTime : contactTime;
 231429                bool responseWitnessIsEarliest = witnessedTime == conservativeTime;
 231430                bool appliedResponse = responseWitnessIsEarliest && (useMixed
 231431                    ? mixedHit.HasClosingContact
 231432                        && TryApplyMixedRotationalContinuousCollisionResponse(
 231433                            mixedHit.Target,
 231434                            mixedHit.Contact,
 231435                            mixedHit.ContactTime,
 231436                            currentPosition,
 231437                            segmentDisplacement,
 231438                            currentRotation,
 231439                            segmentTargetRotation,
 231440                            elapsedTime,
 231441                            remainingTime,
 231442                            sourceIsKinematic: true,
 231443                            manifold: mixedHit.Manifold)
 231444                    : hasContact
 231445                        && target!.Body is SolidBody targetBody
 231446                        && !targetBody.IsKinematic
 231447                        && IsMovingRotationalContinuousCollisionTarget(targetBody)
 231448                        && TryApplyRotationalContinuousCollisionResponse(
 231449                            targetBody,
 231450                            contact,
 231451                            contactTime,
 231452                            currentPosition,
 231453                            segmentDisplacement,
 231454                            currentRotation,
 231455                            elapsedTime,
 231456                            remainingTime,
 231457                            sourceIsKinematic: true));
 231458                bool deferUnresolvedWitness = !responseWitnessIsEarliest
 231459                    && witnessedTime > conservativeTime
 231460                    && conservativeTime > Fixed64.Epsilon
 231461                    && conservativeRefinementCount
 231462                        < ContinuousCollisionMath.RotationalIntervalMaxDepth;
 231463                Fixed64 eventTime = appliedResponse ? witnessedTime : conservativeTime;
 231464                if (appliedResponse)
 465                {
 86466                    Fixed64 consumedTime = remainingTime * eventTime;
 86467                    elapsedTime += consumedTime;
 86468                    remainingTime -= consumedTime;
 86469                    currentPosition += segmentDisplacement * eventTime;
 86470                    currentRotation = FixedQuaternion.Slerp(
 86471                        currentRotation,
 86472                        segmentTargetRotation,
 86473                        eventTime).Normalized;
 86474                    LastContinuousCollisionToiIterationCount++;
 86475                    if (useMixed)
 476                    {
 44477                        _continuousCollisionHandoffIgnoredCollider2D = mixedHit.Target;
 44478                        ignoredTarget = null;
 479                    }
 480                    else
 481                    {
 42482                        ignoredTarget = target;
 42483                        _continuousCollisionHandoffIgnoredCollider2D = null;
 484                    }
 86485                    if (remainingTime <= Fixed64.Epsilon)
 486                        break;
 86487                    if (LastContinuousCollisionToiIterationCount >= maxIterations)
 488                    {
 3489                        LastContinuousCollisionToiIterationLimitReached = true;
 3490                        currentPosition = proposedPosition;
 3491                        currentRotation = targetRotation;
 3492                        break;
 493                    }
 494
 495                    continue;
 496                }
 497
 145498                currentPosition += segmentDisplacement * eventTime;
 145499                currentRotation = FixedQuaternion.Slerp(
 145500                    currentRotation,
 145501                    segmentTargetRotation,
 145502                    eventTime).Normalized;
 145503                Fixed64 conservativeConsumedTime = remainingTime * eventTime;
 145504                Fixed64 impactElapsedTime = elapsedTime + conservativeConsumedTime;
 145505                if (deferUnresolvedWitness)
 506                {
 103507                    conservativeRefinementCount++;
 508                    // Pure rotation keeps the candidate proxy unchanged; responses regather.
 103509                    reuseCandidates = segmentDisplacement == Vector3d.Zero;
 103510                    elapsedTime = impactElapsedTime;
 103511                    remainingTime -= conservativeConsumedTime;
 103512                    continue;
 513                }
 514
 42515                LastContinuousCollisionToiIterationCount++;
 516                // Kinematic sources never receive handoff trajectory segments.
 517                // Their prepared authored segment is therefore the only retained
 518                // segment before this terminal clamp, so the validated positive
 519                // TOI budget always admits this single replacement tail.
 42520                bool unresolvedInterval = !responseWitnessIsEarliest
 42521                    || !(useMixed ? mixedHit.HasContact : hasContact)
 42522                    || (useMixed && !mixedHit.HasClosingContact);
 42523                if (unresolvedInterval)
 524                {
 18525                    LastContinuousCollisionToiIterationLimitReached =
 18526                        remainingTime - conservativeConsumedTime > Fixed64.Epsilon;
 18527                    if (LastContinuousCollisionToiIterationLimitReached)
 18528                        Context.Physics.ReportContinuousCollisionIterationLimit();
 529                }
 42530                _ = Context.Physics.TryReserveContinuousCollisionCandidateRefresh(this);
 42531                AppendContinuousCollisionSegment(
 42532                    currentPosition,
 42533                    currentRotation,
 42534                    Vector3d.Zero,
 42535                    Vector3d.Zero,
 42536                    impactElapsedTime);
 42537                Context.Physics.RefreshContinuousCollisionCandidate(this);
 42538                proposedPosition = currentPosition;
 42539                proposedRotation = currentRotation;
 42540                return true;
 541            }
 542
 114543            proposedPosition = currentPosition;
 114544            proposedRotation = currentRotation;
 114545            return true;
 546        }
 547        finally
 548        {
 156549            Position3d = originalPosition;
 156550            Rotation = originalRotation;
 156551            Collider.RebuildRuntimeShapeOnly(refreshMassProperties: false);
 156552            _positionMutated = originalPositionMutated;
 156553            _rotationMutated = originalRotationMutated;
 156554            _continuousCollisionHandoffIgnoredCollider2D = originalIgnoredMixedTarget;
 156555        }
 156556    }
 557
 558    private bool IsRotationalIntervalSeparated(LSCollider target, Fixed64 motionBound)
 559    {
 2329560        if (TryGetSpherePairSeparationGap(target, out Fixed64 closestPointGap))
 2107561            return closestPointGap > motionBound;
 562
 222563        return ContinuousCollisionBounds.AreSeparated(Collider, target, motionBound);
 564    }
 565
 566    private bool TryGetSpherePairSeparationGap(LSCollider target, out Fixed64 separationGap)
 567    {
 2329568        if (Collider is LSSphereCollider sourceSphere)
 167569            return TryGetSphereSeparationGap(sourceSphere, target, out separationGap);
 570
 2162571        if (target is LSSphereCollider targetSphere)
 1940572            return TryGetSphereSeparationGap(targetSphere, Collider, out separationGap);
 573
 222574        separationGap = default;
 222575        return false;
 576    }
 577
 578    internal static bool TryGetSphereSeparationGap(
 579        LSSphereCollider sphere,
 580        LSCollider other,
 581        out Fixed64 separationGap)
 582    {
 583        Vector3d separation;
 584        Fixed64 otherRadius;
 2118585        if (other is LSSphereCollider otherSphere)
 586        {
 6587            if (!Vector3d.TrySubtract(
 6588                    sphere.Center,
 6589                    otherSphere.Center,
 6590                    out separation))
 591            {
 1592                separationGap = default;
 1593                return false;
 594            }
 5595            otherRadius = otherSphere.ScaledRadius;
 596        }
 2112597        else if (other is LSCuboidCollider cuboid)
 598        {
 2111599            var sphereCenter = new FixedPointAnchor(
 2111600                sphere.Center,
 2111601                FixedQuaternion.Identity,
 2111602                Vector3d.Zero);
 2111603            if (!sphereCenter.TryGetOffsetFrom(
 2111604                    cuboid.OrientedBox.GetClosestPointAnchor(sphere.Center),
 2111605                    out separation))
 606            {
 1607                separationGap = default;
 1608                return false;
 609            }
 2110610            otherRadius = Fixed64.Zero;
 611        }
 612        else
 613        {
 1614            separationGap = default;
 1615            return false;
 616        }
 617
 2115618        if (!Vector3d.TryGetMagnitude(separation, out Fixed64 distance))
 619        {
 1620            separationGap = default;
 1621            return false;
 622        }
 623
 624        // Both committed sphere radii are admitted (at most 4096 each);
 625        // their sum stays representable independently of world separation.
 2114626        Fixed64 combinedRadius = sphere.ScaledRadius + otherRadius;
 2114627        Fixed64 rawGap = distance - combinedRadius;
 2114628        if (!Fixed64.TryAdd(distance, combinedRadius, out Fixed64 characteristicScale)
 2114629            || !Fixed64.TryAdd(characteristicScale, other.ScaledRadius, out characteristicScale)
 2114630            || !ContinuousCollisionMath.TrySubtractClosestFeatureUncertainty(
 2114631                rawGap,
 2114632                characteristicScale,
 2114633                out separationGap))
 634        {
 4635            separationGap = default;
 4636            return false;
 637        }
 638
 2110639        return separationGap > Fixed64.Zero;
 640    }
 641
 642    private void SampleDynamicRotationalContinuousPose(
 643        Vector3d startPosition,
 644        Vector3d displacement,
 645        FixedQuaternion startRotation,
 646        Fixed64 remainingTime,
 647        Fixed64 sampleTime)
 648    {
 1351649        Position3d = startPosition + displacement * sampleTime;
 1351650        Rotation = IntegrateAngularRotation(startRotation, remainingTime * sampleTime);
 1351651        Collider.RebuildRuntimeShapeOnly(refreshMassProperties: false);
 1351652    }
 653
 654    private void SampleKinematicRotationalContinuousPose(
 655        Vector3d startPosition,
 656        Vector3d displacement,
 657        FixedQuaternion startRotation,
 658        FixedQuaternion targetRotation,
 659        Fixed64 sampleTime)
 660    {
 2371661        Position3d = startPosition + displacement * sampleTime;
 2371662        Rotation = FixedQuaternion.Slerp(startRotation, targetRotation, sampleTime).Normalized;
 2371663        Collider.RebuildRuntimeShapeOnly(refreshMassProperties: false);
 2371664    }
 665
 666    private static Fixed64 ResolveKinematicAngularDistanceRadians(
 667        FixedQuaternion startRotation,
 668        FixedQuaternion proposedRotation)
 669    {
 7191670        Fixed64 angleDegrees = FixedQuaternion.Angle(startRotation, proposedRotation);
 7191671        return FixedMath.DegToRad(angleDegrees.Abs());
 672    }
 673
 674    private bool TrySampleRotationalContinuousCollision(
 675        LSCollider target,
 676        Fixed64 frameFraction,
 677        out ManifoldContact contact,
 678        out bool contactIsClosing)
 679    {
 3558680        contact = default;
 3558681        contactIsClosing = false;
 5802682        if (!TryBuildContinuousCollisionManifold(target)) return false;
 1314683        contactIsClosing = TrySelectRotationalClosingContact(target,
 1314684            _rotationalContinuousCollisionManifold, frameFraction, out contact);
 1314685        return true;
 686    }
 687
 688    private bool TryBuildContinuousCollisionManifold(LSCollider target)
 689    {
 4073690        OrderRotationalContinuousCollisionPair(
 4073691            target,
 4073692            out LSCollider colliderA,
 4073693            out LSCollider colliderB,
 4073694            out _);
 4073695        CollisionType collisionType = ColliderSettings.GetCollisionType(colliderA.Shape, colliderB.Shape);
 4073696        _rotationalContinuousCollisionManifold.BeginUpdate(Context.FrameCount);
 4073697        var workItem = new CollisionWorkItem(
 4073698            Context,
 4073699            colliderA,
 4073700            colliderB,
 4073701            collisionType,
 4073702            _rotationalContinuousCollisionManifold);
 4073703        return CollisionDetection.DoCollisionCheck(workItem)
 4073704            && _rotationalContinuousCollisionManifold.HasContact;
 705    }
 706
 707    internal bool TrySelectRotationalClosingContact(LSCollider target,
 708        ContactManifold manifold, Fixed64 frameFraction, out ManifoldContact contact)
 709    {
 1326710        contact = default;
 1326711        OrderRotationalContinuousCollisionPair(target, out _, out _, out bool sourceIsA);
 1326712        Vector3d sourceLinear = IsKinematic
 1326713            ? SampleContinuousCollisionLinearVelocity(frameFraction) : _linearVelocity;
 1326714        Vector3d sourceAngular = IsKinematic
 1326715            ? SampleContinuousCollisionAngularVelocity(frameFraction) : _angularVelocity;
 1326716        var sourceCenter = new ContactAnchor(Position3d, Rotation, _localCenterOfMassOffset);
 1326717        SolidBody? targetBody = target.Body;
 1326718        var targetCenter = targetBody == null
 1326719            ? ContactAnchor.FromWorldPoint(Vector3d.Zero)
 1326720            : new ContactAnchor(targetBody.Position3d, targetBody.Rotation,
 1326721                targetBody._localCenterOfMassOffset);
 1326722        Vector3d targetLinear = Vector3d.Zero;
 1326723        Vector3d targetAngular = Vector3d.Zero;
 1326724        if (targetBody != null && !targetBody.IsStatic)
 725        {
 770726            targetLinear = targetBody.SampleContinuousCollisionLinearVelocity(frameFraction);
 770727            targetAngular = targetBody.SampleContinuousCollisionAngularVelocity(frameFraction);
 728        }
 729
 4417730        foreach (ManifoldContact candidate in manifold)
 731        {
 1424732            ExactLever3D leverA = candidate.AnchorA.GetLeverFrom(
 1424733                sourceIsA ? sourceCenter : targetCenter);
 1424734            ExactLever3D leverB = candidate.AnchorB.GetLeverFrom(
 1424735                sourceIsA ? targetCenter : sourceCenter);
 1424736            ExactLever3D.GetRelativePointVelocityRatio(
 1424737                sourceIsA ? sourceLinear : targetLinear,
 1424738                sourceIsA ? sourceAngular : targetAngular, leverA,
 1424739                sourceIsA ? targetLinear : sourceLinear,
 1424740                sourceIsA ? targetAngular : sourceAngular, leverB, candidate.Normal,
 1424741                out Signed832 numerator, out Signed832 denominator);
 742            // Anchor lever denominators are positive. Exact sign preserves
 743            // closing speeds outside Q32.32; representable speeds use the same
 744            // deadband as the rotational handoff. Depth cannot rank an impact.
 1424745            if (numerator.Sign >= 0
 1424746                || (Fixed64.TryGetSignedRawRatio(numerator, denominator, 0,
 1424747                    out Fixed64 speed) && speed >= -Fixed64.Epsilon))
 748                continue;
 1083749            contact = candidate;
 1083750            return true;
 751        }
 243752        return false;
 1083753    }
 754
 755    private Vector3d ResolveSourceOutwardContactNormal(
 756        LSCollider target,
 757        ManifoldContact contact)
 758    {
 534759        OrderRotationalContinuousCollisionPair(
 534760            target,
 534761            out _,
 534762            out _,
 534763            out bool sourceIsA);
 534764        return sourceIsA ? -contact.Normal : contact.Normal;
 765    }
 766
 767    private bool IsMovingRotationalContinuousCollisionTarget(SolidBody target)
 768    {
 769        // Query/index admission has already validated an active, nonself target.
 593770        if (target.IsKinematic)
 139771            target.EnsureContinuousCollisionFramePrepared(Context.LateSimulateToken);
 772
 593773        return (IsKinematic || !target.ShouldOwnContinuousCollisionMovingPair(
 593774                HasContinuousCollisionRotationalMotion))
 593775            && !target.Collider.IsTrigger
 593776            && (target.IsKinematic
 593777                ? target.HasContinuousCollisionMotion
 593778                : target.IsDynamic)
 593779            && target.Collider != _continuousCollisionHandoffIgnoredCollider3D;
 780    }
 781
 782    private Fixed64 ResolveRotationalFrameFraction(
 783        Fixed64 elapsedTime,
 784        Fixed64 remainingTime,
 785        Fixed64 localFraction) =>
 23713786        FixedMath.Clamp01(
 23713787            (elapsedTime + remainingTime * localFraction) / Context.DeltaTime);
 788
 789    private void OrderRotationalContinuousCollisionPair(
 790        LSCollider target,
 791        out LSCollider colliderA,
 792        out LSCollider colliderB,
 793        out bool sourceIsA)
 794    {
 6470795        if (Collider.Priority >= target.Priority)
 796        {
 4299797            colliderA = Collider;
 4299798            colliderB = target;
 4299799            sourceIsA = true;
 4299800            return;
 801        }
 802
 2171803        colliderA = target;
 2171804        colliderB = Collider;
 2171805        sourceIsA = false;
 2171806    }
 807
 808    private FixedQuaternion IntegrateAngularRotation(FixedQuaternion startRotation, Fixed64 deltaTime)
 75420809        => IntegrateAngularRotation(startRotation, _angularVelocity, deltaTime);
 810
 811    private static FixedQuaternion IntegrateAngularRotation(
 812        FixedQuaternion startRotation,
 813        Vector3d angularVelocity,
 814        Fixed64 deltaTime)
 815    {
 148935816        FixedQuaternion angularVelocityQuaternion = new(
 148935817            angularVelocity.X,
 148935818            angularVelocity.Y,
 148935819            angularVelocity.Z,
 148935820            Fixed64.Zero);
 148935821        FixedQuaternion spin = angularVelocityQuaternion * startRotation * Fixed64.Half * deltaTime;
 148935822        return (startRotation + spin).Normalized;
 823    }
 824
 825}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.ContinuousCollision.Rotational.Mixed.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody.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.Queries;
 14using Gravitas.Support;
 15using SwiftCollections;
 16using SwiftCollections.Query;
 17
 18namespace Gravitas;
 19
 20/// <content>Searches rotating spatial trajectories against planar targets and resolves mixed contact manifolds.</conten
 21public partial class SolidBody
 22{
 840323    private readonly SwiftList<LSCollider2D> _rotationalMixedStatic2DCandidates =
 840324        new(DefaultBodyHitBufferCapacity);
 840325    private readonly SwiftList<int> _rotationalMixedMoving2DCandidateIds =
 840326        new(DefaultBodyHitBufferCapacity);
 840327    private readonly ContactManifold _rotationalMixedKnownManifold = new();
 840328    private readonly ContactManifold _rotationalMixedEarliestManifold = new();
 29    private bool _rotationalMixedKnownContactIsClosing;
 30    private bool _rotationalMixedEarliestContactIsClosing;
 31    private ContactWarmStartCache _mixedContinuousCollisionImpulses;
 32    private ExactContactImpulseState?[]? _mixedContinuousCollisionExactImpulses;
 33    private SwiftHashSet<StaticSurface2D>? _rotationalMixedSurfaceCandidates;
 34
 35    internal void SetMixedContinuousCollisionSamplePose(
 36        Vector3d position,
 37        FixedQuaternion rotation)
 38    {
 576039        _position2dUnmarked = position.ToVector2d();
 576040        _heightPosUnmarked = position.Y;
 576041        _rotation = rotation;
 576042        Collider.RebuildRuntimeShapeOnly(refreshMassProperties: false);
 576043    }
 44
 45    private readonly struct RotationalMixed2DHit
 46    {
 47        public RotationalMixed2DHit(
 48            Fixed64 safeTime,
 49            MixedContact contact,
 50            bool hasContact,
 51            bool hasClosingContact,
 52            Fixed64 contactTime,
 53            LSCollider2D target,
 54            ContactManifold manifold)
 55        {
 22056            SafeTime = safeTime;
 22057            Contact = contact;
 22058            HasContact = hasContact;
 22059            HasClosingContact = hasClosingContact;
 22060            ContactTime = contactTime;
 22061            Target = target;
 22062            Manifold = manifold;
 22063        }
 64
 65        public Fixed64 SafeTime { get; }
 66
 67        public MixedContact Contact { get; }
 68
 69        public bool HasContact { get; }
 70        public bool HasClosingContact { get; }
 71
 72        public Fixed64 ContactTime { get; }
 73
 74        public LSCollider2D Target { get; }
 75        public ContactManifold Manifold { get; }
 76    }
 77
 78    private bool TryFindEarliestMixedRotationalContinuousCollision(
 79        Vector3d startPosition,
 80        Vector3d displacement,
 81        FixedQuaternion startRotation,
 82        FixedQuaternion targetRotation,
 83        Fixed64 angularDistance,
 84        Fixed64 pivotRadius,
 85        Fixed64 elapsedTime,
 86        Fixed64 remainingTime,
 87        bool sourceIsKinematic,
 88        bool reuseCandidates,
 89        out RotationalMixed2DHit hit)
 90    {
 57291        _rotationalMixedSurfaceCandidates?.Clear();
 92        try
 93        {
 57294            hit = default;
 57295            if (!Context.Settings.RuntimeMode.RunsMixedContacts())
 27296                return false;
 97
 30098            if (!reuseCandidates)
 24299                GatherMixedRotationalContinuousCollisionCandidates(
 242100                    startPosition,
 242101                    displacement,
 242102                    pivotRadius);
 103
 300104            bool found = false;
 300105            Fixed64 earliestTime = Fixed64.One;
 300106            int earliestOrder = int.MaxValue;
 300107            MixedContact earliestContact = default;
 300108            bool earliestHasContact = false;
 300109            Fixed64 earliestContactTime = Fixed64.Zero;
 300110            LSCollider2D? earliestTarget = null;
 111
 1078112            for (int i = 0; i < _rotationalMixedStatic2DCandidates.Count; i++)
 113            {
 239114                LSCollider2D target = _rotationalMixedStatic2DCandidates[i];
 239115                if (target.StaticSurface is { } surface
 239116                    && (!IsValidMixedRotationalContinuousCollisionTarget(target)
 239117                        || !(_rotationalMixedSurfaceCandidates ??= new()).Add(surface))) continue;
 145118                ConsiderMixedRotationalContinuousCollisionCandidate(
 145119                    target,
 145120                    startPosition,
 145121                    displacement,
 145122                    startRotation,
 145123                    targetRotation,
 145124                    angularDistance,
 145125                    pivotRadius,
 145126                    elapsedTime,
 145127                    remainingTime,
 145128                    sourceIsKinematic,
 145129                    ref found,
 145130                    ref earliestTime,
 145131                    ref earliestOrder,
 145132                    ref earliestContact,
 145133                    ref earliestHasContact,
 145134                    ref earliestContactTime,
 145135                    ref earliestTarget);
 136            }
 137
 950138            for (int i = 0; i < _rotationalMixedMoving2DCandidateIds.Count; i++)
 139            {
 175140                SolidBody2D targetBody = Context.Physics2D.GetContinuousCollisionCandidate(
 175141                    _rotationalMixedMoving2DCandidateIds[i]);
 175142                if (!IsMovingMixedRotationalContinuousCollisionTarget(targetBody))
 143                {
 144                    continue;
 145                }
 146
 127147                ConsiderMixedRotationalContinuousCollisionCandidate(
 127148                    targetBody.Collider,
 127149                    startPosition,
 127150                    displacement,
 127151                    startRotation,
 127152                    targetRotation,
 127153                    angularDistance,
 127154                    pivotRadius,
 127155                    elapsedTime,
 127156                    remainingTime,
 127157                    sourceIsKinematic,
 127158                    ref found,
 127159                    ref earliestTime,
 127160                    ref earliestOrder,
 127161                    ref earliestContact,
 127162                    ref earliestHasContact,
 127163                    ref earliestContactTime,
 127164                    ref earliestTarget);
 165            }
 166
 300167            if (!found)
 80168                return false;
 169
 220170            hit = new RotationalMixed2DHit(
 220171                earliestTime,
 220172                earliestContact,
 220173                earliestHasContact,
 220174                _rotationalMixedEarliestContactIsClosing,
 220175                earliestContactTime,
 220176                earliestTarget!,
 220177                _rotationalMixedEarliestManifold);
 220178            return true;
 179        }
 1144180        finally { _rotationalMixedSurfaceCandidates?.Clear(); }
 572181    }
 182
 183    private void GatherMixedRotationalContinuousCollisionCandidates(
 184        Vector3d startPosition,
 185        Vector3d displacement,
 186        Fixed64 pivotRadius)
 187    {
 242188        _rotationalMixedStatic2DCandidates.FastClear();
 242189        _rotationalMixedMoving2DCandidateIds.FastClear();
 242190        FixedBoundVolume bounds = DynamicCcdCandidateIndex.CreateSweptSphereBounds(
 242191            startPosition,
 242192            displacement,
 242193            pivotRadius);
 242194        Context.MixedCollisions.Collect2DCandidatesInMixedBounds(
 242195            bounds.Min,
 242196            bounds.Max,
 242197            PhysicsLayerMask.All,
 242198            _rotationalMixedStatic2DCandidates,
 242199            staticStyleOnly: true,
 242200            cachePartitionRefresh: true);
 242201        _rotationalMixedStatic2DCandidates.SortInPlace(default(ReplayOrderComparer<LSCollider2D>));
 202
 242203        SwiftList<int> candidateIds =
 242204            Context.Physics2D.QueryMixedContinuousCollisionCandidates(bounds);
 242205        _rotationalMixedMoving2DCandidateIds.EnsureCapacity(candidateIds.Count);
 722206        for (int i = 0; i < candidateIds.Count; i++)
 119207            _rotationalMixedMoving2DCandidateIds.Add(candidateIds[i]);
 242208    }
 209
 210    private void ConsiderMixedRotationalContinuousCollisionCandidate(
 211        LSCollider2D target,
 212        Vector3d startPosition,
 213        Vector3d displacement,
 214        FixedQuaternion startRotation,
 215        FixedQuaternion targetRotation,
 216        Fixed64 angularDistance,
 217        Fixed64 pivotRadius,
 218        Fixed64 elapsedTime,
 219        Fixed64 remainingTime,
 220        bool sourceIsKinematic,
 221        ref bool found,
 222        ref Fixed64 earliestTime,
 223        ref int earliestOrder,
 224        ref MixedContact earliestContact,
 225        ref bool earliestHasContact,
 226        ref Fixed64 earliestContactTime,
 227        ref LSCollider2D? earliestTarget)
 228    {
 229        // Kinematic clamps against immovable targets consume geometry/time only;
 230        // dynamic stopping and movable-target response still require full contacts.
 272231        int candidateOrder = target.StaticSurface?.ContinuousOrder ?? ((IPhysicsColliderRegistryItem)target).ReplayOrder
 272232        if (!IsValidMixedRotationalContinuousCollisionTarget(target)
 272233            || !TryFindEarliestMixedRotationalContinuousCollisionAgainstTarget(
 272234                target,
 272235                startPosition,
 272236                displacement,
 272237                startRotation,
 272238                targetRotation,
 272239                angularDistance,
 272240                pivotRadius,
 272241                elapsedTime,
 272242                remainingTime,
 272243                sourceIsKinematic,
 272244                Fixed64.Zero,
 272245                Fixed64.One,
 272246                needsResponseContact: !IsKinematic || target.Body?.HasSolverMobility == true,
 272247                out Fixed64 candidateTime,
 272248                out MixedContact candidateContact,
 272249                out bool candidateHasContact,
 272250                out Fixed64 candidateContactTime)
 272251            || !ContinuousCollisionMath.ShouldReplaceContinuousCollisionHit(
 272252                candidateTime,
 272253                candidateOrder,
 272254                found,
 272255                earliestTime,
 272256                earliestOrder))
 257        {
 52258            return;
 259        }
 260
 220261        found = true;
 220262        earliestTime = candidateTime;
 220263        earliestOrder = candidateOrder;
 220264        earliestContact = candidateContact;
 220265        earliestHasContact = candidateHasContact;
 220266        earliestContactTime = candidateContactTime;
 220267        earliestTarget = target;
 220268        _rotationalMixedEarliestManifold.CopyFrom(_rotationalMixedKnownManifold, Context.FrameCount);
 220269        _rotationalMixedEarliestContactIsClosing = _rotationalMixedKnownContactIsClosing;
 220270    }
 271
 272    private bool TryGetInvariantMixedPlane(Vector3d initialNormal,
 273        Vector3d displacement, FixedQuaternion startRotation, FixedQuaternion targetRotation, bool isKinematic,
 274        out Vector3d outward, out RationalPoint3d plane)
 275    {
 88276        outward = default; plane = default;
 45277        if (Collider is not LSCapsuleCollider) return false;
 278        System.Diagnostics.Debug.Assert(initialNormal != Vector3d.Zero);
 43279        outward = -initialNormal;
 43280        if (!HasInvariantContinuousProjection(outward, displacement, startRotation, targetRotation, isKinematic))
 13281            return false;
 282        // The source is already sampled at T0. This exact minimum remains
 283        // unchanged by rotation and cannot move toward any member behind it.
 284        // Penetrating members never qualify for this separation certificate.
 30285        plane = RationalPoint3d.FromAnchor(new ConvexShape(Collider, Vector3d.Zero).GetSupportAnchor(-outward));
 30286        return true;
 287    }
 288
 289    private bool TryFindEarliestMixedRotationalContinuousCollisionAgainstTarget(
 290        LSCollider2D target,
 291        Vector3d startPosition,
 292        Vector3d displacement,
 293        FixedQuaternion startRotation,
 294        FixedQuaternion targetRotation,
 295        Fixed64 angularDistance,
 296        Fixed64 pivotRadius,
 297        Fixed64 elapsedTime,
 298        Fixed64 remainingTime,
 299        bool sourceIsKinematic,
 300        Fixed64 searchStart,
 301        Fixed64 searchEnd,
 302        bool needsResponseContact,
 303        out Fixed64 safeTime,
 304        out MixedContact contact,
 305        out bool hasContact,
 306        out Fixed64 contactTime)
 307    {
 303308        safeTime = Fixed64.Zero;
 303309        contact = default;
 303310        hasContact = false;
 303311        contactTime = Fixed64.Zero;
 303312        _rotationalMixedKnownManifold.BeginUpdate(Context.FrameCount);
 303313        _rotationalMixedKnownContactIsClosing = false;
 303314        StaticSurface2D? surface = target.StaticSurface;
 303315        Vector3d? separationOutward = null;
 303316        RationalPoint3d separationPlane = default;
 303317        SolidBody2D? targetBody = target.Body;
 303318        bool samplesTargetMotion = targetBody != null
 303319            && IsMovingMixedRotationalContinuousCollisionTarget(targetBody);
 303320        Vector2d originalTargetPosition = targetBody?.Position ?? Vector2d.Zero;
 303321        Fixed64 originalTargetRotation = targetBody?.Rotation ?? Fixed64.Zero;
 303322        if (samplesTargetMotion)
 172323            targetBody!.EnsureContinuousCollisionFramePrepared(Context.LateSimulateToken);
 324
 325        try
 326        {
 303327            SampleMixedRotationalContinuousPairPose(startPosition, displacement, startRotation, targetRotation,
 303328                searchStart, elapsedTime, remainingTime, sourceIsKinematic, targetBody, samplesTargetMotion);
 329            // A touching start can already supply a closing contact. Waiting
 330            // for the first midpoint would invent penetration before response.
 303331            if (needsResponseContact && (surface?.IntersectsMixedContinuous(Collider)
 303332                    ?? CollisionDetectionMixed.IntersectsForContinuousCollision(Collider, target)))
 333            {
 99334                BuildMixedRotationalContinuousContact(target,
 99335                    ResolveRotationalFrameFraction(elapsedTime, remainingTime, searchStart),
 99336                    out contact, out _rotationalMixedKnownContactIsClosing);
 99337                ContactManifold initial = Context.CollisionScratch.MixedManifold;
 99338                if (_rotationalMixedKnownContactIsClosing)
 339                {
 53340                    _rotationalMixedKnownManifold.CopyFrom(initial, Context.FrameCount);
 53341                    safeTime = contactTime = searchStart;
 53342                    hasContact = true;
 53343                    return true;
 344                }
 46345                if (surface != null && initial.HasContact
 46346                    && TryGetInvariantMixedPlane(initial.PrimaryContact.Normal, displacement, startRotation,
 76347                        targetRotation, sourceIsKinematic, out Vector3d axis, out separationPlane)) separationOutward = 
 348            }
 250349            Span<RotationalInterval> intervals =
 250350                stackalloc RotationalInterval[
 250351                    ContinuousCollisionMath.RotationalIntervalMaxDepth + 2];
 250352            int intervalCount = 1;
 250353            int processedNodeCount = 0;
 250354            bool hasKnownContact = false;
 250355            Fixed64 knownContactTime = Fixed64.One;
 250356            intervals[0] = new RotationalInterval(
 250357                searchStart,
 250358                searchEnd,
 250359                depth: 0);
 360
 5957361            while (intervalCount > 0)
 362            {
 5924363                RotationalInterval interval = intervals[--intervalCount];
 5924364                Fixed64 midpoint =
 5924365                    (interval.LowerTime + interval.UpperTime) * Fixed64.Half;
 5924366                Fixed64 intervalSpan = interval.UpperTime - interval.LowerTime;
 5924367                SampleMixedRotationalContinuousPairPose(
 5924368                    startPosition,
 5924369                    displacement,
 5924370                    startRotation,
 5924371                    targetRotation,
 5924372                    midpoint,
 5924373                    elapsedTime,
 5924374                    remainingTime,
 5924375                    sourceIsKinematic,
 5924376                    targetBody,
 5924377                    samplesTargetMotion);
 5924378                processedNodeCount++;
 379
 5924380                bool hasMotionBound =
 5924381                    ContinuousCollisionMath.TryResolveRotationalIntervalMotionBound(
 5924382                        displacement,
 5924383                        angularDistance,
 5924384                        pivotRadius,
 5924385                        intervalSpan,
 5924386                        out Fixed64 motionBound);
 5924387                if (hasMotionBound && samplesTargetMotion)
 388                {
 5057389                    Fixed64 lowerFraction = ResolveRotationalFrameFraction(
 5057390                        elapsedTime,
 5057391                        remainingTime,
 5057392                        interval.LowerTime);
 5057393                    Fixed64 upperFraction = ResolveRotationalFrameFraction(
 5057394                        elapsedTime,
 5057395                        remainingTime,
 5057396                        interval.UpperTime);
 5057397                    Fixed64 targetRadius = targetBody!.ResolveContinuousCollisionProxyRadius();
 5057398                    bool targetBoundResolved = targetBody.TryResolveContinuousCollisionMotionBound(
 5057399                        lowerFraction,
 5057400                        upperFraction,
 5057401                        targetRadius,
 5057402                        out Fixed64 targetMotionBound);
 5057403                    bool combinedBoundResolved = Fixed64.TryAdd(
 5057404                        motionBound,
 5057405                        targetMotionBound,
 5057406                        out motionBound);
 5057407                    hasMotionBound = targetBoundResolved & combinedBoundResolved;
 408                }
 409
 5924410                bool sampleHasContact = surface?.IntersectsMixedContinuous(Collider, separationOutward: separationOutwar
 5924411                    ?? CollisionDetectionMixed.IntersectsForContinuousCollision(Collider, target);
 5924412                if (sampleHasContact
 5924413                    && (!hasKnownContact || midpoint < knownContactTime))
 414                {
 1194415                    hasKnownContact = true;
 1194416                    knownContactTime = midpoint;
 417                }
 418
 5924419                Fixed64 shapeGap = default;
 5924420                bool shapeGapSupported = false;
 5924421                bool hasShapeGap = surface == null && CollisionDetectionMixed.TryGetRotationalSeparationGap(
 5924422                    Collider,
 5924423                    target,
 5924424                    out shapeGap,
 5924425                    out shapeGapSupported);
 5924426                Fixed64 characteristicScale = target is LSCircleCollider2D circle
 5924427                    ? FixedMath.Max(pivotRadius, circle.ScaledRadius)
 5924428                    : pivotRadius;
 5924429                bool shapeGapCertifiesSeparation = shapeGapSupported
 5924430                    && hasShapeGap
 5924431                    && ContinuousCollisionMath.TrySubtractClosestFeatureUncertainty(
 5924432                        shapeGap,
 5924433                        characteristicScale,
 5924434                        out Fixed64 conservativeShapeGap)
 5924435                    && conservativeShapeGap > motionBound;
 5924436                bool fallbackBoundsCertifySeparation = !sampleHasContact && hasMotionBound && !shapeGapSupported
 5924437                    && (surface != null ? !surface.IntersectsMixedContinuous(Collider, motionBound, separationOutward, s
 5924438                        : ContinuousCollisionBounds.AreSeparated(Collider, target, motionBound)
 5924439                            || !CollisionDetectionMixed.IntersectsForContinuousCollision(Collider, target, motionBound))
 5924440                if (!sampleHasContact
 5924441                    && hasMotionBound
 5924442                    && (shapeGapCertifiesSeparation || fallbackBoundsCertifySeparation))
 443                {
 444                    continue;
 445                }
 446
 3983447                if (ContinuousCollisionMath.TryResolveRotationalSearchLimit(
 3983448                        interval,
 3983449                        processedNodeCount,
 3983450                        hasKnownContact,
 3983451                        knownContactTime,
 3983452                        out safeTime,
 3983453                        out contactTime,
 3983454                        out hasContact))
 455                {
 456                    // A later witness cannot authorize response at an earlier unresolved frontier.
 217457                    if (hasContact && needsResponseContact && contactTime == safeTime)
 458                    {
 459                        // Reconstruct complete matched rows at the admitted response pose.
 75460                        SampleMixedRotationalContinuousPairPose(startPosition, displacement, startRotation, targetRotati
 75461                            elapsedTime, remainingTime, sourceIsKinematic, targetBody, samplesTargetMotion);
 75462                        BuildMixedRotationalContinuousContact(target,
 75463                            ResolveRotationalFrameFraction(elapsedTime, remainingTime, contactTime),
 75464                            out contact, out _rotationalMixedKnownContactIsClosing, separationOutward, separationPlane);
 75465                        _rotationalMixedKnownManifold.CopyFrom(Context.CollisionScratch.MixedManifold, Context.FrameCoun
 466                    }
 217467                    return true;
 468                }
 469
 3766470                int childDepth = interval.Depth + 1;
 3766471                if (sampleHasContact)
 472                {
 1161473                    intervals[intervalCount++] =
 1161474                        new RotationalInterval(
 1161475                            interval.LowerTime,
 1161476                            midpoint,
 1161477                            childDepth);
 1161478                    continue;
 479                }
 480
 2605481                intervals[intervalCount++] =
 2605482                    new RotationalInterval(
 2605483                        midpoint,
 2605484                        interval.UpperTime,
 2605485                        childDepth);
 2605486                intervals[intervalCount++] =
 2605487                    new RotationalInterval(
 2605488                        interval.LowerTime,
 2605489                        midpoint,
 2605490                        childDepth);
 491            }
 492
 33493            return false;
 494        }
 495        finally
 496        {
 303497            if (targetBody != null)
 181498                targetBody.SetMixedContinuousCollisionSamplePose(
 181499                    originalTargetPosition,
 181500                    originalTargetRotation);
 303501        }
 303502    }
 503
 504    private void SampleMixedRotationalContinuousPairPose(
 505        Vector3d startPosition,
 506        Vector3d displacement,
 507        FixedQuaternion startRotation,
 508        FixedQuaternion targetRotation,
 509        Fixed64 sampleTime,
 510        Fixed64 elapsedTime,
 511        Fixed64 remainingTime,
 512        bool sourceIsKinematic,
 513        SolidBody2D? targetBody,
 514        bool samplesTargetMotion)
 515    {
 6302516        Position3d = startPosition + displacement * sampleTime;
 6302517        Rotation = sourceIsKinematic
 6302518            ? FixedQuaternion.Slerp(startRotation, targetRotation, sampleTime).Normalized
 6302519            : IntegrateAngularRotation(
 6302520                startRotation,
 6302521                _angularVelocity,
 6302522                remainingTime * sampleTime);
 6302523        Collider.RebuildRuntimeShapeOnly(refreshMassProperties: false);
 524
 6302525        if (!samplesTargetMotion)
 1023526            return;
 527
 5279528        Fixed64 frameFraction = ResolveRotationalFrameFraction(
 5279529            elapsedTime,
 5279530            remainingTime,
 5279531            sampleTime);
 5279532        SolidBody2D movingTarget = targetBody!;
 5279533        movingTarget.SetMixedContinuousCollisionSamplePose(
 5279534            movingTarget.SampleContinuousCollisionPosition(frameFraction),
 5279535            movingTarget.SampleContinuousCollisionRotation(frameFraction));
 5279536    }
 537
 538    private bool TryGetExactSphereCircleTranslationalContact(
 539        SolidBody2D target,
 540        ContinuousCollisionMotionSegment2D targetSegment,
 541        FixedSegment2d targetPath,
 542        Vector2d targetDisplacement,
 543        Fixed64 targetTravelDistance,
 544        FixedSegment sourcePath,
 545        Vector3d sourceDisplacement,
 546        Fixed64 sourceTravelDistance,
 547        FixedQuaternion sourceStartRotation,
 548        Fixed64 sourceAngularDistance,
 549        out bool requiresRotationalFallback,
 550        out Fixed64 relativeContactDistance,
 551        out Fixed64 sourceContactDistance,
 552        out Fixed64 localClosingSpeed,
 553        out MixedContact contact)
 554    {
 79555        requiresRotationalFallback = true;
 79556        relativeContactDistance = default;
 79557        sourceContactDistance = default;
 79558        localClosingSpeed = default;
 79559        contact = default;
 79560        if (Collider is not LSSphereCollider sphere
 79561            || target.Collider is not LSCircleCollider2D circle
 79562            || (targetSegment.AngularDistance != Fixed64.Zero
 79563                && circle.ScaledLocalOffset != Vector2d.Zero)
 79564            || (sourceAngularDistance != Fixed64.Zero
 79565                && sphere.ScaledOffset != Vector3d.Zero))
 566        {
 47567            return false;
 568        }
 569
 32570        Vector3d originalSourcePosition = Position3d;
 32571        FixedQuaternion originalSourceRotation = Rotation;
 32572        Vector2d originalTargetPosition = target.Position;
 32573        Fixed64 originalTargetRotation = target.Rotation;
 574        try
 575        {
 32576            SetMixedContinuousCollisionSamplePose(
 32577                sourcePath.Start,
 32578                sourceStartRotation);
 32579            target.SetMixedContinuousCollisionSamplePose(
 32580                targetPath.Start,
 32581                targetSegment.StartRotation);
 32582            Vector3d sphereStart = sphere.Center;
 32583            Vector3d cylinderStart = new(
 32584                circle.Center.X,
 32585                circle.MixedSlabCenterY,
 32586                circle.Center.Y);
 32587            Vector3d cylinderDisplacement =
 32588                targetDisplacement.ToVector3d(Fixed64.Zero);
 32589            Vector3d relativeDisplacement =
 32590                ContinuousCollisionSweepRange.ValidateRelativeDisplacement(
 32591                    sourceDisplacement,
 32592                    cylinderDisplacement,
 32593                    out Fixed64 totalDistance);
 32594            if (!WideFiniteAxisIntersection
 32595                    .TryGetSphericallyExpandedFiniteCylinderDirectionFirstDistanceFromHalfAxisLength(
 32596                        sphereStart,
 32597                        relativeDisplacement,
 32598                        cylinderStart,
 32599                        Vector3d.Up,
 32600                        circle.MixedHalfThickness,
 32601                        circle.ScaledRadius,
 32602                        sphere.ScaledRadius,
 32603                        totalDistance,
 32604                        out Fixed64 contactDistance))
 605            {
 1606                requiresRotationalFallback = false;
 1607                return false;
 608            }
 609
 31610            relativeContactDistance = contactDistance;
 31611            _ = Fixed64.TryMultiplyDivide(
 31612                sourceTravelDistance,
 31613                contactDistance,
 31614                totalDistance,
 31615                out sourceContactDistance);
 31616            Fixed64 candidateDistance = contactDistance;
 66617            for (int attempt = 0; attempt < 2; attempt++)
 618            {
 33619                _ = Fixed64.TryMultiplyDivide(
 33620                    sourceTravelDistance,
 33621                    candidateDistance,
 33622                    totalDistance,
 33623                    out Fixed64 sampleSourceDistance);
 33624                _ = Fixed64.TryMultiplyDivide(
 33625                    targetTravelDistance,
 33626                    candidateDistance,
 33627                    totalDistance,
 33628                    out Fixed64 targetContactDistance);
 33629                SetMixedContinuousCollisionSamplePose(
 33630                    sourcePath.GetPointAtDistance(
 33631                        sampleSourceDistance,
 33632                        sourceTravelDistance),
 33633                    sourceStartRotation);
 33634                target.SetMixedContinuousCollisionSamplePose(
 33635                    targetPath.GetPointAtDistance(
 33636                        targetContactDistance,
 33637                        targetTravelDistance),
 33638                    targetSegment.StartRotation);
 33639                ContactManifold manifold = Context.CollisionScratch.MixedManifold;
 33640                if (CollisionDetectionMixed.TryBuildManifold(Collider, target.Collider, manifold))
 641                {
 642                    // This analytic owner admits only a single sphere/circle leaf pair.
 31643                    contact = MixedContact.FromManifoldContact(manifold.PrimaryContact);
 31644                    bool closingResolved = Vector3d.TryDot(
 31645                            relativeDisplacement,
 31646                            contact.Normal3DTo2D,
 31647                            out localClosingSpeed);
 31648                    if (closingResolved
 31649                        && localClosingSpeed > Fixed64.Epsilon)
 650                    {
 28651                        return true;
 652                    }
 653
 3654                    if (closingResolved)
 3655                        requiresRotationalFallback = false;
 656
 3657                    break;
 658                }
 659
 2660                if (candidateDistance >= totalDistance)
 661                    break;
 662
 2663                candidateDistance = FixedMath.Min(
 2664                    totalDistance,
 2665                    candidateDistance + Fixed64.MinIncrement);
 666            }
 667
 3668            relativeContactDistance = default;
 3669            sourceContactDistance = default;
 3670            localClosingSpeed = default;
 3671            contact = default;
 3672            return false;
 673        }
 674        finally
 675        {
 32676            SetMixedContinuousCollisionSamplePose(
 32677                originalSourcePosition,
 32678                originalSourceRotation);
 32679            target.SetMixedContinuousCollisionSamplePose(
 32680                originalTargetPosition,
 32681                originalTargetRotation);
 32682        }
 32683    }
 684
 685    private void BuildMixedRotationalContinuousContact(LSCollider2D target,
 686        Fixed64 frameFraction, out MixedContact contact, out bool contactIsClosing,
 687        Vector3d? separationOutward = null, RationalPoint3d separationPlane = default)
 688    {
 174689        ContactManifold manifold = Context.CollisionScratch.MixedManifold;
 174690        contact = default;
 174691        contactIsClosing = false;
 692        // Admission already retained this geometric witness; closing rows only authorize response.
 174693        if (target.StaticSurface is { } surface)
 694        {
 109695            manifold.BeginUpdate(Context.FrameCount);
 109696            surface.AppendMixedContinuousContacts(Collider, manifold, separationOutward, separationPlane);
 697        }
 65698        else CollisionDetectionMixed.TryBuildDiscreteManifold(Collider, target, manifold);
 174699        SolidBody2D? other = target.Body;
 174700        ContactAnchor center2D = other == null
 174701            ? ContactAnchor.FromWorldPoint(MixedEmbedded2DGeometry.GetCenter3D(target))
 174702            : new ContactAnchor(new Vector3d(other.Position.X, target.MixedSlabCenterY, other.Position.Y),
 174703                FixedQuaternion.FromAxisAngle(Vector3d.Up, -other.Rotation),
 174704                ExactContactLever2D.ToSpatial(other.LocalCenterOfMassOffset));
 174705        contactIsClosing = MixedContact.TrySelectClosingContact(manifold, GetCenterOfMassAnchor(),
 174706            IsKinematic ? SampleContinuousCollisionLinearVelocity(frameFraction) : _linearVelocity,
 174707            IsKinematic ? SampleContinuousCollisionAngularVelocity(frameFraction) : _angularVelocity,
 174708            center2D, other?.SampleContinuousCollisionLinearVelocity(frameFraction) ?? Vector2d.Zero,
 174709            other?.SampleContinuousCollisionAngularVelocity(frameFraction) ?? Fixed64.Zero, out contact, target);
 174710    }
 711
 712    private bool IsValidMixedRotationalContinuousCollisionTarget(LSCollider2D target)
 713    {
 866714        return target.IsActive
 866715            && !target.IsTrigger
 866716            && !ContinuousCollisionCandidateOrdering.IsIgnoredTarget(
 866717                target,
 866718                _continuousCollisionHandoffIgnoredCollider2D)
 866719            && Context.MixedCollisions.RequireCollisionPair(Collider, target);
 720    }
 721
 722    private bool IsMovingMixedRotationalContinuousCollisionTarget(SolidBody2D target)
 723    {
 464724        if (target.IsKinematic)
 151725            target.EnsureContinuousCollisionFramePrepared(Context.LateSimulateToken);
 726
 464727        return (IsKinematic || !target.ShouldOwnContinuousCollisionMovingPair(
 464728                HasContinuousCollisionRotationalMotion))
 464729            && IsValidMixedRotationalContinuousCollisionTarget(target.Collider)
 464730            && (target.IsKinematic
 464731                ? target.HasContinuousCollisionMotion
 464732                : target.IsDynamic);
 733    }
 734
 735    private bool TryApplyMixedTranslationalManifoldResponse(SolidBody2D target,
 736        Vector3d sourcePosition, Vector2d targetPosition, Fixed64 elapsedTime, Fixed64 remainingTime)
 737    {
 7738        Fixed64 fraction = ResolveContinuousCollisionFrameFraction(elapsedTime);
 7739        FixedQuaternion sourceRotation = SampleContinuousCollisionRotation(fraction);
 7740        Fixed64 targetRotation = target.SampleContinuousCollisionRotation(fraction);
 7741        Vector3d originalSourcePosition = Position3d;
 7742        FixedQuaternion originalSourceRotation = Rotation;
 7743        Vector2d originalTargetPosition = target.Position;
 7744        Fixed64 originalTargetRotation = target.Rotation;
 745        try
 746        {
 747            // Sample geometry only; velocity and trajectory changes remain atomic after the pure solve.
 7748            SetMixedContinuousCollisionSamplePose(sourcePosition, sourceRotation);
 7749            target.SetMixedContinuousCollisionSamplePose(targetPosition, targetRotation);
 7750            if (!CollisionDetectionMixed.TryBuildManifold(Collider, target.Collider, _rotationalMixedKnownManifold))
 1751                return false;
 6752        }
 753        finally
 754        {
 7755            SetMixedContinuousCollisionSamplePose(originalSourcePosition, originalSourceRotation);
 7756            target.SetMixedContinuousCollisionSamplePose(originalTargetPosition, originalTargetRotation);
 7757        }
 6758        return TryApplyMixedRotationalContinuousCollisionResponse(target.Collider,
 6759            MixedContact.FromManifoldContact(_rotationalMixedKnownManifold.PrimaryContact), Fixed64.Zero,
 6760            sourcePosition, Vector3d.Zero, sourceRotation, sourceRotation, elapsedTime, remainingTime,
 6761            IsKinematic, _rotationalMixedKnownManifold);
 1762    }
 763
 764    internal bool TryApplyMixedRotationalContinuousCollisionResponse(
 765        LSCollider2D targetCollider,
 766        MixedContact contact,
 767        Fixed64 localContactTime,
 768        Vector3d sourceSegmentStart,
 769        Vector3d sourceDisplacement,
 770        FixedQuaternion sourceSegmentStartRotation,
 771        FixedQuaternion sourceSegmentTargetRotation,
 772        Fixed64 elapsedTime,
 773        Fixed64 remainingTime,
 774        bool sourceIsKinematic,
 775        ContactManifold? manifold = null)
 776    {
 275777        Fixed64 frameFraction = ResolveRotationalFrameFraction(
 275778            elapsedTime,
 275779            remainingTime,
 275780            localContactTime);
 275781        Fixed64 consumedTime = remainingTime * localContactTime;
 275782        Fixed64 impactElapsedTime = elapsedTime + consumedTime;
 275783        Fixed64 remainingAfterImpact = remainingTime - consumedTime;
 275784        Vector3d sourcePosition = sourceSegmentStart
 275785            + sourceDisplacement * localContactTime;
 275786        FixedQuaternion sourceRotation = sourceIsKinematic
 275787            ? FixedQuaternion.Slerp(
 275788                sourceSegmentStartRotation,
 275789                sourceSegmentTargetRotation,
 275790                localContactTime).Normalized
 275791            : IntegrateAngularRotation(
 275792                sourceSegmentStartRotation,
 275793                _angularVelocity,
 275794                consumedTime);
 275795        Vector3d sourceLinearVelocity = sourceIsKinematic
 275796            ? SampleContinuousCollisionLinearVelocity(frameFraction)
 275797            : _linearVelocity;
 275798        Vector3d sourceAngularVelocity = sourceIsKinematic
 275799            ? SampleContinuousCollisionAngularVelocity(frameFraction)
 275800            : _angularVelocity;
 801
 275802        SolidBody2D? target = targetCollider.Body;
 275803        Vector2d targetPosition = target?.SampleContinuousCollisionPosition(frameFraction)
 275804            ?? targetCollider.Center;
 275805        Fixed64 targetRotation = target?.SampleContinuousCollisionRotation(frameFraction)
 275806            ?? Fixed64.Zero;
 275807        Vector2d targetLinearVelocity = target?.SampleContinuousCollisionLinearVelocity(frameFraction)
 275808            ?? Vector2d.Zero;
 275809        Fixed64 targetAngularVelocity = target?.SampleContinuousCollisionAngularVelocity(frameFraction)
 275810            ?? Fixed64.Zero;
 275811        Vector2d targetLocalCenterOfMassOffset =
 275812            target?.LocalCenterOfMassOffset ?? Vector2d.Zero;
 275813        SolidBody? sourceResponseBody = HasSolverMobility ? this : null;
 275814        SolidBody2D? targetResponseBody = target?.HasSolverMobility == true ? target : null;
 275815        var sourceCenter = new ContactAnchor(sourcePosition, sourceRotation, LocalCenterOfMassOffset);
 275816        Vector3d targetPosition3D = new(targetPosition.X, targetCollider.MixedSlabCenterY, targetPosition.Y);
 275817        var targetCenter = new ContactAnchor(targetPosition3D,
 275818            FixedQuaternion.FromAxisAngle(Vector3d.Up, -targetRotation),
 275819            ExactContactLever2D.ToSpatial(targetLocalCenterOfMassOffset));
 275820        if (manifold == null)
 821        {
 822            // Direct one-row callers retain the same response contract without publishing a runtime pair.
 7823            _rotationalMixedKnownManifold.BeginUpdate(Context.FrameCount);
 7824            _rotationalMixedKnownManifold.AddContact(default, contact.ToManifoldContact());
 7825            manifold = _rotationalMixedKnownManifold;
 826        }
 275827        if (!MixedContact.TrySelectClosingContact(manifold, sourceCenter, sourceLinearVelocity,
 275828                sourceAngularVelocity, targetCenter, targetLinearVelocity, targetAngularVelocity, out _, targetCollider)
 2829            return false;
 273830        var impact = new ContactResponseSnapshot(sourcePosition, targetPosition3D, sourceCenter, targetCenter,
 273831            sourceLinearVelocity, sourceAngularVelocity, ExactContactLever2D.ToSpatial(targetLinearVelocity),
 273832            new Vector3d(Fixed64.Zero, -targetAngularVelocity, Fixed64.Zero), targetAngularVelocity);
 273833        if (!CollisionResponseMixed.TrySolveManifoldVelocity(Collider, targetCollider, manifold, impact,
 273834                ref _mixedContinuousCollisionImpulses, ref _mixedContinuousCollisionExactImpulses,
 274835                out ContactResponseSnapshot solved)) return false;
 272836        Vector3d postSourceLinearVelocity = solved.LinearA;
 272837        Vector3d postSourceAngularVelocity = solved.AngularA;
 272838        Vector2d postTargetLinearVelocity = solved.LinearB.ToVector2d();
 272839        Fixed64 postTargetAngularVelocity = solved.PlanarAngularB;
 272840        bool sourceStateAvailable = sourceResponseBody == null
 272841            || CanAppendContinuousCollisionSegment(impactElapsedTime);
 272842        bool targetStateAvailable = targetResponseBody == null
 272843            || targetResponseBody.CanApplyContinuousCollisionHandoffState(
 272844                targetPosition, targetRotation, remainingAfterImpact, out targetPosition);
 272845        if (!(sourceStateAvailable & targetStateAvailable))
 846        {
 3847            return false;
 848        }
 849
 269850        if (sourceResponseBody != null)
 224851            ValidateContinuousCollisionPosition(sourcePosition, postSourceLinearVelocity, Context.DeltaTime - impactElap
 269852        if (targetResponseBody != null)
 48853            targetResponseBody.ValidateContinuousCollisionHandoffPosition(targetPosition, postTargetLinearVelocity, post
 854
 269855        if (sourceResponseBody != null)
 224856            _ = Context.Physics.TryReserveContinuousCollisionCandidateRefresh(this);
 857
 269858        if (targetResponseBody != null)
 859        {
 48860            _ = Context.Physics2D.TryReserveContinuousCollisionCandidateRefresh(targetResponseBody);
 48861            targetResponseBody.ApplyContinuousCollisionHandoffStateReserved(
 48862                targetPosition,
 48863                targetRotation,
 48864                postTargetLinearVelocity,
 48865                postTargetAngularVelocity,
 48866                remainingAfterImpact,
 48867                ignoredCollider3D: Collider);
 868        }
 869
 269870        if (sourceResponseBody != null)
 871        {
 224872            ApplyCollisionVelocityState(
 224873                postSourceLinearVelocity,
 224874                postSourceAngularVelocity);
 224875            AppendContinuousCollisionSegment(
 224876                sourcePosition,
 224877                sourceRotation,
 224878                _linearVelocity,
 224879                _angularVelocity,
 224880                impactElapsedTime);
 224881            Context.Physics.RefreshContinuousCollisionCandidate(this);
 882        }
 883
 269884        return true;
 885    }
 886}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.ContinuousCollision.Rotational.Response.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody.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>Calculates and applies spatial rotational contact impulses and handles remaining motion.</content>
 15public partial class SolidBody
 16{
 17    internal bool TryApplyRotationalContinuousCollisionResponse(
 18        SolidBody target,
 19        ManifoldContact contact,
 20        Fixed64 localContactTime,
 21        Vector3d sourceSegmentStart,
 22        Vector3d sourceDisplacement,
 23        FixedQuaternion sourceSegmentStartRotation,
 24        Fixed64 elapsedTime,
 25        Fixed64 remainingTime,
 26        bool sourceIsKinematic)
 27    {
 6328        Fixed64 frameFraction = ResolveRotationalFrameFraction(
 6329            elapsedTime,
 6330            remainingTime,
 6331            localContactTime);
 6332        Fixed64 consumedTime = remainingTime * localContactTime;
 6333        Fixed64 impactElapsedTime = elapsedTime + consumedTime;
 6334        Fixed64 remainingAfterImpact = remainingTime - consumedTime;
 6335        Vector3d sourcePosition = sourceSegmentStart
 6336            + sourceDisplacement * localContactTime;
 6337        FixedQuaternion sourceRotation = sourceIsKinematic
 6338            ? FixedQuaternion.Slerp(
 6339                sourceSegmentStartRotation,
 6340                ContinuousCollisionFrameTargetRotation,
 6341                localContactTime).Normalized
 6342            : IntegrateAngularRotation(
 6343                sourceSegmentStartRotation,
 6344                _angularVelocity,
 6345                consumedTime);
 6346        Vector3d sourceLinearVelocity = sourceIsKinematic
 6347            ? SampleContinuousCollisionLinearVelocity(frameFraction)
 6348            : _linearVelocity;
 6349        Vector3d sourceAngularVelocity = sourceIsKinematic
 6350            ? SampleContinuousCollisionAngularVelocity(frameFraction)
 6351            : _angularVelocity;
 6352        Vector3d targetPosition = target.SampleContinuousCollisionPosition(frameFraction);
 6353        FixedQuaternion targetRotation = target.SampleContinuousCollisionRotation(frameFraction);
 6354        Vector3d targetLinearVelocity = target.SampleContinuousCollisionLinearVelocity(frameFraction);
 6355        Vector3d targetAngularVelocity = target.SampleContinuousCollisionAngularVelocity(frameFraction);
 56
 6357        OrderRotationalContinuousCollisionPair(
 6358            target.Collider,
 6359            out _,
 6360            out _,
 6361            out bool sourceIsA);
 6362        SolidBody bodyA = sourceIsA ? this : target;
 6363        SolidBody bodyB = sourceIsA ? target : this;
 6364        Vector3d positionA = sourceIsA ? sourcePosition : targetPosition;
 6365        Vector3d positionB = sourceIsA ? targetPosition : sourcePosition;
 6366        FixedQuaternion rotationA = sourceIsA ? sourceRotation : targetRotation;
 6367        FixedQuaternion rotationB = sourceIsA ? targetRotation : sourceRotation;
 6368        Vector3d linearVelocityA = sourceIsA ? sourceLinearVelocity : targetLinearVelocity;
 6369        Vector3d linearVelocityB = sourceIsA ? targetLinearVelocity : sourceLinearVelocity;
 6370        Vector3d angularVelocityA = sourceIsA ? sourceAngularVelocity : targetAngularVelocity;
 6371        Vector3d angularVelocityB = sourceIsA ? targetAngularVelocity : sourceAngularVelocity;
 6372        ContactLever3D contactArmA = ContactLever3D.Create(
 6373            contact.AnchorA,
 6374            new ContactAnchor(
 6375                positionA,
 6376                rotationA,
 6377                bodyA._localCenterOfMassOffset));
 6378        ContactLever3D contactArmB = ContactLever3D.Create(
 6379            contact.AnchorB,
 6380            new ContactAnchor(
 6381                positionB,
 6382                rotationB,
 6383                bodyB._localCenterOfMassOffset));
 84
 6385        Fixed64 restitution = PhysicsMaterial.CombineRestitution(
 6386            Collider.Material,
 6387            target.Collider.Material);
 6388        Fixed3x3 originalSourceInverseInertia = _inverseInertiaTensor;
 6389        Fixed3x3 originalTargetInverseInertia = target._inverseInertiaTensor;
 90        ContactNormalVelocityDeltaResult3D result;
 91        bool calculated;
 92        try
 93        {
 6394            _inverseInertiaTensor = ResolveContinuousCollisionInverseInertia(sourceRotation);
 6395            target._inverseInertiaTensor = target.ResolveContinuousCollisionInverseInertia(targetRotation);
 6396            if (contactArmA.IsExact || contactArmB.IsExact)
 97            {
 398                calculated = TryCalculateExactRotationalVelocityDeltas(
 399                    contact,
 3100                    bodyA,
 3101                    positionA,
 3102                    rotationA,
 3103                    linearVelocityA,
 3104                    angularVelocityA,
 3105                    bodyB,
 3106                    positionB,
 3107                    rotationB,
 3108                    linearVelocityB,
 3109                    angularVelocityB,
 3110                    restitution,
 3111                    out result);
 112            }
 113            else
 114            {
 60115                calculated = ContactNormalImpulse3D.TryCalculateVelocityDeltas(
 60116                    bodyA,
 60117                    linearVelocityA,
 60118                    angularVelocityA,
 60119                    contactArmA.Vector,
 60120                    bodyB,
 60121                    linearVelocityB,
 60122                    angularVelocityB,
 60123                    contactArmB.Vector,
 60124                    contact.Normal,
 60125                    restitution,
 60126                    Context.Settings.RestitutionVelocityThreshold,
 60127                    out result);
 60128                if (!calculated)
 129                {
 49130                    calculated = TryCalculateExactRotationalVelocityDeltas(
 49131                        contact,
 49132                        bodyA,
 49133                        positionA,
 49134                        rotationA,
 49135                        linearVelocityA,
 49136                        angularVelocityA,
 49137                        bodyB,
 49138                        positionB,
 49139                        rotationB,
 49140                        linearVelocityB,
 49141                        angularVelocityB,
 49142                        restitution,
 49143                        out result);
 144                }
 145            }
 60146        }
 147        finally
 148        {
 63149            _inverseInertiaTensor = originalSourceInverseInertia;
 63150            target._inverseInertiaTensor = originalTargetInverseInertia;
 63151        }
 152
 63153        if (!calculated
 63154            || !result.IsClosing
 63155            || (result.HasRepresentableNormalVelocity
 63156                && result.NormalVelocity >= -Fixed64.Epsilon))
 157        {
 4158            return false;
 159        }
 160
 59161        Vector3d sourceLinearDelta = sourceIsA
 59162            ? result.LinearVelocityDeltaA
 59163            : result.LinearVelocityDeltaB;
 59164        Vector3d sourceAngularDelta = sourceIsA
 59165            ? result.AngularVelocityDeltaA
 59166            : result.AngularVelocityDeltaB;
 59167        Vector3d targetLinearDelta = sourceIsA
 59168            ? result.LinearVelocityDeltaB
 59169            : result.LinearVelocityDeltaA;
 59170        Vector3d targetAngularDelta = sourceIsA
 59171            ? result.AngularVelocityDeltaB
 59172            : result.AngularVelocityDeltaA;
 59173        bool sourceNeedsRefresh = HasSolverMobility;
 59174        bool targetNeedsRefresh = target.HasSolverMobility;
 59175        Vector3d postSourceLinearVelocity = sourceLinearVelocity;
 59176        Vector3d postSourceAngularVelocity = sourceAngularVelocity;
 59177        bool sourceStateResolved = !sourceNeedsRefresh
 59178            || (Vector3d.TryAdd(
 59179                    sourceLinearVelocity,
 59180                    ProjectLinearMotion(sourceLinearDelta),
 59181                    out postSourceLinearVelocity)
 59182                & Vector3d.TryAdd(
 59183                    sourceAngularVelocity,
 59184                    ProjectAngularMotion(sourceAngularDelta),
 59185                    out postSourceAngularVelocity)
 59186                & CanAppendContinuousCollisionSegment(impactElapsedTime));
 59187        if (!sourceStateResolved)
 188        {
 1189            return false;
 190        }
 191
 58192        Vector3d resolvedTargetPosition = default;
 58193        Vector3d resolvedTargetLinearVelocity = targetLinearVelocity;
 58194        Vector3d resolvedTargetAngularVelocity = targetAngularVelocity;
 58195        bool targetStateResolved = !targetNeedsRefresh
 58196            || (Vector3d.TryAdd(
 58197                    targetLinearVelocity,
 58198                    target.ProjectLinearMotion(targetLinearDelta),
 58199                    out resolvedTargetLinearVelocity)
 58200                & Vector3d.TryAdd(
 58201                    targetAngularVelocity,
 58202                    target.ProjectAngularMotion(targetAngularDelta),
 58203                    out resolvedTargetAngularVelocity)
 58204                & target.CanApplyContinuousCollisionHandoff(
 58205                    targetPosition,
 58206                    targetRotation,
 58207                    remainingAfterImpact,
 58208                    out resolvedTargetPosition));
 58209        if (!targetStateResolved)
 210        {
 1211            return false;
 212        }
 213
 57214        if (sourceNeedsRefresh)
 15215            ValidateContinuousCollisionPosition(sourcePosition, postSourceLinearVelocity, Context.DeltaTime - impactElap
 57216        if (targetNeedsRefresh)
 49217            target.ValidateContinuousCollisionHandoffPosition(resolvedTargetPosition, resolvedTargetLinearVelocity, reso
 218
 57219        if (sourceNeedsRefresh && targetNeedsRefresh)
 7220            _ = Context.Physics.TryReserveContinuousCollisionCandidateRefresh(this, target);
 50221        else if (sourceNeedsRefresh)
 8222            _ = Context.Physics.TryReserveContinuousCollisionCandidateRefresh(this);
 223        else
 42224            _ = Context.Physics.TryReserveContinuousCollisionCandidateRefresh(target);
 225
 57226        if (sourceNeedsRefresh)
 227        {
 15228            ApplyCollisionVelocityState(
 15229                postSourceLinearVelocity,
 15230                postSourceAngularVelocity);
 15231            AppendContinuousCollisionSegment(
 15232                sourcePosition,
 15233                sourceRotation,
 15234                _linearVelocity,
 15235                _angularVelocity,
 15236                impactElapsedTime);
 15237            Context.Physics.RefreshContinuousCollisionCandidate(this);
 238        }
 239
 57240        if (targetNeedsRefresh)
 241        {
 49242            target.ApplyContinuousCollisionHandoffReserved(
 49243                resolvedTargetPosition,
 49244                targetRotation,
 49245                resolvedTargetLinearVelocity,
 49246                resolvedTargetAngularVelocity,
 49247                remainingAfterImpact,
 49248                ignoredCollider3D: Collider);
 249        }
 250
 57251        return true;
 252    }
 253
 254    [System.Runtime.CompilerServices.MethodImpl(
 255        System.Runtime.CompilerServices.MethodImplOptions.NoInlining)]
 256    private bool TryCalculateExactRotationalVelocityDeltas(
 257        ManifoldContact contact,
 258        SolidBody bodyA,
 259        Vector3d positionA,
 260        FixedQuaternion rotationA,
 261        Vector3d linearVelocityA,
 262        Vector3d angularVelocityA,
 263        SolidBody bodyB,
 264        Vector3d positionB,
 265        FixedQuaternion rotationB,
 266        Vector3d linearVelocityB,
 267        Vector3d angularVelocityB,
 268        Fixed64 restitution,
 269        out ContactNormalVelocityDeltaResult3D result)
 270    {
 52271        var centerA = new ContactAnchor(
 52272            positionA,
 52273            rotationA,
 52274            bodyA._localCenterOfMassOffset);
 52275        var centerB = new ContactAnchor(
 52276            positionB,
 52277            rotationB,
 52278            bodyB._localCenterOfMassOffset);
 52279        ExactLever3D exactA =
 52280            contact.AnchorA.GetLeverFrom(centerA);
 52281        ExactLever3D exactB =
 52282            contact.AnchorB.GetLeverFrom(centerB);
 52283        return ContactNormalImpulse3D.TryCalculateVelocityDeltasExact(
 52284                bodyA,
 52285                linearVelocityA,
 52286                angularVelocityA,
 52287                exactA,
 52288                bodyB,
 52289                linearVelocityB,
 52290                angularVelocityB,
 52291                exactB,
 52292                contact.Normal,
 52293                restitution,
 52294                Context.Settings.RestitutionVelocityThreshold,
 52295                out result);
 296    }
 297
 298    private Fixed3x3 ResolveContinuousCollisionInverseInertia(FixedQuaternion rotation)
 299    {
 128300        if (_inverseLocalInertiaTensor == Fixed3x3.Zero)
 56301            return Fixed3x3.Zero;
 302
 303        // Sampled poses use the same active tensor frame as ordinary response.
 72304        Fixed3x3 orientation = rotation.Conjugate().ToMatrix3x3();
 72305        return orientation
 72306            * _inverseLocalInertiaTensor
 72307            * rotation.ToMatrix3x3();
 308    }
 309
 310    private void StopRotationalContinuousCollision(Vector3d contactNormal)
 311    {
 38312        Vector3d lastVelocity = _angularVelocity;
 38313        _angularVelocity = Vector3d.Zero;
 38314        _angularDirection = Vector3d.Zero;
 38315        _angularAccelerationStore = Vector3d.Zero;
 38316        _angularAcceleration = Vector3d.Zero;
 38317        _deltaTorque = Vector3d.Zero;
 38318        RefreshAngularMotionState(lastVelocity);
 38319        Context.Diagnostics.EmitAngularVelocityDelta(this, lastVelocity, _angularVelocity);
 38320        RemoveClosingContinuousCollisionVelocity(contactNormal);
 38321    }
 322}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.ContinuousCollision.Rotational.Search.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody.ContinuousCollision.Rotational.Search.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.Queries;
 14using Gravitas.Support;
 15using SwiftCollections;
 16using SwiftCollections.Query;
 17
 18namespace Gravitas;
 19
 20/// <content>Gathers spatial rotation candidates and bounds separation to find the earliest contact.</content>
 21public partial class SolidBody
 22{
 23    internal bool HasNearbyRotationalContinuousCollisionTarget(
 24        Vector3d startPosition,
 25        Vector3d displacement,
 26        Fixed64 pivotRadius)
 27    {
 742728        FixedBoundVolume bounds = DynamicCcdCandidateIndex.CreateSweptSphereBounds(
 742729            startPosition,
 742730            displacement,
 742731            pivotRadius);
 742732        SwiftList<int> candidateIds = Context.Physics.QueryContinuousCollisionCandidates(bounds);
 3006033        for (int i = 0; i < candidateIds.Count; i++)
 34        {
 762935            SolidBody target = Context.Physics.GetContinuousCollisionCandidate(candidateIds[i]);
 762936            if (IsRotatingContinuousCollisionTarget(target))
 37            {
 2638                return true;
 39            }
 40        }
 41
 740142        if (!Context.Settings.RuntimeMode.RunsMixedContacts())
 707843            return false;
 44
 32345        candidateIds = Context.Physics2D.QueryMixedContinuousCollisionCandidates(bounds);
 84446        for (int i = 0; i < candidateIds.Count; i++)
 47        {
 10848            SolidBody2D target = Context.Physics2D.GetContinuousCollisionCandidate(candidateIds[i]);
 10849            if (IsRotatingContinuousCollisionTarget(target))
 950                return true;
 51        }
 52
 31453        return false;
 54    }
 55
 56    private bool IsRotatingContinuousCollisionTarget(SolidBody target)
 57    {
 762958        if (ReferenceEquals(target, this)
 762959            || !IsMovingRotationalContinuousCollisionTarget(target))
 60        {
 747961            return false;
 62        }
 63
 15064        target.EnsureContinuousCollisionFramePrepared(Context.LateSimulateToken);
 15065        return !target.HasRotationInvariantCollider && target.HasContinuousCollisionRotationalMotion;
 66    }
 67
 68    private bool IsRotatingContinuousCollisionTarget(SolidBody2D target)
 69    {
 10870        if (!IsMovingMixedRotationalContinuousCollisionTarget(target))
 6371            return false;
 72
 4573        target.EnsureContinuousCollisionFramePrepared(Context.LateSimulateToken);
 4574        return !target.HasRotationInvariantCollider && target.HasContinuousCollisionRotationalMotion;
 75    }
 76
 77    internal int GatherRotationalContinuousCollisionCandidates(
 78        Vector3d startPosition,
 79        Vector3d proposedPosition,
 80        Vector3d displacement,
 81        Fixed64 pivotRadius)
 82    {
 48483        _rotationalContinuousCollisionCandidateIds.FastClear();
 84        // The live root registry cannot supply another target when it contains only this source.
 48485        if (Context.Physics.ColliderCount == 1
 48486            && ReferenceEquals(Context.Physics.GetColliderByServiceIndex(0), Collider))
 87        {
 24688            _continuousCollisionHits.Clear();
 24689            return 0;
 90        }
 91
 23892        int staticHitCount = displacement.MagnitudeSquared <= Fixed64.Epsilon
 23893            ? Context.Query3D.OverlapSphereAgainstStaticAll(
 23894                startPosition,
 23895                pivotRadius,
 23896                PhysicsLayerMask.All,
 23897                _continuousCollisionHits,
 23898                Collider,
 23899                includeTriggers: false)
 238100            : Context.Query3D.SweepSphereAgainstStaticAll(
 238101                startPosition,
 238102                proposedPosition,
 238103                pivotRadius,
 238104                PhysicsLayerMask.All,
 238105                _continuousCollisionHits,
 238106                Collider,
 238107                includeTriggers: false);
 108
 238109        SwiftList<int> candidateIds = Context.Physics.QueryContinuousCollisionCandidates(
 238110            DynamicCcdCandidateIndex.CreateSweptSphereBounds(
 238111                startPosition,
 238112                displacement,
 238113                pivotRadius));
 238114        _rotationalContinuousCollisionCandidateIds.EnsureCapacity(candidateIds.Count);
 1238115        for (int i = 0; i < candidateIds.Count; i++)
 116        {
 381117            int candidateId = candidateIds[i];
 381118            if (candidateId != DynamicId)
 147119                _rotationalContinuousCollisionCandidateIds.Add(candidateId);
 120        }
 121
 238122        return staticHitCount + _rotationalContinuousCollisionCandidateIds.Count;
 123    }
 124
 125    private bool TryFindEarliestRotationalContinuousCollision(
 126        Vector3d startPosition,
 127        Vector3d displacement,
 128        FixedQuaternion startRotation,
 129        FixedQuaternion targetRotation,
 130        Fixed64 angularDistance,
 131        Fixed64 pivotRadius,
 132        Fixed64 elapsedTime,
 133        Fixed64 remainingTime,
 134        bool isKinematic,
 135        LSCollider? ignoredTarget,
 136        out Fixed64 safeTime,
 137        out ManifoldContact contact,
 138        out bool hasContact,
 139        out Fixed64 contactTime,
 140        out LSCollider? hitTarget)
 141    {
 572142        safeTime = Fixed64.Zero;
 572143        contact = default;
 572144        hasContact = false;
 572145        contactTime = Fixed64.Zero;
 572146        hitTarget = null;
 147
 572148        bool foundCollision = false;
 572149        Fixed64 earliestTime = Fixed64.One;
 572150        int earliestTargetId = int.MaxValue;
 572151        ManifoldContact earliestContact = default;
 572152        bool earliestHasContact = false;
 572153        Fixed64 earliestContactTime = Fixed64.Zero;
 572154        LSCollider? earliestTarget = null;
 155
 1392156        for (int hitIndex = 0; hitIndex < _continuousCollisionHits.Count; hitIndex++)
 157        {
 124158            Physics3DHit hit = _continuousCollisionHits[hitIndex];
 124159            LSCollider target = hit.Collider!;
 124160            if (target == ignoredTarget
 124161                || !IsValidContinuousCollisionTarget(target)
 124162                || ColliderSettings.GetCollisionType(Collider.Shape, target.Shape) == CollisionType.None
 124163                || CanExcludeInvariantTangentialContact(hit, startPosition, displacement,
 124164                    startRotation, targetRotation, isKinematic)
 124165                || !TryFindEarliestRotationalContinuousCollisionAgainstTarget(
 124166                    target,
 124167                    startPosition,
 124168                    displacement,
 124169                    startRotation,
 124170                    targetRotation,
 124171                    angularDistance,
 124172                    pivotRadius,
 124173                    elapsedTime,
 124174                    remainingTime,
 124175                    isKinematic,
 124176                    out Fixed64 candidateTime,
 124177                    out ManifoldContact candidateContact,
 124178                    out bool candidateHasContact,
 124179                    out Fixed64 candidateContactTime))
 180            {
 181                continue;
 182            }
 183
 80184            if (!ContinuousCollisionMath.ShouldReplaceContinuousCollisionHit(
 80185                    candidateTime,
 80186                    target.Id,
 80187                    foundCollision,
 80188                    earliestTime,
 80189                    earliestTargetId))
 190            {
 191                continue;
 192            }
 193
 79194            foundCollision = true;
 79195            earliestTime = candidateTime;
 79196            earliestTargetId = target.Id;
 79197            earliestContact = candidateContact;
 79198            earliestHasContact = candidateHasContact;
 79199            earliestContactTime = candidateContactTime;
 79200            earliestTarget = target;
 201        }
 202
 572203        for (int candidateIndex = 0;
 760204            candidateIndex < _rotationalContinuousCollisionCandidateIds.Count;
 188205            candidateIndex++)
 206        {
 188207            int dynamicId = _rotationalContinuousCollisionCandidateIds[candidateIndex];
 188208            SolidBody targetBody = Context.Physics.GetContinuousCollisionCandidate(dynamicId);
 188209            if (targetBody.Collider == ignoredTarget
 188210                || !IsMovingRotationalContinuousCollisionTarget(targetBody)
 188211                || ColliderSettings.GetCollisionType(
 188212                    Collider.Shape,
 188213                    targetBody.Collider.Shape) == CollisionType.None
 188214                || !TryFindEarliestRotationalContinuousCollisionAgainstTarget(
 188215                    targetBody.Collider,
 188216                    startPosition,
 188217                    displacement,
 188218                    startRotation,
 188219                    targetRotation,
 188220                    angularDistance,
 188221                    pivotRadius,
 188222                    elapsedTime,
 188223                    remainingTime,
 188224                    isKinematic,
 188225                    out Fixed64 candidateTime,
 188226                    out ManifoldContact candidateContact,
 188227                    out bool candidateHasContact,
 188228                    out Fixed64 candidateContactTime))
 229            {
 230                continue;
 231            }
 232
 107233            if (!ContinuousCollisionMath.ShouldReplaceContinuousCollisionHit(
 107234                    candidateTime,
 107235                    targetBody.Collider.Id,
 107236                    foundCollision,
 107237                    earliestTime,
 107238                    earliestTargetId))
 239            {
 240                continue;
 241            }
 242
 105243            foundCollision = true;
 105244            earliestTime = candidateTime;
 105245            earliestTargetId = targetBody.Collider.Id;
 105246            earliestContact = candidateContact;
 105247            earliestHasContact = candidateHasContact;
 105248            earliestContactTime = candidateContactTime;
 105249            earliestTarget = targetBody.Collider;
 250        }
 251
 572252        if (!foundCollision)
 392253            return false;
 254
 180255        safeTime = earliestTime;
 180256        contact = earliestContact;
 180257        hasContact = earliestHasContact;
 180258        contactTime = earliestContactTime;
 180259        hitTarget = earliestTarget;
 180260        return true;
 261    }
 262
 263    internal bool CanExcludeInvariantTangentialContact(Physics3DHit hit,
 264        Vector3d startPosition, Vector3d displacement, FixedQuaternion startRotation,
 265        FixedQuaternion targetRotation, bool isKinematic)
 266    {
 122267        if (hit.Normal == Vector3d.Zero)
 1268            return false;
 269
 270        // Keep the existing centered-sphere rule: these primitive normals are
 271        // global convex supporting planes even for an initially overlapping cast.
 121272        if (HasRotationInvariantCollider
 121273            && hit.Collider is (LSCuboidCollider or LSSphereCollider or LSCapsuleCollider
 121274                or LSCylinderCollider or LSConeCollider))
 18275            return !IsClosingContinuousCollisionHit(displacement, ResolveShapeExactContinuousClosingNormal(hit));
 276
 103277        if (Collider is LSCompoundCollider
 103278            || (hit.Collider!.Body is SolidBody targetBody && !targetBody.IsStatic))
 8279            return false;
 95280        Vector3d normal = hit.Normal;
 95281        if (!HasInvariantContinuousProjection(normal, displacement, startRotation, targetRotation, isKinematic))
 67282            return false;
 283
 284        // Earlier candidate searches leave the source at their last sampled
 285        // pose. The separating plane must be proved from this segment's start.
 28286        Position3d = startPosition;
 28287        Rotation = startRotation;
 28288        Collider.RebuildRuntimeShapeOnly(refreshMassProperties: false);
 289        // Production collider types are closed; compound sources were excluded
 290        // above. A global supporting plane is valid for every mesh vertex even
 291        // when that source mesh uses concave triangle-surface collision policy.
 292        System.Diagnostics.Debug.Assert(Collider is LSMeshCollider || ConvexColliderSupport.IsSupported(Collider));
 28293        RationalPoint3d sourceMin = Collider is LSMeshCollider
 28294            ? GetExactMeshSupportPoint((LSMeshCollider)Collider, -normal)
 28295            : RationalPoint3d.FromAnchor(new ConvexShape(Collider, Vector3d.Zero).GetSupportAnchor(-normal));
 28296        if (!TryGetFullColliderSupportPoint(hit.Collider, normal, out RationalPoint3d targetMax))
 3297            return false;
 298        // Full-target support includes every concave vertex and compound leaf;
 299        // a later wall cannot hide behind the initial floor hit. A nonnegative
 300        // exact gap plus invariant support projection proves the entire path safe.
 25301        return RationalPoint3d.CompareProjection(sourceMin, targetMax, normal) >= 0;
 302    }
 303
 304    private bool HasInvariantContinuousProjection(Vector3d normal, Vector3d displacement,
 305        FixedQuaternion startRotation, FixedQuaternion targetRotation, bool isKinematic)
 306    {
 138307        int axis = normal.X != Fixed64.Zero ? 0 : normal.Y != Fixed64.Zero ? 1 : 2;
 308        // This is a certificate for every rounded intermediate pose, rather
 309        // than an approximate parallel-axis or endpoint-only check. Integration,
 310        // Slerp and normalization preserve these literal zero components. General
 311        // rotations still require the conservative interval search.
 138312        if (Vector3d.CompareProjection(displacement, Vector3d.Zero, normal) < 0)
 5313            return false;
 510314        for (int offset = 1; offset <= 2; offset++)
 315        {
 197316            int transverse = (axis + offset) % 3;
 197317            bool preservesProjection = normal[transverse] == Fixed64.Zero
 197318                & startRotation[transverse] == Fixed64.Zero
 197319                & targetRotation[transverse] == Fixed64.Zero
 197320                & (isKinematic | _angularVelocity[transverse] == Fixed64.Zero);
 197321            if (!preservesProjection)
 75322                return false;
 323        }
 324
 58325        return true;
 326    }
 327
 328    private static bool TryGetFullColliderSupportPoint(LSCollider collider,
 329        Vector3d direction, out RationalPoint3d point)
 330    {
 28331        if (collider is LSCompoundCollider compound)
 332        {
 11333            if (!TryGetLeafColliderSupportPoint(compound.GetPartCollider(0), direction, out point)) return false;
 26334            for (int index = 1; index < compound.PartCount; index++)
 335            {
 5336                if (!TryGetLeafColliderSupportPoint(compound.GetPartCollider(index), direction,
 6337                        out RationalPoint3d candidate)) return false;
 7338                if (RationalPoint3d.CompareProjection(candidate, point, direction) > 0) point = candidate;
 339            }
 8340            return true;
 341        }
 18342        return TryGetLeafColliderSupportPoint(collider, direction, out point);
 343    }
 344
 345    private static bool TryGetLeafColliderSupportPoint(LSCollider collider,
 346        Vector3d direction, out RationalPoint3d point)
 347    {
 33348        if (collider is LSMeshCollider mesh)
 54349        { point = GetExactMeshSupportPoint(mesh, direction); return true; }
 6350        if (collider is LSCuboidCollider)
 351        {
 3352            point = RationalPoint3d.FromAnchor(new ConvexShape(collider, Vector3d.Zero).GetSupportAnchor(direction));
 3353            return true;
 354        }
 3355        point = default;
 3356        return false;
 357    }
 358
 359    private static RationalPoint3d GetExactMeshSupportPoint(LSMeshCollider collider, Vector3d direction)
 360    {
 31361        PhysicsMesh mesh = collider.Mesh;
 31362        System.ReadOnlySpan<Vector3d> vertices = mesh.GeometryVertices;
 31363        RationalPoint3d support = mesh.GeometryFrame.GetPoint(vertices[0]);
 364        // Exclusion needs the exact global extremum: rounding an inverse
 365        // direction can select a lower vertex even on the compact rigid path.
 292366        for (int index = 1; index < vertices.Length; index++)
 367        {
 115368            RationalPoint3d candidate = mesh.GeometryFrame.GetPoint(vertices[index]);
 115369            if (RationalPoint3d.CompareProjection(candidate, support, direction) > 0)
 3370                support = candidate;
 371        }
 31372        return support;
 373    }
 374
 375    private bool TryFindEarliestRotationalContinuousCollisionAgainstTarget(
 376        LSCollider target,
 377        Vector3d startPosition,
 378        Vector3d displacement,
 379        FixedQuaternion startRotation,
 380        FixedQuaternion targetRotation,
 381        Fixed64 angularDistance,
 382        Fixed64 pivotRadius,
 383        Fixed64 elapsedTime,
 384        Fixed64 remainingTime,
 385        bool isKinematic,
 386        out Fixed64 safeTime,
 387        out ManifoldContact contact,
 388        out bool hasContact,
 389        out Fixed64 contactTime)
 390    {
 212391        safeTime = Fixed64.Zero;
 212392        contact = default;
 212393        hasContact = false;
 212394        contactTime = Fixed64.Zero;
 212395        SolidBody? targetBody = target.Body;
 212396        bool samplesTargetMotion = targetBody != null
 212397            && IsMovingRotationalContinuousCollisionTarget(targetBody);
 212398        Vector3d originalTargetPosition = targetBody?.Position3d ?? Vector3d.Zero;
 212399        FixedQuaternion originalTargetRotation = targetBody?.Rotation ?? FixedQuaternion.Identity;
 212400        bool originalTargetPositionMutated = targetBody?._positionMutated ?? false;
 212401        bool originalTargetRotationMutated = targetBody?._rotationMutated ?? false;
 212402        if (samplesTargetMotion)
 127403            targetBody!.EnsureContinuousCollisionFramePrepared(Context.LateSimulateToken);
 212404        LSMeshCollider? mesh = Collider as LSMeshCollider ?? target as LSMeshCollider;
 212405        LSConeCollider? cone = Collider as LSConeCollider ?? target as LSConeCollider;
 212406        bool deferConeContact = mesh != null && cone != null && mesh.StaticSurface == null
 212407            && (mesh.Mode == MeshColliderMode.Concave || !mesh.IsClosedSurface);
 408
 409        try
 410        {
 212411            Span<RotationalInterval> intervals =
 212412                stackalloc RotationalInterval[
 212413                    ContinuousCollisionMath.RotationalIntervalMaxDepth + 2];
 212414            int intervalCount = 1;
 212415            int processedNodeCount = 0;
 212416            bool hasKnownContact = false;
 212417            bool knownContactIsClosing = false;
 212418            Fixed64 knownContactTime = Fixed64.One;
 212419            ManifoldContact knownContact = default;
 212420            intervals[0] = new RotationalInterval(
 212421                Fixed64.Zero,
 212422                Fixed64.One,
 212423                depth: 0);
 424
 3739425            while (intervalCount > 0)
 426            {
 3714427                RotationalInterval interval = intervals[--intervalCount];
 3714428                Fixed64 midpoint = (interval.LowerTime + interval.UpperTime) * Fixed64.Half;
 3714429                Fixed64 intervalSpan = interval.UpperTime - interval.LowerTime;
 3714430                Fixed64 midpointFrameFraction = SampleRotationalContinuousPairPose(startPosition, displacement,
 3714431                    startRotation, targetRotation, midpoint, elapsedTime, remainingTime, isKinematic,
 3714432                    targetBody, samplesTargetMotion);
 433
 3714434                processedNodeCount++;
 3714435                bool hasMotionBound = ContinuousCollisionMath.TryResolveRotationalIntervalMotionBound(
 3714436                    displacement,
 3714437                    angularDistance,
 3714438                    pivotRadius,
 3714439                    intervalSpan,
 3714440                    out Fixed64 motionBound);
 3714441                if (hasMotionBound && samplesTargetMotion)
 442                {
 2043443                    Fixed64 lowerFrameFraction = ResolveRotationalFrameFraction(
 2043444                        elapsedTime,
 2043445                        remainingTime,
 2043446                        interval.LowerTime);
 2043447                    Fixed64 upperFrameFraction = ResolveRotationalFrameFraction(
 2043448                        elapsedTime,
 2043449                        remainingTime,
 2043450                        interval.UpperTime);
 2043451                    Fixed64 targetRadius = targetBody!.ResolveContinuousCollisionProxyRadius();
 2043452                    bool targetBoundResolved = targetBody.TryResolveContinuousCollisionMotionBound(
 2043453                        lowerFrameFraction,
 2043454                        upperFrameFraction,
 2043455                        targetRadius,
 2043456                        out Fixed64 targetMotionBound);
 2043457                    bool combinedBoundResolved = Fixed64.TryAdd(
 2043458                        motionBound,
 2043459                        targetMotionBound,
 2043460                        out motionBound);
 2043461                    hasMotionBound = targetBoundResolved & combinedBoundResolved;
 462                }
 3714463                ManifoldContact sampleContact = default;
 3714464                bool sampleContactIsClosing = false;
 3714465                bool sampleHasContact = deferConeContact
 3714466                    ? CollisionDetection.IntersectsMeshConeSurface(mesh!, cone!, Context.CollisionScratch)
 3714467                    : TrySampleRotationalContinuousCollision(target, midpointFrameFraction,
 3714468                        out sampleContact, out sampleContactIsClosing);
 3714469                if (sampleHasContact
 3714470                    && (!hasKnownContact || midpoint < knownContactTime))
 471                {
 1385472                    hasKnownContact = true;
 1385473                    knownContactTime = midpoint;
 1385474                    knownContact = sampleContact;
 1385475                    knownContactIsClosing = sampleContactIsClosing;
 476                }
 477
 3714478                if (!sampleHasContact
 3714479                    && hasMotionBound
 3714480                    && IsRotationalIntervalSeparated(target, motionBound))
 481                {
 482                    continue;
 483                }
 484
 2689485                if (ContinuousCollisionMath.TryResolveRotationalSearchLimit(
 2689486                        interval,
 2689487                        processedNodeCount,
 2689488                        hasKnownContact,
 2689489                        knownContactTime,
 2689490                        out safeTime,
 2689491                        out contactTime,
 2689492                        out hasContact))
 493                {
 187494                    if (hasContact && deferConeContact && contactTime == safeTime)
 495                    {
 496                        // Closing admission needs the complete manifold at the retained witness, not the safe frontier.
 8497                        Fixed64 witnessFraction = SampleRotationalContinuousPairPose(startPosition, displacement,
 8498                            startRotation, targetRotation, contactTime, elapsedTime, remainingTime, isKinematic,
 8499                            targetBody, samplesTargetMotion);
 8500                        _ = TrySampleRotationalContinuousCollision(target, witnessFraction,
 8501                            out knownContact, out knownContactIsClosing);
 502                    }
 503                    // Geometric overlap remains the interval's certified upper
 504                    // bound. Only its closing witness can authorize a response;
 505                    // absence of one does not prove the intervening path separated.
 187506                    hasContact &= knownContactIsClosing;
 187507                    contact = knownContact;
 187508                    return true;
 509                }
 510
 2502511                int childDepth = interval.Depth + 1;
 2502512                if (sampleHasContact)
 513                {
 514                    // A real midpoint contact is already an upper bound. Only the
 515                    // earlier half can contain an earlier first contact.
 1320516                    intervals[intervalCount++] = new RotationalInterval(
 1320517                        interval.LowerTime,
 1320518                        midpoint,
 1320519                        childDepth);
 1320520                    continue;
 521                }
 522
 1182523                intervals[intervalCount++] = new RotationalInterval(
 1182524                    midpoint,
 1182525                    interval.UpperTime,
 1182526                    childDepth);
 1182527                intervals[intervalCount++] = new RotationalInterval(
 1182528                    interval.LowerTime,
 1182529                    midpoint,
 1182530                    childDepth);
 531            }
 532
 25533            return false;
 534        }
 535        finally
 536        {
 212537            if (samplesTargetMotion)
 538            {
 127539                targetBody!.Position3d = originalTargetPosition;
 127540                targetBody.Rotation = originalTargetRotation;
 127541                targetBody.Collider.RebuildRuntimeShapeOnly(refreshMassProperties: false);
 127542                targetBody._positionMutated = originalTargetPositionMutated;
 127543                targetBody._rotationMutated = originalTargetRotationMutated;
 544            }
 212545        }
 212546    }
 547
 548
 549    private Fixed64 SampleRotationalContinuousPairPose(Vector3d startPosition, Vector3d displacement,
 550        FixedQuaternion startRotation, FixedQuaternion targetRotation, Fixed64 time, Fixed64 elapsedTime,
 551        Fixed64 remainingTime, bool isKinematic, SolidBody? targetBody, bool samplesTargetMotion)
 552    {
 3722553        if (isKinematic)
 2371554            SampleKinematicRotationalContinuousPose(startPosition, displacement, startRotation, targetRotation, time);
 555        else
 1351556            SampleDynamicRotationalContinuousPose(startPosition, displacement, startRotation, remainingTime, time);
 3722557        Fixed64 frameFraction = ResolveRotationalFrameFraction(elapsedTime, remainingTime, time);
 3722558        if (samplesTargetMotion)
 559        {
 2045560            targetBody!.Position3d = targetBody.SampleContinuousCollisionPosition(frameFraction);
 2045561            targetBody.Rotation = targetBody.SampleContinuousCollisionRotation(frameFraction);
 2045562            targetBody.Collider.RebuildRuntimeShapeOnly(refreshMassProperties: false);
 563        }
 3722564        return frameFraction;
 565    }
 566
 567}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody.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 Chronicler;
 11using FixedMathSharp;
 12using Gravitas.Colliders;
 13using Gravitas.CollisionHandling;
 14using Gravitas.Queries;
 15using GridForge.Grids;
 16using SwiftCollections;
 17
 18namespace Gravitas;
 19
 20/// <summary>Represents deterministic 3D rigid-body state owned by one world context.</summary>
 21/// <content>Owns motion, mass properties, grounding, CCD and recorded body state.</content>
 22public partial class SolidBody : IRecordable
 23{
 24    /// <summary>Stores the host-controlled body diagnostic flag.</summary>
 25    public bool Debug = false;
 26
 27    /// <summary>Gets whether the body is initialized and registered.</summary>
 28    public bool Active { get; private set; }
 29
 840330    private int _dynamicId = -1;
 31
 32    /// <summary>
 33    /// Gets the ephemeral simulated-body slot, or <c>-1</c> for a static or
 34    /// unregistered body.
 35    /// </summary>
 31062036    public int DynamicId => _dynamicId;
 37
 38    private BodyFreezeAxes3D _freezeAxes;
 39
 40    /// <summary>
 41    /// Gets or sets the 3D translational and rotational degrees of freedom
 42    /// frozen for solver response, integration, and CCD.
 43    /// </summary>
 44    public BodyFreezeAxes3D FreezeAxes
 45    {
 46        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 194347        get => _freezeAxes;
 48        set
 49        {
 517350            ChronicleGuard.ThrowIfArgument(
 517351                (value & ~BodyFreezeAxes3D.All) != BodyFreezeAxes3D.None,
 517352                nameof(value),
 517353                "Unsupported 3D freeze axis bits.");
 54
 517355            if (_freezeAxes == value)
 414056                return;
 57
 103358            _freezeAxes = value;
 103359            ApplyFreezeConstraintsToMotion();
 103360            if (Active)
 40761                RefreshInertiaTensor(Mass);
 103362            RefreshPartitionMobility();
 103363            RefreshPartitionAwakeState();
 103364        }
 65    }
 66
 67    /// <summary>
 68    /// Gets whether all translation axes are frozen.
 69    /// </summary>
 70    public bool IsPositionFullyFrozen
 71    {
 72        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 1369112873        get => (_freezeAxes & BodyFreezeAxes3D.Position) == BodyFreezeAxes3D.Position;
 74    }
 75
 76    private PhysicsMobilityType _motionType;
 77
 78    /// <summary>
 79    /// Gets this body's explicit solver-controlled, host-controlled, or static
 80    /// runtime role.
 81    /// </summary>
 45082    public PhysicsMobilityType MotionType => _motionType;
 83
 84    /// <summary>
 85    /// Gets whether the solver controls this body.
 86    /// </summary>
 1474255787    public bool IsDynamic => _motionType == PhysicsMobilityType.Dynamic;
 88
 89    /// <summary>
 90    /// Gets whether this body is excluded from per-frame motion.
 91    /// </summary>
 52688492    public bool IsStatic => _motionType == PhysicsMobilityType.Static;
 93
 94    /// <summary>
 95    /// Gets whether the host controls this body's pose.
 96    /// </summary>
 257457297    public bool IsKinematic => _motionType == PhysicsMobilityType.Kinematic;
 98
 99    /// <summary>
 100    /// Changes this registered body's runtime role between fixed-step
 101    /// transactions.
 102    /// </summary>
 103    /// <remarks>
 104    /// The transition preserves body, collider, pair, joint, and host identity,
 105    /// but clears incompatible motion, sleep, CCD, and solver-cache state before
 106    /// repartitioning. Freeze axes remain independent of the selected role.
 107    /// </remarks>
 108    /// <exception cref="ArgumentOutOfRangeException">
 109    /// <paramref name="motionType"/> is undefined.
 110    /// </exception>
 111    /// <exception cref="InvalidOperationException">
 112    /// The body is not currently registered, its context registration has been
 113    /// reset, or a simulation transaction or callback is active.
 114    /// </exception>
 115    public void SetMotionType(PhysicsMobilityType motionType)
 116    {
 49117        if (!PrepareMotionTypeTransition(motionType))
 1118            return;
 119
 40120        CommitMotionTypeTransition(motionType);
 40121    }
 122
 123    internal bool PrepareMotionTypeTransition(PhysicsMobilityType motionType)
 124    {
 171125        motionType.ThrowIfInvalid(nameof(motionType));
 170126        ThrowIfRuntimeRegistrationMissing();
 166127        if (_motionType == motionType)
 101128            return false;
 129
 65130        Context.ThrowIfFixedStepMutationNotAllowed();
 63131        Collider.ValidateCurrentRuntimeTransform();
 61132        if (motionType != PhysicsMobilityType.Dynamic)
 41133            PublishAuthoritativePose();
 134
 61135        return true;
 136    }
 137
 138    internal void CommitMotionTypeTransition(PhysicsMobilityType motionType)
 139    {
 60140        ReconcileMotionTypeRegistration(motionType);
 141
 60142        ClearMotionForSleep();
 60143        _isSleeping = false;
 60144        _sleepFrameCount = 0;
 60145        InvalidateContinuousCollisionTrajectory();
 60146        ApplyFreezeConstraintsToMotion();
 60147        RefreshInertiaTensor(Mass);
 60148        RefreshPartitionMobility();
 60149    }
 150
 151    internal void ApplyLoadedMotionType(PhysicsMobilityType motionType)
 152    {
 86153        motionType.ThrowIfInvalid(nameof(motionType));
 86154        if (_motionType == motionType)
 68155            return;
 156
 18157        if (!Active)
 158        {
 4159            _motionType = motionType;
 4160            return;
 161        }
 162
 14163        ReconcileMotionTypeRegistration(motionType);
 14164        InvalidateContinuousCollisionTrajectory();
 14165    }
 166
 167    internal void PreflightLoadedMotionType(PhysicsMobilityType motionType)
 168    {
 91169        motionType.ThrowIfInvalid(nameof(motionType));
 90170        if (!Active)
 6171            return;
 172
 84173        ThrowIfRuntimeRegistrationMissing();
 82174        Context.ThrowIfFixedStepMutationNotAllowed();
 82175        Collider.ValidateCurrentRuntimeTransform();
 82176    }
 177
 178    private void ThrowIfRuntimeRegistrationMissing()
 179    {
 8518180        ChronicleGuard.ThrowIfTrue(
 8518181            !Active,
 8518182            nameof(SolidBody),
 8518183            "Body runtime state cannot change before initialization or after deactivation.");
 8515184        ChronicleGuard.ThrowIfTrue(
 8515185            !Context.Physics.TryGetColliderById(Collider.Id, out LSCollider? registeredCollider)
 8515186                || !ReferenceEquals(registeredCollider, Collider),
 8515187            nameof(SolidBody),
 8515188            "Body runtime state cannot change after its registration has been reset or replaced.");
 8507189    }
 190
 191    private void ReconcileMotionTypeRegistration(PhysicsMobilityType motionType)
 192    {
 74193        PhysicsMobilityType previousMotionType = _motionType;
 74194        InvalidateMassDependentSolverState();
 195
 74196        _motionType = motionType;
 74197        Context.Physics.RefreshBodyMotionTypeRegistration(this, previousMotionType);
 74198        Context.Physics.RefreshColliderServiceRefreshRegistration(Collider);
 74199    }
 200
 39201    internal void SetDynamicId(int dynamicId) => _dynamicId = dynamicId;
 202
 203    private ContinuousCollisionMode _continuousCollisionMode = ContinuousCollisionMode.Inherit;
 8403204    private long _continuousCollisionFrameToken = -1;
 8403205    private readonly SwiftList<ContinuousCollisionMotionSegment3D> _continuousCollisionTrajectory =
 8403206        new(PhysicsSettings.DefaultContinuousCollisionMaxToiIterations + 1);
 207    private Vector3d _continuousCollisionAngularVelocityStepStart;
 208    private bool _continuousCollisionHandoffPending;
 8403209    private long _continuousCollisionHandoffToken = -1;
 210    private Fixed64 _continuousCollisionHandoffRemainingTime;
 211
 212    /// <summary>
 213    /// Selects the deterministic tunneling guard used when this body commits frame movement.
 214    /// Inherited values resolve through the cached top-parent body before falling back to context settings.
 215    /// </summary>
 216    /// <exception cref="ArgumentOutOfRangeException">The value is not a declared continuous-collision mode.</exception>
 217    public ContinuousCollisionMode ContinuousCollisionMode
 218    {
 219        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 164220        get => _continuousCollisionMode;
 221        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 222        set
 223        {
 1297224            value.ThrowIfInvalid(nameof(value));
 1293225            _continuousCollisionMode = value;
 1293226        }
 227    }
 228
 229    /// <summary>
 230    /// Gets the number of continuous-collision impacts consumed by the most recent late simulation step.
 231    /// </summary>
 232    public int LastContinuousCollisionToiIterationCount { get; private set; }
 233
 234    /// <summary>
 235    /// Gets whether a CCD work/state limit or unresolved refinement required a conservative clamp in the most recent la
 236    /// </summary>
 237    public bool LastContinuousCollisionToiIterationLimitReached { get; private set; }
 238
 239    private FixedTransform _positionTransform = null!;
 240    /// <summary>Gets the host transform that receives authoritative or visual position updates.</summary>
 495241    public FixedTransform PositionTransform => _positionTransform;
 242
 243    private FixedTransform _rotationTransform = null!;
 244    /// <summary>Gets the host transform that receives authoritative or visual rotation updates.</summary>
 26245    public FixedTransform RotationTransform => _rotationTransform;
 246
 247    #region Position Properties
 248
 249    private bool _positionMutated;
 250    private bool _positionChangedBuffer;
 251    /// <summary>Gets whether an authoritative position change awaits presentation.</summary>
 74323252    public bool PositionChangePending => _positionMutated || _positionChangedBuffer;
 253
 254    private Vector2d _position2dUnmarked;
 255    private void SetPosition2d(Vector2d value)
 256    {
 79385257        if (_position2dUnmarked == value)
 15314258            return;
 259
 64071260        _position2dUnmarked = value;
 64071261        _positionMutated = true;
 64071262    }
 263
 264    /// <summary>Gets the authoritative world-space position.</summary>
 265    public Vector3d Position3d
 266    {
 1967563267        get => _position2dUnmarked.ToVector3d(_heightPosUnmarked);
 268        private set
 269        {
 95448270            if (Position3d == value)
 33978271                return;
 61470272            _position2dUnmarked.Set(value.X, value.Z);
 61470273            _heightPosUnmarked = value.Y;
 61470274            _positionMutated = true;
 61470275        }
 276    }
 277
 8403278    private Fixed64 _heightPosUnmarked = Fixed64.Zero;  // Actor's transform position Y
 279    /// <summary>Gets the authoritative world-space Y coordinate.</summary>
 280    public Fixed64 HeightPos
 281    {
 9282        get => _heightPosUnmarked;
 283        private set
 284        {
 72665285            if (_heightPosUnmarked == value)
 21748286                return;
 50917287            _heightPosUnmarked = value;
 50917288            _positionMutated = true;
 50917289        }
 290    }
 291
 292    private Vector3d _lastPosition;
 293
 294    private const int DefaultBodyHitBufferCapacity = 16;
 8403295    private readonly SwiftList<Physics3DHit> _continuousCollisionHits = new(DefaultBodyHitBufferCapacity);
 8403296    private readonly SwiftList<PhysicsMixedHit> _continuousMixedCollisionHits = new(DefaultBodyHitBufferCapacity);
 8403297    private readonly SwiftList<int> _rotationalContinuousCollisionCandidateIds = new(DefaultBodyHitBufferCapacity);
 8403298    private readonly ContactManifold _rotationalContinuousCollisionManifold = new();
 8403299    private readonly SweptSphereQueryWorker _shapeExactContinuousSweepWorker = new();
 8403300    private readonly ConvexSweepQueryWorker _shapeExactContinuousConvexSweepWorker = new();
 301    // Shape-exact CCD must remain separated even when a degenerate support
 302    // feature is recognized at the far edge of the convex sweep contact band.
 1303    private static readonly Fixed64 ShapeExactContinuousContactSlop =
 1304        ConvexSweepQueryWorker.ContactTolerance * (Fixed64)4;
 305
 306
 307    /// <summary>Enables publishing interpolated positions to <see cref="PositionTransform"/>.</summary>
 308    public bool CanSetVisualPosition;
 309
 310    private Vector3d _visualPosition;
 311    /// <summary>Gets the current position interpolation endpoint.</summary>
 1312    public Vector3d VisualPosition => _visualPosition;
 313
 314    private Vector3d _lastVisualPosition;
 315    /// <summary>Gets the previous position interpolation endpoint.</summary>
 1316    public Vector3d LastVisualPosition => _lastVisualPosition;
 317
 318    #endregion
 319
 320    #region Rotation Properties
 321
 322    private bool _rotationMutated;
 323    private bool _rotationChangedBuffer;
 324    /// <summary>Gets whether an authoritative rotation change awaits presentation.</summary>
 46198325    public bool RotationChangePending => _rotationMutated || _rotationChangedBuffer;
 326
 327    private FixedQuaternion _rotation;
 328
 329    /// <summary>Gets the authoritative world-space rotation.</summary>
 330    public FixedQuaternion Rotation
 331    {
 1453908332        get => _rotation;
 333        private set
 334        {
 36426335            if (_rotation == value)
 19733336                return;
 16693337            _rotation = value;
 16693338            _rotationMutated = true;
 16693339        }
 340    }
 341
 342    /// <summary>Gets the body's world-space forward direction.</summary>
 1343    public Vector3d Forward => _rotation.Rotate(Vector3d.Forward);
 344    /// <summary>Gets the body's world-space up direction.</summary>
 1345    public Vector3d Up => _rotation.Rotate(Vector3d.Up);
 346    /// <summary>Gets the body's world-space right direction.</summary>
 4347    public Vector3d Right => _rotation.Rotate(Vector3d.Right);
 348
 349    /// <summary>Enables publishing interpolated rotations to <see cref="RotationTransform"/>.</summary>
 350    public bool CanSetVisualRotation;
 351
 352    private FixedQuaternion _visualRotation;
 353    /// <summary>Gets the current rotation interpolation endpoint.</summary>
 13354    public FixedQuaternion VisualRotation => _visualRotation;
 355
 356    private FixedQuaternion _lastVisualRotation;
 357    /// <summary>Gets the previous rotation interpolation endpoint.</summary>
 1358    public FixedQuaternion LastVisualRotation => _lastVisualRotation;
 359
 360    /// <summary>
 361    /// Gets whether all rotation axes are frozen.
 362    /// </summary>
 363    public bool IsRotationFullyFrozen
 364    {
 365        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 8260543366        get => (_freezeAxes & BodyFreezeAxes3D.Rotation) == BodyFreezeAxes3D.Rotation;
 367    }
 368
 369    /// <summary>Controls visual rotation interpolation when the host agent is not interacting.</summary>
 8403370    public Fixed64 DefaultRotationSpeed = (Fixed64)30; // 1 for NPC...
 371
 372    /// <summary>Controls visual rotation interpolation while the host agent is interacting.</summary>
 8403373    public Fixed64 InteractionRotationSpeed = (Fixed64)3; // 0.15 for NPC...
 374
 375    private Fixed64 _rotationSpeed;
 376    private Fixed64 _rotationInterpoleSpeed;
 377
 378    #endregion
 379
 380    private int _settingVisualsCounter;
 74207381    private bool SettingVisuals => _settingVisualsCounter > 0;
 382
 383    /// <summary>
 384    /// The desiredSpeed an object has in a specific direction
 385    /// AKA units per second the unit is moving
 386    /// </summary>
 387    private Vector3d _linearVelocity;
 388    /// <summary>Gets the authoritative world-space linear velocity.</summary>
 2151714389    public Vector3d LinearVelocity => _linearVelocity;
 390
 391    private Vector3d _linearDirection;
 392
 393    /// <summary>
 394    /// Represents the angular velocity of the body.
 395    /// </summary>
 396    private Vector3d _angularVelocity;
 397    /// <summary>Gets the authoritative world-space angular velocity.</summary>
 2111336398    public Vector3d AngularVelocity => _angularVelocity;
 399
 400    private Vector3d _angularDirection;
 401
 402    /// <summary>
 403    /// Represents the torque applied to the body.
 404    /// </summary>
 405    private Vector3d _deltaTorque;
 406
 407    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 408    internal Fixed64 GetConstrainedInverseMass(Vector3d axis)
 409    {
 2019447410        if (!CanTranslate || axis == Vector3d.Zero)
 74640411            return Fixed64.Zero;
 412
 1944807413        Fixed64 axisMagnitudeSquared = axis.MagnitudeSquared;
 1944807414        if (axisMagnitudeSquared <= Fixed64.Epsilon)
 1415            return Fixed64.Zero;
 416
 1944806417        Vector3d allowedAxis = ProjectLinearMotion(axis);
 1944806418        Fixed64 allowedScale = Vector3d.Dot(allowedAxis, axis) / axisMagnitudeSquared;
 1944806419        return allowedScale > Fixed64.Zero ? InverseMass * allowedScale : Fixed64.Zero;
 420    }
 421
 422    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 423    internal Vector3d ApplyConstrainedInverseInertia(Vector3d torqueAxis)
 424    {
 3001388425        if (!CanRotate || torqueAxis == Vector3d.Zero)
 488145426            return Vector3d.Zero;
 427
 2513243428        return ProjectAngularMotion(_inverseInertiaTensor * torqueAxis);
 429    }
 430
 431    internal Fixed3x3 GetConstrainedInverseInertiaTensor() =>
 718815432        new(
 718815433            ApplyConstrainedInverseInertia(Vector3d.Right),
 718815434            ApplyConstrainedInverseInertia(Vector3d.Up),
 718815435            ApplyConstrainedInverseInertia(Vector3d.Forward));
 436
 8403437    private Fixed64 _gravityScale = Fixed64.One;
 438
 439    /// <summary>
 440    /// Multiplies context environment gravity for this body. Zero disables gravity-derived acceleration and grounded we
 441    /// </summary>
 442    public Fixed64 GravityScale
 443    {
 444        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 162445        get => _gravityScale;
 446        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 447        set
 448        {
 102449            ChronicleGuard.ThrowIfArgument(
 102450                value < Fixed64.Zero,
 102451                nameof(value),
 102452                "Gravity scale cannot be negative.");
 101453            _gravityScale = value;
 101454            RefreshGroundNormalForce();
 101455        }
 456    }
 457
 458    /// <summary>Gets whether both linear and angular velocity are exactly zero.</summary>
 158459    public bool IsAtRest => _linearVelocity.IsZero && _angularVelocity.IsZero;
 460
 461    private bool _isSleeping;
 462    private int _sleepFrameCount;
 8403463    private bool _sleepEnabled = true;
 8403464    private int _sleepFrameThreshold = 16;
 8403465    private Fixed64 _sleepLinearSpeedThreshold = (Fixed64)0.001f;
 8403466    private Fixed64 _sleepAngularSpeedThreshold = (Fixed64)0.001f;
 467
 468    /// <summary>
 469    /// Gets whether this dynamic body is currently excluded from solver work until a deterministic wake event occurs.
 470    /// </summary>
 5986796471    public bool IsSleeping => _isSleeping;
 472
 473    /// <summary>
 474    /// Enables deterministic sleep evaluation for this body.
 475    /// </summary>
 476    public bool SleepEnabled
 477    {
 72604478        get => _sleepEnabled;
 479        set
 480        {
 829481            if (_sleepEnabled == value)
 13482                return;
 483
 816484            _sleepEnabled = value;
 816485            if (!value)
 706486                Wake();
 816487        }
 488    }
 489
 490    /// <summary>
 491    /// Number of consecutive fixed frames below sleep thresholds required before the body sleeps.
 492    /// </summary>
 493    public int SleepFrameThreshold
 494    {
 1896495        get => _sleepFrameThreshold;
 496        set
 497        {
 37498            ChronicleGuard.ThrowIfNegative(value, nameof(value));
 37499            _sleepFrameThreshold = value;
 37500        }
 501    }
 502
 503    /// <summary>
 504    /// Linear speed at or below which the body can count toward sleeping.
 505    /// </summary>
 506    public Fixed64 SleepLinearSpeedThreshold
 507    {
 6779508        get => _sleepLinearSpeedThreshold;
 509        set
 510        {
 4511            ChronicleGuard.ThrowIfArgument(
 4512                value < Fixed64.Zero,
 4513                nameof(value),
 4514                "Sleep linear speed threshold cannot be negative.");
 4515            _sleepLinearSpeedThreshold = value;
 4516        }
 517    }
 518
 519    /// <summary>
 520    /// Angular speed at or below which the body can count toward sleeping.
 521    /// </summary>
 522    public Fixed64 SleepAngularSpeedThreshold
 523    {
 2062524        get => _sleepAngularSpeedThreshold;
 525        set
 526        {
 2527            ChronicleGuard.ThrowIfArgument(
 2528                value < Fixed64.Zero,
 2529                nameof(value),
 2530                "Sleep angular speed threshold cannot be negative.");
 2531            _sleepAngularSpeedThreshold = value;
 2532        }
 533    }
 534
 5825352535    internal bool IsAwakeForCollision => HasSolverMobility && !IsSleeping;
 536
 537    // LinearVelocity magnitude
 538    private Fixed64 _linearSpeed;
 539    /// <summary>Gets the magnitude of <see cref="LinearVelocity"/>.</summary>
 1279540    public Fixed64 LinearSpeed => _linearSpeed;
 541
 542    /// <summary>
 543    /// Represents the total accumulated force on the object. This can be the sum of all external forces acting on the o
 544    /// Changing this value directly affects the object's acceleration and, subsequently, its velocity and position.
 545    /// </summary>
 546    private Vector3d _linearAccelerationStore;
 547    private Vector3d _deltaAcceleration;
 548
 549    private Vector3d _linearAcceleration;
 550    /// <summary>Gets the linear acceleration measured during the latest integration step.</summary>
 3551    public Vector3d LinearAcceleration => _linearAcceleration;
 552
 553    private Fixed64 _angularSpeed;
 554    /// <summary>Gets the magnitude of <see cref="AngularVelocity"/>.</summary>
 15555    public Fixed64 AngularSpeed => _angularSpeed;
 556
 557    private Vector3d _angularAccelerationStore;
 558
 559    private Vector3d _angularAcceleration;
 560    /// <summary>Gets the angular acceleration measured during the latest integration step.</summary>
 9561    public Vector3d AngularAcceleration => _angularAcceleration;
 562
 563
 564    /// <summary>
 565    /// Controls empirical linear-in-speed drag. Multiplied by air density and
 566    /// frontal area, it forms a velocity-decay rate independent of mass.
 567    /// Nonnegative values remove at most the remaining motion per fixed step;
 568    /// the coefficient is not constrained to [0, 1].
 569    /// </summary>
 8403570    public Fixed64 LinearDragCoefficient = (Fixed64)0.75f;
 571
 8403572    private Fixed64 AngularDragCoefficient = (Fixed64)0.75f;
 573
 574    /// <summary>
 575    /// Represents the normal force on the object.
 576    /// It's usually perpendicular to the contact surface and prevents the object from "falling" into the surface.
 577    /// Can be updated to simulate changes in terrain or surface inclination.
 578    /// </summary>
 579    private Vector3d _normalForce;
 580
 581    // Weight is a measure of how the force of gravity acts upon the mass.
 582    // Weight (in Newtons) is mass (in Kilograms) multiplied by the acceleration of gravity (g).
 583    // ex: 68 kg * 9.8 m/s^2 = 667 Newtons / PhysicsEnvironment.PoundToNewton = 150 Pounds
 1463584    private Fixed64 Weight => Mass * Context.Environment.Gravity * _gravityScale;
 585
 586    /// <summary>Gets the host agent that owns this body.</summary>
 587    public IMatterAgent Agent { get; private set; } = null!;
 588
 589    /// <summary>Gets the world context supplied by the host agent.</summary>
 590    public GravitasWorldContext Context { get; private set; } = null!;
 591
 592    /// <summary>Gets the GridForge world owned by <see cref="Context"/>.</summary>
 18790593    public GridWorld World => Context.World;
 594
 595    /// <summary>Gets the runtime collider attached to this body.</summary>
 596    public LSCollider Collider { get; private set; } = null!;
 597
 598    /// <summary>
 599    /// Called after authoritative position or rotation changes have been
 600    /// committed during simulation.
 601    /// </summary>
 602    public Action? OnMoved;
 603
 604    /// <summary>Creates a 3D body bound to a host agent and collider.</summary>
 8403605    public SolidBody(IMatterAgent agent, LSCollider collider)
 606    {
 8403607        ChronicleGuard.ThrowIfNull(agent, nameof(agent));
 8403608        ChronicleGuard.ThrowIfNull(collider, nameof(collider));
 609
 8403610        GravitasWorldContext context = agent.Context;
 8403611        ChronicleGuard.ThrowIfNull(context, nameof(agent.Context));
 8403612        ChronicleGuard.ThrowIfArgument(
 8403613            collider.TryGetBoundContext(out GravitasWorldContext? colliderContext)
 8403614                && !ReferenceEquals(colliderContext, context),
 8403615            nameof(collider),
 8403616            "Agent and collider must be bound to the same context.");
 617
 8402618        Agent = agent;
 8402619        Collider = collider;
 8402620        Context = context;
 8402621        Collider.BindContext(context);
 622
 8402623        _positionTransform = agent.Transform;
 8402624        _rotationTransform = agent.Transform;
 625
 8402626        _rotationSpeed = DefaultRotationSpeed;
 8402627        _rotationInterpoleSpeed = Fixed64.Zero;
 8402628    }
 629
 630    /// <summary>Initializes and registers the body at an authoritative world pose.</summary>
 631    public void Initialize(
 632        Vector3d startPosition,
 633        FixedQuaternion startRotation,
 634        PhysicsMobilityType motionType = PhysicsMobilityType.Dynamic)
 635    {
 8428636        motionType.ThrowIfInvalid(nameof(motionType));
 8428637        ChronicleGuard.ThrowIfArgument(
 8428638            Collider.Id >= 0
 8428639                || (Collider.HasHostBinding && !ReferenceEquals(Collider.Body, this)),
 8428640            nameof(Collider),
 8428641            "Body collider must be unregistered and free of another host binding before initialization.");
 8426642        FixedQuaternion normalizedRotation = startRotation.Normalized;
 8426643        Collider.PreflightBodyInitialization(this, startPosition, normalizedRotation);
 644
 8416645        _motionType = motionType;
 8416646        Active = true;
 8416647        _collisionPositionPublicationPending = false;
 648
 8416649        InvalidateContinuousCollisionTrajectory();
 8416650        ClearMotionForSleep();
 8416651        _normalForce = Vector3d.Zero;
 8416652        _isSleeping = false;
 8416653        _sleepFrameCount = 0;
 654
 8416655        _isGrounded = false;
 8416656        _wasGrounded = false;
 8416657        _groundedTransitionCapturedForStep = false;
 8416658        _skipGroundingCheck = false;
 8416659        _lastGroundCheckFrame = -1;
 8416660        ResetGroundCalculations();
 661
 8416662        _positionChangedBuffer = true;
 8416663        _position2dUnmarked = startPosition.ToVector2d();
 8416664        _lastGroundedPosition = _lastPosition = startPosition;
 8416665        _heightPosUnmarked = startPosition.Y;
 666
 8416667        _rotationChangedBuffer = true;
 8416668        _rotation = normalizedRotation;
 669
 8416670        if (!IsKinematic)
 671        {
 7613672            _lastVisualPosition = _visualPosition = Position3d;
 7613673            _visualRotation = normalizedRotation;
 7613674            _lastVisualRotation = _visualRotation;
 675        }
 676
 8416677        OnVisualize();
 678
 8416679        _dynamicId = Context.Physics.AssimilateBody(this, motionType);
 8416680        Collider!.Initialize(this);
 8416681        RefreshMassPropertiesFromColliderShape();
 8415682        CheckGround(force: true);
 8415683    }
 684
 685
 686    /// <summary>Completes deferred body integration and post-step bookkeeping.</summary>
 687    /// <exception cref="OverflowException">An integrated or corrected body position cannot be represented.</exception>
 688    public void LateSimulate()
 689    {
 13690        Context.EnterSimulationPhase();
 691        try
 692        {
 13693            LateSimulate(updateSleepState: true, updateColliderState: true);
 13694        }
 695        finally
 696        {
 13697            Context.ExitSimulationPhase();
 13698        }
 13699    }
 700
 701    internal void LateSimulate(bool updateSleepState, bool updateColliderState)
 702    {
 74228703        if (!Active) return;
 704
 74226705        CaptureGroundedStepState();
 706        try
 707        {
 74226708            LastContinuousCollisionToiIterationCount = 0;
 74226709            LastContinuousCollisionToiIterationLimitReached = false;
 710
 74226711            if (!EnsureContinuousCollisionFramePrepared(Context.LateSimulateToken, refreshGroundSupport: true))
 1712                return;
 74225713            _lastPosition = Position3d;
 74225714            if (TryConsumeContinuousCollisionHandoff(updateSleepState, updateColliderState))
 63715                return;
 716
 74162717            if (IsKinematic)
 6514718                UpdateKinematicPositionAndRotation();
 719
 74162720            if (HasSolverMobility)
 721            {
 67633722                if (!IsSleeping)
 723                {
 65212724                    ProcessMovable();
 65211725                    if (updateSleepState)
 8726                        UpdateSleepState();
 727                }
 728
 67632729                if (updateColliderState)
 13730                    Collider!.Simulate();
 731            }
 732
 74161733            if (SettingVisuals)
 63166734                _settingVisualsCounter--;
 735
 74161736            if (PositionChangePending || RotationChangePending)
 70889737                OnMoved?.Invoke();
 3288738        }
 739        finally
 740        {
 74226741            CompleteGroundedStepState();
 74226742        }
 74225743    }
 744
 745    private void UpdateKinematicPositionAndRotation()
 746    {
 6514747        EnsureContinuousCollisionFramePrepared(Context.LateSimulateToken);
 6514748        Vector3d startPosition = Position3d;
 6514749        FixedQuaternion startRotation = Rotation;
 6514750        Vector3d requestedPosition = _positionTransform.WorldPosition;
 6514751        FixedQuaternion requestedRotation = _rotationTransform.WorldRotation;
 6514752        Vector3d kinematicPosition = ContinuousCollisionFrameEnd;
 6514753        FixedQuaternion kinematicRotation = ContinuousCollisionFrameTargetRotation;
 6514754        if (startPosition == kinematicPosition && startRotation == kinematicRotation)
 755        {
 6756            SetPositionTransformWorldPosition(kinematicPosition);
 6757            SetRotationTransformWorldRotation(kinematicRotation);
 6758            return;
 759        }
 760
 6508761        Wake();
 762
 6508763        Vector3d resolvedPosition = ProjectLinearEndpoint(startPosition, kinematicPosition);
 6508764        if (ShouldUseContinuousCollision(out _))
 6461765            _ = ContinuousCollisionSweepRange.ValidateEndpoint(startPosition, resolvedPosition, out _);
 6508766        FixedQuaternion resolvedRotation = kinematicRotation;
 6508767        if (!TryResolveKinematicRotationalContinuousCollision(
 6508768                startPosition,
 6508769                ref resolvedPosition,
 6508770                startRotation,
 6508771                ref resolvedRotation))
 772        {
 6352773            TryResolveKinematicContinuousCollision(startPosition, ref resolvedPosition);
 774        }
 775
 6508776        if (resolvedPosition != requestedPosition)
 6223777            SetPositionTransformWorldPosition(resolvedPosition);
 6508778        if (resolvedRotation != requestedRotation)
 42779            SetRotationTransformWorldRotation(resolvedRotation);
 780
 6508781        SetPosition2d(resolvedPosition.ToVector2d());
 6508782        HeightPos = resolvedPosition.Y;
 6508783        SetVisualPosition(resolvedPosition);
 784
 6508785        Rotation = resolvedRotation;
 6508786        StoreVisualRotation(resolvedRotation);
 6508787    }
 788
 789
 790    /// <summary>
 791    /// Puts the body to sleep and keeps it partitioned for queries and deterministic wake propagation.
 792    /// </summary>
 793    public void Sleep()
 794    {
 483795        if (!CanSleep)
 6796            return;
 797
 477798        _sleepFrameCount = _sleepFrameThreshold;
 477799        ClearMotionForSleep();
 477800        if (_isSleeping)
 2801            return;
 802
 475803        _isSleeping = true;
 475804        RefreshPartitionAwakeState();
 475805    }
 806
 807    /// <summary>
 808    /// Wakes a sleeping body because a deterministic simulation or host stimulus changed its state.
 809    /// </summary>
 810    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 811    public void Wake()
 812    {
 21420813        _sleepFrameCount = 0;
 21420814        if (!_isSleeping)
 21366815            return;
 816
 54817        _isSleeping = false;
 54818        RefreshPartitionAwakeState();
 54819    }
 820
 821    internal void WakeFromCollision()
 822    {
 497300823        if (!_isSleeping)
 497072824            return;
 825
 228826        _sleepFrameCount = 0;
 228827        _isSleeping = false;
 228828        RefreshPartitionAwakeState();
 228829    }
 830
 72602831    private bool CanSleep => SleepEnabled && HasSolverMobility;
 832
 833
 834    private void SetTransformWorldPose(Vector3d position, FixedQuaternion rotation)
 835    {
 7511836        Vector3d originalLocalPosition = _positionTransform.LocalPosition;
 7511837        FixedQuaternion originalLocalRotation = _positionTransform.LocalRotation;
 7511838        ChronicleGuard.ThrowIfTrue(
 7511839            !_positionTransform.TrySetWorldPose(position, rotation),
 7511840            nameof(FixedTransform),
 7511841            "Host transform cannot represent the requested world pose.");
 842
 843        try
 844        {
 7510845            if (Active)
 846            {
 7509847                ChronicleGuard.ThrowIfTrue(
 7509848                    !Collider.TryPrepareBodyPose(position, rotation),
 7509849                    nameof(position),
 7509850                    "The requested body pose produces collider geometry outside the representable coordinate domain.");
 851            }
 7506852        }
 4853        catch
 854        {
 4855            _positionTransform.LocalPosition = originalLocalPosition;
 4856            _positionTransform.LocalRotation = originalLocalRotation;
 4857            throw;
 858        }
 7506859    }
 860
 861    /// <summary>Releases the body's collider and context-local registration.</summary>
 862    public void Deactivate()
 863    {
 179864        if (!ReferenceEquals(Collider.Body, this))
 11865            return;
 866
 168867        DiscardContinuousCollisionHandoff();
 168868        InvalidateContinuousCollisionTrajectory();
 168869        Collider.DeactivateRuntimeRegistration();
 167870        Context.Physics.DessimilateBody(this);
 167871        _dynamicId = -1;
 167872        Active = false;
 167873    }
 874
 875    /// <summary>Sets an authoritative target rotation and visual interpolation rate.</summary>
 876    public void UpdateRotation(FixedQuaternion targetRotation, Fixed64 bufferInterpolation)
 877    {
 10878        FixedQuaternion normalizedRotation = targetRotation.Normalized;
 10879        PreflightStaticPoseChange();
 9880        bool preparedPose = PrepareExplicitBodyPose(
 9881            Position3d,
 9882            normalizedRotation,
 9883            nameof(targetRotation),
 9884            "The requested target rotation produces collider geometry outside the representable coordinate domain.");
 9885        _rotationInterpoleSpeed = bufferInterpolation;
 9886        _rotationSpeed = Agent.IsInteracting
 9887            ? InteractionRotationSpeed
 9888            : DefaultRotationSpeed;
 9889        bool poseChanged = Rotation != normalizedRotation;
 9890        Rotation = normalizedRotation;
 9891        PublishExplicitBodyPose(preparedPose, poseChanged);
 9892    }
 893
 894    /// <summary>
 895    /// Gets a world-space point from the authoritative body pose and committed collider scale.
 896    /// </summary>
 897    /// <param name="point">The body-local point.</param>
 898    /// <returns>The corresponding world-space point.</returns>
 899    /// <exception cref="InvalidOperationException">
 900    /// The collider has no committed scale or the final world-space point is not representable.
 901    /// </exception>
 902    public Vector3d GetWorldPoint(Vector3d point)
 903    {
 7904        bool transformed = TryGetWorldPoint(point, out Vector3d result);
 7905        ChronicleGuard.ThrowIfTrue(
 7906            !transformed,
 7907            nameof(Collider),
 7908            "Cannot get the world point because the collider has no committed scale or the final coordinate is not repre
 5909        return result;
 910    }
 911
 912    /// <summary>
 913    /// Attempts to get a world-space point from the authoritative body pose and committed collider scale.
 914    /// </summary>
 915    /// <param name="point">The body-local point.</param>
 916    /// <param name="result">The world-space point on success; otherwise zero.</param>
 917    /// <returns><see langword="true"/> when a committed scale exists and the final point is representable.</returns>
 918    public bool TryGetWorldPoint(Vector3d point, out Vector3d result)
 919    {
 221920        if (!Collider.TryGetCommittedOwnerScale(out Vector3d scale))
 921        {
 2922            result = Vector3d.Zero;
 2923            return false;
 924        }
 925
 219926        return Rotation.TryTransformScaledPoint(Position3d, point, scale, out result);
 927    }
 928
 929    /// <summary>
 930    /// Gets a body-local point from the authoritative body pose and committed collider scale.
 931    /// </summary>
 932    /// <param name="point">The world-space point.</param>
 933    /// <returns>The corresponding body-local point.</returns>
 934    /// <exception cref="InvalidOperationException">
 935    /// The collider has no committed scale, its scale is singular, or the final body-local point is not representable.
 936    /// </exception>
 937    public Vector3d GetLocalPoint(Vector3d point)
 938    {
 5939        bool transformed = TryGetLocalPoint(point, out Vector3d result);
 5940        ChronicleGuard.ThrowIfTrue(
 5941            !transformed,
 5942            nameof(Collider),
 5943            "Cannot get the local point because the collider has no committed scale, its scale is singular, or the final
 3944        return result;
 945    }
 946
 947    /// <summary>
 948    /// Attempts to get a body-local point from the authoritative body pose and committed collider scale.
 949    /// </summary>
 950    /// <param name="point">The world-space point.</param>
 951    /// <param name="result">The body-local point on success; otherwise zero.</param>
 952    /// <returns>
 953    /// <see langword="true"/> when a committed nonsingular scale exists and the final point is representable.
 954    /// </returns>
 955    public bool TryGetLocalPoint(Vector3d point, out Vector3d result)
 956    {
 219957        if (!Collider.TryGetCommittedOwnerScale(out Vector3d scale))
 958        {
 2959            result = Vector3d.Zero;
 2960            return false;
 961        }
 962
 217963        return Rotation.TryInverseTransformScaledPoint(Position3d, point, scale, out result);
 964    }
 965
 966    /// <summary>Resets the authoritative pose, motion, grounding, and presentation state.</summary>
 967    public void ResetPosition(Vector3d position = default, FixedQuaternion rotation = default)
 968    {
 7513969        FixedQuaternion normalizedRotation = rotation.Normalized;
 7513970        PreflightResetPoseChange();
 7511971        SetTransformWorldPose(position, normalizedRotation);
 7506972        InvalidateContinuousCollisionTrajectory();
 7506973        ClearMotionForSleep();
 7506974        bool wasSleeping = _isSleeping;
 7506975        _isSleeping = false;
 7506976        _sleepFrameCount = 0;
 7506977        _normalForce = Vector3d.Zero;
 978
 7506979        SetPosition2d(position.ToVector2d());
 7506980        HeightPos = position.Y;
 7506981        _lastPosition = position;
 7506982        _lastVisualPosition = _visualPosition = position;
 7506983        Rotation = normalizedRotation;
 984
 7506985        _visualRotation = normalizedRotation;
 986
 7506987        PublishExplicitBodyPose(Active, poseChanged: true);
 7506988        if (wasSleeping)
 27989            RefreshPartitionAwakeState();
 7506990    }
 991
 992}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.Grounding.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody.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.Collections.Generic;
 10using System.Runtime.CompilerServices;
 11using FixedMathSharp;
 12using Gravitas.Colliders;
 13using Gravitas.CollisionHandling;
 14using Gravitas.Queries;
 15using Gravitas.Support;
 16using SwiftCollections;
 17
 18namespace Gravitas;
 19
 20/// <content>Maintains spatial ground probes, platform contacts, and grounded-state transitions.</content>
 21public partial class SolidBody
 22{
 23    #region Grounding
 24
 25    private bool _skipGroundingCheck = false;
 26
 27    /// <summary>
 28    /// Selects whether Gravitas probes for ground automatically or preserves host-supplied grounding state.
 29    /// </summary>
 30    public GroundingMode GroundingMode { get; private set; } = GroundingMode.Automatic;
 31
 32    // how close to the actor's feet (or whatever touches the ground) do we check for grounding
 33    /// <summary>Vertical offset applied to the origin of automatic ground probes.</summary>
 840334    public Fixed64 GroundOriginOffset = (Fixed64)0.5f;
 35
 36    /// <summary>Automatic ground-probe distance used while grounded.</summary>
 840337    public Fixed64 GroundedDistanceRay = (Fixed64)0.5f;
 38
 39    /// <summary>Automatic ground-probe distance used while airborne.</summary>
 840340    public Fixed64 GroundDownDistanceOnAir = (Fixed64)0.5f;
 41
 42    /// <summary>
 43    /// Selects the deterministic query primitive used for ground checks.
 44    /// </summary>
 45    public GroundProbeMode GroundProbeMode
 46    {
 1872247        get => _groundProbeMode;
 48        set
 49        {
 24650            if (_groundProbeMode == value) return;
 6851            _groundProbeMode = value;
 13552            if (GroundingMode == GroundingMode.Automatic) _lastGroundCheckFrame = -1;
 6853        }
 54    }
 55
 56    private GroundProbeMode _groundProbeMode = GroundProbeMode.Auto;
 57
 58    /// <summary>
 59    /// Optional explicit radius for swept-sphere ground probes. Nonpositive values derive it from the collider.
 60    /// </summary>
 61    public Fixed64 GroundProbeRadius
 62    {
 1941363        get => _groundProbeRadius;
 64        set
 65        {
 15466            ChronicleGuard.ThrowIfArgument(value > ColliderGeometryPolicy.MaximumGeometryReach,
 15467                nameof(value), "Ground probe radius must not exceed 4,096 world units.");
 23768            if (_groundProbeRadius == value) return;
 6969            _groundProbeRadius = value;
 13770            if (GroundingMode == GroundingMode.Automatic) _lastGroundCheckFrame = -1;
 6971        }
 72    }
 73
 74    private Fixed64 _groundProbeRadius;
 75
 840376    private Fixed64 _groundMinNormalDot = Fixed64.Half;
 77
 78    /// <summary>
 79    /// Minimum normalized support-normal dot product with world up, in [0, 1].
 80    /// Defaults to one half (slopes through 60 degrees). Zero permits any
 81    /// upward-facing normal; horizontal, downward-facing and zero normals never support.
 82    /// </summary>
 83    public Fixed64 GroundMinNormalDot
 84    {
 600585        get => _groundMinNormalDot;
 86        set
 87        {
 3888            ValidateGroundMinNormalDot(value);
 3689            if (_groundMinNormalDot == value)
 290                return;
 91
 3492            _groundMinNormalDot = value;
 3493            if (GroundingMode == GroundingMode.Automatic)
 94            {
 95                // Cached support and sleeping motion were accepted under the old
 96                // slope policy. Reconsider them on the next authoritative step.
 2997                _lastGroundCheckFrame = -1;
 2998                Wake();
 99            }
 34100        }
 101    }
 102
 8403103    private long _lastGroundCheckFrame = -1;
 104    private const int _groundCheckFrameThreshold = 10;
 8403105    private readonly Fixed64 _groundCheckThreshold = (Fixed64)0.01f;
 8403106    private readonly SwiftList<Physics3DHit> _groundProbeHits = new(DefaultBodyHitBufferCapacity);
 107
 108    /// <summary>Minimum downward snap distance used to preserve grounding across steps.</summary>
 8403109    public Fixed64 StepOffset = (Fixed64)0.5f;
 110
 8403111    private Vector3d _groundNormal = Vector3d.Zero;
 112    /// <summary>Gets the world-space normal of the current supporting surface.</summary>
 107113    public Vector3d GroundNormal => _groundNormal;
 114
 115    private FixedTransform? _hitPlatform;
 116    private ColliderLifetimeToken _groundSupport;
 117    private long _groundingStateVersion;
 118    private uint _groundSupportVersion;
 119    private uint _groundProbeShapeVersion;
 120    private Fixed64 _groundSupportOriginOffset;
 121    private Fixed64 _groundSupportProbeDistance;
 122    /// <summary>Gets the transform of the current supporting collider, if available.</summary>
 42123    public FixedTransform? HitPlatform => _hitPlatform;
 124
 125    private Vector3d _hitPlatformPosition;
 126
 127    private Vector3d _hitPoint;
 128    private bool _hasHitPoint;
 129
 130    /// <summary>
 131    /// Gets whether the current ground point is representable.
 132    /// </summary>
 38133    public bool HasHitPoint => _hasHitPoint;
 134
 135    /// <summary>
 136    /// Gets the current materialized ground point.
 137    /// </summary>
 138    /// <exception cref="InvalidOperationException">
 139    /// No representable ground point is available.
 140    /// </exception>
 141    public Vector3d HitPoint =>
 61142        _hasHitPoint || !_isGrounded
 61143            ? _hitPoint
 61144            : throw new InvalidOperationException(
 61145                "No representable ground point is available. Use TryGetHitPoint.");
 146
 147    /// <summary>
 148    /// Attempts to get the current materialized ground point.
 149    /// </summary>
 150    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 151    public bool TryGetHitPoint(out Vector3d hitPoint)
 152    {
 2880153        hitPoint = _hitPoint;
 2880154        return _hasHitPoint;
 155    }
 156
 157    /// <summary>Raised when the grounded state changes.</summary>
 158    public Action<bool>? OnGrounded;
 159
 160    private bool _isGrounded;
 161    /// <summary>Gets whether the body currently has valid ground support.</summary>
 162    public bool IsGrounded
 163    {
 14091164        get => _isGrounded;
 165        private set
 166        {
 15342167            if (_isGrounded == value)
 13932168                return;
 1410169            _isGrounded = value;
 1410170            OnGrounded?.Invoke(value);
 22171        }
 172    }
 173
 174    private bool _wasGrounded;
 175    private bool _groundedTransitionCapturedForStep;
 176
 177    /// <summary>
 178    /// Gets whether this body was grounded before the latest authoritative grounding refresh or manual grounding change
 179    /// </summary>
 64180    public bool WasGrounded => _wasGrounded;
 181
 182    private Vector3d _lastGroundedPosition;
 183    /// <summary>Gets the body position captured before its latest grounded movement.</summary>
 1184    public Vector3d LastGroundedPosition => _lastGroundedPosition;
 185
 186    #endregion
 187
 188    /// <summary>Temporarily disables automatic ground probes and clears grounded state.</summary>
 189    public void SkipGrounding(Fixed64 secs)
 190    {
 1191        _skipGroundingCheck = true;
 1192        CaptureGroundedTransitionState();
 1193        ClearGrounding();
 1194        Context.Coroutines.StartCoroutine(SkipGroundingCoroutine(secs));
 1195    }
 196
 197    /// <summary>
 198    /// Gives the host ownership of grounded state and disables automatic ground probes.
 199    /// </summary>
 200    public void UseManualGrounding(bool clearGrounding = true)
 201    {
 1242202        GroundingMode = GroundingMode.Manual;
 1242203        _skipGroundingCheck = false;
 1242204        if (clearGrounding)
 205        {
 1239206            CaptureGroundedTransitionState();
 1239207            ClearGrounding();
 208        }
 1242209    }
 210
 211    /// <summary>
 212    /// Returns grounded-state ownership to Gravitas and optionally refreshes the automatic probe immediately.
 213    /// </summary>
 214    public void UseAutomaticGrounding(bool checkGroundImmediately = true)
 215    {
 18216        GroundingMode = GroundingMode.Automatic;
 18217        if (checkGroundImmediately && Active)
 1218            CheckGround();
 18219    }
 220
 221    /// <summary>
 222    /// Sets host-owned grounded state for manual grounding sources such as deterministic heightmaps.
 223    /// </summary>
 224    public void SetManualGrounding(Vector3d hitPoint, Vector3d groundNormal, FixedTransform? hitPlatform = null)
 225    {
 310226        ChronicleGuard.ThrowIfArgument(
 310227            groundNormal.MagnitudeSquared <= Fixed64.Epsilon,
 310228            nameof(groundNormal),
 310229            "Manual grounding normal must be non-zero.");
 230
 310231        GroundingMode = GroundingMode.Manual;
 310232        _skipGroundingCheck = false;
 310233        CaptureGroundedTransitionState();
 310234        SetGroundingState(hitPoint, groundNormal.Normalized, hitPlatform);
 310235    }
 236
 237    /// <summary>
 238    /// Clears host-owned grounded state while leaving automatic probes disabled.
 239    /// </summary>
 240    public void ClearManualGrounding()
 241    {
 3242        GroundingMode = GroundingMode.Manual;
 3243        _skipGroundingCheck = false;
 3244        CaptureGroundedTransitionState();
 3245        ClearGrounding();
 3246    }
 247
 248    private IEnumerator<ILockedYieldInstruction> SkipGroundingCoroutine(Fixed64 secs)
 249    {
 1250        yield return Context.Coroutines.WaitForRealSeconds(secs);
 1251        _skipGroundingCheck = false;
 1252    }
 253
 254    /// <summary>Immediately refreshes automatic grounded state.</summary>
 255    public void CheckGround()
 256    {
 111257        CaptureGroundedTransitionState();
 111258        CheckGround(force: true);
 111259    }
 260
 261    private static void ValidateGroundMinNormalDot(Fixed64 value) =>
 131262        ChronicleGuard.ThrowIfArgument(value < Fixed64.Zero || value > Fixed64.One,
 131263            nameof(value), "Ground support normal threshold must be between zero and one.");
 264
 65371265    private void CheckGroundForSimulation() => CheckGround(force: false);
 266
 219184267    private bool HasCurrentGroundSupport => _isGrounded
 219184268        && (GroundingMode == GroundingMode.Manual || _lastGroundCheckFrame >= 0);
 269
 270    internal void WakeForContactChange()
 271    {
 1469272        if (!IsSleeping)
 1418273            return;
 51274        if (GroundingMode == GroundingMode.Automatic)
 4275            _lastGroundCheckFrame = -1;
 51276        WakeFromCollision();
 51277    }
 278
 279    private bool RefreshAutomaticGroundSupport()
 280    {
 148632281        if (GroundingMode != GroundingMode.Automatic || !_isGrounded)
 143257282            return true;
 283
 5375284        if (_lastGroundCheckFrame >= 0 && HasCurrentGroundSupportDependency())
 5296285            return true;
 286
 79287        var registration = new ColliderLifetimeToken(Collider);
 79288        long lastGroundCheckFrame = _lastGroundCheckFrame;
 79289        CaptureGroundedStepState();
 79290        CheckGround(force: true);
 79291        if (!registration.IsActive)
 3292            return false;
 293
 294        // Reconstructing a derived cache must not change an unchanged sleeper's saved probe clock.
 76295        if (lastGroundCheckFrame >= 0 && IsSleeping && GroundingMode == GroundingMode.Automatic)
 8296            _lastGroundCheckFrame = lastGroundCheckFrame;
 76297        return true;
 298    }
 299
 300    private bool HasCurrentGroundSupportDependency() =>
 6504301        _groundSupport.Collider != null && _groundSupport.IsActive
 6504302        && _groundSupportVersion == _groundSupport.Collider.ContactDependencyVersion
 6504303        && _groundProbeShapeVersion == Collider.ContactDependencyVersion
 6504304        && _groundSupportOriginOffset == GroundOriginOffset
 6504305        && _groundSupportProbeDistance == GroundedDistanceRay
 6504306        && IsValidGroundCollider(_groundSupport.Collider);
 307
 308    private void CaptureGroundedTransitionState()
 309    {
 74562310        _wasGrounded = _isGrounded;
 74562311        _groundedTransitionCapturedForStep = true;
 74562312    }
 313
 314    private void CaptureGroundedStepState()
 315    {
 74305316        if (!_groundedTransitionCapturedForStep)
 72898317            CaptureGroundedTransitionState();
 74305318    }
 319
 74226320    private void CompleteGroundedStepState() => _groundedTransitionCapturedForStep = false;
 321
 322    private void CheckGround(bool force)
 323    {
 73976324        if (GroundingMode == GroundingMode.Manual)
 55186325            return;
 326
 18790327        if (_skipGroundingCheck || World is null)
 328        {
 1329            ClearGrounding();
 1330            return;
 331        }
 332
 333        // Only perform SphereCast if enough frames have passed
 18789334        bool hitPlatformMoved = _hitPlatform != null && _hitPlatform.WorldPosition != _hitPlatformPosition;
 18789335        bool frameGuard = !force
 18789336            && !hitPlatformMoved
 18789337            && Vector3d.Distance(_lastPosition, Position3d) < _groundCheckThreshold
 18789338            && _lastGroundCheckFrame >= 0
 18789339            && Context.FrameCount - _lastGroundCheckFrame < _groundCheckFrameThreshold;
 18789340        if (frameGuard)
 5007341            return;
 342
 13782343        _lastGroundCheckFrame = Context.FrameCount;
 344        // We want origin to be close to the actor's feet
 13782345        Vector3d origin = Position3d;
 346        // but not to close...
 13782347        origin.Y += GroundOriginOffset;
 348
 13782349        Fixed64 dis = GroundedDistanceRay;
 13782350        if (!IsGrounded)
 13452351            dis = GroundDownDistanceOnAir;
 352
 13782353        GroundProbeMode mode = ResolveGroundProbeMode();
 13782354        Fixed64 radius = mode == GroundProbeMode.SweptSphere
 13782355            ? ResolveGroundProbeRadius()
 13782356            : Fixed64.Zero;
 13782357        Vector3d end = origin + Vector3d.Down * dis;
 358        Physics3DHit hit;
 359        bool foundGround;
 360        try
 361        {
 13782362            foundGround = TryFindGroundHit(mode, radius, origin, dis, out hit);
 13782363        }
 364        finally
 365        {
 13782366            _groundProbeHits.Clear();
 13782367        }
 13782368        Context.Diagnostics.EmitGroundProbe(this, mode, origin, end, radius, foundGround, hit);
 369
 13782370        if (!foundGround)
 371        {
 12642372            ClearGrounding();
 12642373            return;
 374        }
 375
 1140376        var registration = new ColliderLifetimeToken(Collider);
 1140377        _groundSupport = new ColliderLifetimeToken(hit.Collider!);
 1140378        _groundSupportVersion = hit.Collider!.ContactDependencyVersion;
 1140379        _groundProbeShapeVersion = Collider.ContactDependencyVersion;
 1140380        _groundSupportOriginOffset = GroundOriginOffset;
 1140381        _groundSupportProbeDistance = GroundedDistanceRay;
 1140382        long groundingStateVersion = SetGroundingState(hit.Anchor, hit.Normal, hit.Collider.Transform);
 383        // Callbacks may replace this body, its support, or automatic grounding ownership.
 1140384        if (registration.IsCurrentLifetime && _groundingStateVersion == groundingStateVersion
 1140385            && GroundingMode == GroundingMode.Automatic
 1140386            && (!registration.IsActive || _lastGroundCheckFrame < 0 || !HasCurrentGroundSupportDependency()))
 6387            ClearGrounding();
 1140388    }
 389
 390    private bool TryFindGroundHit(
 391        GroundProbeMode mode,
 392        Fixed64 radius,
 393        Vector3d origin,
 394        Fixed64 distance,
 395        out Physics3DHit hit)
 396    {
 13782397        if (mode == GroundProbeMode.SweptSphere && TryFindGroundHitWithSweptSphere(origin, distance, radius, out hit))
 1074398            return true;
 399
 12708400        if (mode == GroundProbeMode.SweptSphere)
 401        {
 11072402            hit = default;
 11072403            return false;
 404        }
 405
 1636406        return TryFindGroundHitWithRay(origin, distance, out hit);
 407    }
 408
 409    private bool TryFindGroundHitWithRay(Vector3d origin, Fixed64 distance, out Physics3DHit hit)
 410    {
 1647411        Vector3d end = origin + Vector3d.Down * distance;
 1647412        int hitCount = Context.Query3D.RaycastAll(origin, end, Context.Settings.GroundCheckLayerMask, _groundProbeHits);
 6000413        for (int i = 0; i < hitCount; i++)
 414        {
 1422415            Physics3DHit current = _groundProbeHits[i];
 1422416            if (!IsValidGroundHit(current))
 417                continue;
 418
 69419            hit = current;
 69420            return true;
 421        }
 422
 1578423        hit = default;
 1578424        return false;
 425    }
 426
 427    private bool TryFindGroundHitWithSweptSphere(Vector3d origin, Fixed64 distance, Fixed64 radius, out Physics3DHit hit
 428    {
 12146429        if (radius <= Fixed64.Epsilon)
 11430            return TryFindGroundHitWithRay(origin, distance, out hit);
 431
 12135432        Vector3d end = origin + Vector3d.Down * distance;
 12135433        int hitCount = Context.Query3D.SweepSphereAll(
 12135434            origin,
 12135435            end,
 12135436            radius,
 12135437            Context.Settings.GroundCheckLayerMask,
 12135438            _groundProbeHits,
 12135439            Collider);
 440
 28196441        for (int i = 0; i < hitCount; i++)
 442        {
 3034443            Physics3DHit current = _groundProbeHits[i];
 3034444            if (!IsValidGroundHit(current))
 445                continue;
 446
 1071447            hit = current;
 1071448            return true;
 449        }
 450
 11064451        hit = default;
 11064452        return false;
 453    }
 454
 455    private bool IsValidGroundHit(Physics3DHit hit)
 456    {
 4456457        LSCollider hitCollider = hit.Collider!;
 4456458        if (ReferenceEquals(hitCollider, Collider))
 1287459            return false;
 460
 3169461        if (!IsValidGroundCollider(hitCollider))
 1773462            return false;
 463
 464        // A sweep can start tangent to a wall or inside geometry. Its valid
 465        // intersection witness is not necessarily upward support. Reject those
 466        // normals before normalization, even with the most permissive slope limit.
 1396467        return hit.Normal.Y > Fixed64.Zero
 1396468            && hit.Normal.Normalized.Y >= GroundMinNormalDot;
 469    }
 470
 471    private bool IsValidGroundCollider(LSCollider collider) =>
 9602472        !collider.IsTrigger
 9602473        && Context.Settings.GroundCheckLayerMask.Includes(collider.Layer)
 9602474        && ColliderCollisionFilter.AllowsPhysicalPair(Collider, collider)
 9602475        && (collider.IsStatic || collider.Body!.IsKinematic);
 476
 477    private GroundProbeMode ResolveGroundProbeMode()
 478    {
 13782479        if (GroundProbeMode != GroundProbeMode.Auto)
 89480            return GroundProbeMode;
 481
 13693482        return Collider is LSSphereCollider
 13693483            || Collider is LSCapsuleCollider
 13693484            || Collider is LSCylinderCollider
 13693485            || (Collider is LSCuboidCollider && ResolveGroundProbeRadius() > Fixed64.FromFraction(1, 8))
 13693486            || (Collider is LSCompoundCollider && ResolveGroundProbeRadius() > Fixed64.FromFraction(1, 8))
 13693487                ? GroundProbeMode.SweptSphere
 13693488                : GroundProbeMode.Ray;
 489    }
 490
 491    private Fixed64 ResolveGroundProbeRadius()
 492    {
 14512493        if (GroundProbeRadius > Fixed64.Zero)
 53494            return GroundProbeRadius;
 495
 14459496        return Collider switch
 14459497        {
 14459498            LSSphereCollider or
 14459499            LSCapsuleCollider or
 14459500            LSCylinderCollider or
 14459501            LSCuboidCollider or
 14459502            LSCompoundCollider =>
 14458503                Collider.CanonicalGroundProbeRadius,
 1504            _ => Fixed64.Zero
 14459505        };
 506    }
 507
 508    private void ClearGrounding()
 509    {
 13892510        _groundingStateVersion++;
 13892511        if (GroundingMode == GroundingMode.Automatic && _isGrounded)
 71512            WakeFromCollision();
 13892513        ResetGroundCalculations();
 13892514        IsGrounded = false;
 13892515    }
 516
 517    private void ApplyGroundedHeightOrReset()
 518    {
 65371519        if (_isGrounded)
 520        {
 2829521            if (TryGetHitPoint(out Vector3d hitPoint))
 2828522                HeightPos = hitPoint.Y;
 2829523            return;
 524        }
 525
 62542526        ResetGroundCalculations();
 62542527    }
 528
 529    private void SetGroundingState(Vector3d hitPoint, Vector3d groundNormal, FixedTransform? hitPlatform)
 530    {
 310531        _groundingStateVersion++;
 310532        _groundSupport = default;
 310533        _groundSupportVersion = 0;
 310534        _hitPlatform = hitPlatform;
 310535        _hitPlatformPosition = _hitPlatform?.WorldPosition ?? Vector3d.Zero;
 310536        _hitPoint = hitPoint;
 310537        _hasHitPoint = true;
 310538        _groundNormal = groundNormal;
 539
 310540        RefreshGroundNormalForce();
 310541        IsGrounded = true;
 310542    }
 543
 544    private long SetGroundingState(ContactAnchor anchor, Vector3d groundNormal, FixedTransform hitPlatform)
 545    {
 1140546        long groundingStateVersion = ++_groundingStateVersion;
 1140547        bool hadPoint = _hasHitPoint;
 1140548        Vector3d oldPoint = _hitPoint;
 1140549        _hitPlatform = hitPlatform;
 1140550        _hitPlatformPosition = hitPlatform.WorldPosition;
 1140551        _hasHitPoint = anchor.TryGetWorldPoint(out _hitPoint);
 1140552        if (_isGrounded
 1140553            && (_groundNormal != groundNormal || hadPoint != _hasHitPoint
 1140554                || (_hasHitPoint && oldPoint != _hitPoint)))
 53555            WakeFromCollision();
 1140556        _groundNormal = groundNormal;
 557
 1140558        RefreshGroundNormalForce();
 1140559        IsGrounded = true;
 1140560        return groundingStateVersion;
 561    }
 562
 563    private void RefreshGroundNormalForce()
 564    {
 1551565        if (!_isGrounded && _groundNormal.MagnitudeSquared <= Fixed64.Epsilon)
 566        {
 88567            _normalForce = Vector3d.Zero;
 88568            return;
 569        }
 570
 1463571        Vector3d weightVector = Weight * Vector3d.Down;
 1463572        Fixed64 weightInNormalDirection = Vector3d.Dot(weightVector, _groundNormal);
 1463573        _normalForce = weightInNormalDirection * _groundNormal;
 1463574    }
 575
 576    private void ResetGroundCalculations()
 577    {
 84850578        _hitPlatform = null;
 84850579        _groundSupport = default;
 84850580        _groundSupportVersion = 0;
 84850581        _hitPlatformPosition = Vector3d.Zero;
 84850582        _hitPoint = Vector3d.Zero;
 84850583        _hasHitPoint = false;
 84850584        _groundNormal = Vector3d.Zero;
 84850585        _normalForce = Vector3d.Zero;
 84850586    }
 587
 588}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.MassProperties.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody.MassProperties.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;
 9
 10namespace Gravitas;
 11
 12/// <content>Maintains spatial mass and inertia tensors and derives effective mobility for the solver.</content>
 13public partial class SolidBody
 14{
 8651615    private bool CanUseAngularInertia => IsDynamic && !IsRotationFullyFrozen;
 16
 17    private Fixed64 _mass;
 18
 19    /// <summary>Gets or sets mass in kilograms. Non-positive mass disables solver motion.</summary>
 20    /// <remarks>
 21    /// A changed runtime value must be assigned between fixed steps. It refreshes inertia,
 22    /// invalidates cached impulses and CCD state, and wakes the body without changing its
 23    /// role, pose, velocities, or already accepted accelerations. Equal values are a no-op.
 24    /// </remarks>
 25    public Fixed64 Mass
 26    {
 2408475227        get => _mass;
 28        set
 29        {
 886930            if (_mass == value)
 2131                return;
 32
 884833            if (!Active)
 34            {
 833235                _mass = value;
 833236                return;
 37            }
 38
 51639            ThrowIfRuntimeRegistrationMissing();
 51440            Context.ThrowIfFixedStepMutationNotAllowed();
 51141            Collider.ValidateCurrentRuntimeTransform();
 42            // Calculate before publishing mass: custom collider math may throw.
 51143            RefreshInertiaTensor(value);
 51044            _mass = value;
 51045            InvalidateMassDependentSolverState();
 51046            InvalidateContinuousCollisionTrajectory();
 51047            _sleepFrameCount = 0;
 51048            _isSleeping = false;
 49            // A zero-mass body can already be awake; Wake alone cannot add it back.
 51050            RefreshPartitionAwakeState();
 51051        }
 52    }
 53
 54    private void InvalidateMassDependentSolverState()
 55    {
 66056        Context.Physics.ClearWarmStartCachesForCollider(Collider);
 66057        Context.Constraints3D.ClearSolverCachesForBody(this);
 66058        Context.Physics.InvalidateContinuousCollisionStateForBodyMutation(this, DynamicId);
 66059    }
 60
 61    /// <summary>Gets the reciprocal mass, or zero when <see cref="Mass"/> is non-positive.</summary>
 1199520362    public Fixed64 InverseMass => Mass > Fixed64.Zero
 1199520363        ? Fixed64.One / Mass
 1199520364        : Fixed64.Zero;
 65
 66    private Fixed3x3 _inertiaTensor;
 67    private Fixed3x3 _worldInertiaTensor;
 68    private Fixed3x3 _inverseLocalInertiaTensor;
 69    private Fixed3x3 _inverseInertiaTensor;
 70    /// <summary>Gets the constrained world-space inverse inertia tensor.</summary>
 6871    public Fixed3x3 InverseInertiaTensor => _inverseInertiaTensor;
 72
 73    /// <summary>
 74    /// Gets whether solver-side response may translate this body.
 75    /// </summary>
 992869976    public bool CanTranslate => Active && _dynamicId >= 0 && IsDynamic && !IsPositionFullyFrozen && InverseMass > Fixed6
 77
 78    /// <summary>
 79    /// Gets whether solver-side response may rotate this body.
 80    /// </summary>
 500952881    public bool CanRotate => Active
 500952882        && _dynamicId >= 0
 500952883        && IsDynamic
 500952884        && !IsRotationFullyFrozen
 500952885        && _inverseInertiaTensor != Fixed3x3.Zero;
 86
 651918887    internal bool HasSolverMobility => CanTranslate || CanRotate;
 88
 89    /// <summary>
 90    /// Gets the inverse mass that should be used by collision response.
 91    /// Translation-frozen, static, and kinematic bodies expose their raw mass
 92    /// but contribute zero constrained inverse mass.
 93    /// </summary>
 42454394    public Fixed64 EffectiveInverseMass => CanTranslate ? InverseMass : Fixed64.Zero;
 95
 96    /// <summary>
 97    /// Gets the inverse inertia tensor that should be used by collision response.
 98    /// Bodies that cannot rotate expose a zero tensor even when raw inertia is available.
 99    /// </summary>
 874100    public Fixed3x3 EffectiveInverseInertiaTensor => CanRotate ? _inverseInertiaTensor : Fixed3x3.Zero;
 101
 102    internal void RefreshMassPropertiesFromColliderShape()
 103    {
 85515104        if (!_centerOfMassOffsetExplicit)
 85496105            _localCenterOfMassOffset = Collider.CalculateLocalCenterOfMassOffset();
 106
 85515107        RefreshInertiaTensor(Mass);
 85514108    }
 109
 110    private void UpdateInertiaTensorOrientation()
 111    {
 23520112        CalculateInertiaTensorOrientation(_inertiaTensor, _inverseLocalInertiaTensor,
 23520113            out _worldInertiaTensor, out _inverseInertiaTensor);
 23520114    }
 115
 116    private void CalculateInertiaTensorOrientation(
 117        Fixed3x3 inertia,
 118        Fixed3x3 inverseLocalInertia,
 119        out Fixed3x3 worldInertia,
 120        out Fixed3x3 inverseInertia)
 121    {
 110034122        if (inertia == Fixed3x3.Zero || inverseLocalInertia == Fixed3x3.Zero)
 123        {
 56688124            worldInertia = Fixed3x3.Zero;
 56688125            inverseInertia = Fixed3x3.Zero;
 56688126            return;
 127        }
 128
 129        // Convert the row-vector matrix convention to active tensor transport.
 53346130        Fixed3x3 inverseOrientation = Rotation.ToMatrix3x3();
 53346131        Fixed3x3 orientation = Rotation.Conjugate().ToMatrix3x3();
 132
 53346133        worldInertia = orientation * inertia * inverseOrientation;
 53346134        inverseInertia = orientation * inverseLocalInertia * inverseOrientation;
 53346135    }
 136
 137    private void RefreshInertiaTensor(Fixed64 mass)
 138    {
 86516139        Fixed3x3 inertia = CanUseAngularInertia && mass > Fixed64.Zero
 86516140            ? Collider.CalculateInertiaTensor(mass, _localCenterOfMassOffset)
 86516141            : Fixed3x3.Zero;
 86514142        Fixed3x3 inverseLocalInertia = InertiaTensorMath.InvertForSolver(inertia);
 86514143        CalculateInertiaTensorOrientation(inertia, inverseLocalInertia,
 86514144            out Fixed3x3 worldInertia, out Fixed3x3 inverseInertia);
 145
 146        // Publish the complete tensor state only after all derived math succeeds.
 86514147        _inertiaTensor = inertia;
 86514148        _inverseLocalInertiaTensor = inverseLocalInertia;
 86514149        _worldInertiaTensor = worldInertia;
 86514150        _inverseInertiaTensor = inverseInertia;
 86514151    }
 152
 153}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.Motion.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody.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>Projects spatial motion, applies forces and impulses, and updates the body pose and sleep state.</content>
 15public partial class SolidBody
 16{
 17    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 18    internal Vector3d ProjectLinearMotion(Vector3d value)
 19    {
 477193020        if (value == Vector3d.Zero || IsPositionFullyFrozen)
 76273821            return Vector3d.Zero;
 22
 400919223        Fixed64 x = (_freezeAxes & BodyFreezeAxes3D.PositionX) == BodyFreezeAxes3D.PositionX ? Fixed64.Zero : value.X;
 400919224        Fixed64 y = (_freezeAxes & BodyFreezeAxes3D.PositionY) == BodyFreezeAxes3D.PositionY ? Fixed64.Zero : value.Y;
 400919225        Fixed64 z = (_freezeAxes & BodyFreezeAxes3D.PositionZ) == BodyFreezeAxes3D.PositionZ ? Fixed64.Zero : value.Z;
 400919226        return new Vector3d(x, y, z);
 27    }
 28
 29    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 30    private Vector3d ProjectLinearEndpoint(Vector3d start, Vector3d end)
 31    {
 7827932        Fixed64 x = (_freezeAxes & BodyFreezeAxes3D.PositionX) == BodyFreezeAxes3D.PositionX ? start.X : end.X;
 7827933        Fixed64 y = (_freezeAxes & BodyFreezeAxes3D.PositionY) == BodyFreezeAxes3D.PositionY ? start.Y : end.Y;
 7827934        Fixed64 z = (_freezeAxes & BodyFreezeAxes3D.PositionZ) == BodyFreezeAxes3D.PositionZ ? start.Z : end.Z;
 7827935        return new Vector3d(x, y, z);
 36    }
 37
 38    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 39    internal Vector3d ProjectAngularMotion(Vector3d value)
 40    {
 483702841        if (value == Vector3d.Zero || IsRotationFullyFrozen)
 144934542            return Vector3d.Zero;
 43
 338768344        Fixed64 x = (_freezeAxes & BodyFreezeAxes3D.RotationX) == BodyFreezeAxes3D.RotationX ? Fixed64.Zero : value.X;
 338768345        Fixed64 y = (_freezeAxes & BodyFreezeAxes3D.RotationY) == BodyFreezeAxes3D.RotationY ? Fixed64.Zero : value.Y;
 338768346        Fixed64 z = (_freezeAxes & BodyFreezeAxes3D.RotationZ) == BodyFreezeAxes3D.RotationZ ? Fixed64.Zero : value.Z;
 338768347        return new Vector3d(x, y, z);
 48    }
 49
 50    private void UpdateSleepState()
 51    {
 952        if (AdvanceSleepCounter())
 153            Sleep();
 954    }
 55
 7211956    private bool CanCountTowardSleep => CanSleep
 7211957        && _linearSpeed <= SleepLinearSpeedThreshold && _angularSpeed <= SleepAngularSpeedThreshold
 7211958        && _deltaAcceleration == Vector3d.Zero && _deltaTorque == Vector3d.Zero;
 59
 53660    internal bool ReadyForAutomaticSleep => _sleepFrameCount >= SleepFrameThreshold && CanCountTowardSleep;
 61
 62    internal bool AdvanceSleepCounter()
 63    {
 7415964        if (_isSleeping)
 232465            return false;
 7183566        if (!CanCountTowardSleep)
 67        {
 7047768            _sleepFrameCount = 0;
 7047769            return false;
 70        }
 71
 135872        if (_sleepFrameCount < _sleepFrameThreshold)
 110773            _sleepFrameCount++;
 74
 135875        return _sleepFrameCount >= SleepFrameThreshold;
 76    }
 77
 78    private void ClearMotionForSleep()
 79    {
 1645980        _linearVelocity = Vector3d.Zero;
 1645981        _linearDirection = Vector3d.Zero;
 1645982        _linearAccelerationStore = Vector3d.Zero;
 1645983        _deltaAcceleration = Vector3d.Zero;
 1645984        _linearAcceleration = Vector3d.Zero;
 1645985        _linearSpeed = Fixed64.Zero;
 1645986        _angularVelocity = Vector3d.Zero;
 1645987        _angularDirection = Vector3d.Zero;
 1645988        _deltaTorque = Vector3d.Zero;
 1645989        _angularAccelerationStore = Vector3d.Zero;
 1645990        _angularAcceleration = Vector3d.Zero;
 1645991        _angularSpeed = Fixed64.Zero;
 1645992    }
 93
 94    private void ApplyFreezeConstraintsToMotion()
 95    {
 117596        Vector3d lastLinearVelocity = _linearVelocity;
 117597        _linearVelocity = ProjectLinearMotion(_linearVelocity);
 117598        _linearAccelerationStore = ProjectLinearMotion(_linearAccelerationStore);
 117599        _deltaAcceleration = ProjectLinearMotion(_deltaAcceleration);
 1175100        _linearAcceleration = ProjectLinearMotion(_linearAcceleration);
 1175101        RefreshLinearMotionState(lastLinearVelocity);
 102
 1175103        Vector3d lastAngularVelocity = _angularVelocity;
 1175104        _angularVelocity = ProjectAngularMotion(_angularVelocity);
 1175105        _deltaTorque = ProjectAngularMotion(_deltaTorque);
 1175106        _angularAccelerationStore = ProjectAngularMotion(_angularAccelerationStore);
 1175107        _angularAcceleration = ProjectAngularMotion(_angularAcceleration);
 1175108        RefreshAngularMotionState(lastAngularVelocity);
 1175109    }
 110
 111    private void RefreshPartitionAwakeState()
 112    {
 2403113        if (Collider.IsPartitioned)
 1771114            Context.Collisions.RefreshPartitionAwakeState(Collider);
 2403115        Context.MixedCollisions.Refresh3DPartitionAwakeState(Collider);
 2403116    }
 117
 118    private void RefreshPartitionMobility()
 119    {
 10059120        if (!Active || !Collider.TryGetBoundContext(out GravitasWorldContext? context))
 626121            return;
 122
 9433123        Collider.Simulate();
 124
 9433125        if (context!.Settings.RuntimeMode.RunsMixedContacts())
 1887126            context.MixedCollisions.Refresh3DColliderPartition(Collider);
 9433127    }
 128
 129    /// <summary>
 130    /// Queues a torque in mass-distance-squared-per-time-squared units for
 131    /// integration during the next fixed step.
 132    /// </summary>
 133    /// <param name="torque">The world-space torque to apply.</param>
 134    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 135    public void AddTorque(Vector3d torque)
 136    {
 118137        Vector3d accelerationDelta = ProjectAngularMotion(torque * EffectiveInverseInertiaTensor);
 118138        if (accelerationDelta == Vector3d.Zero)
 4139            return;
 140
 114141        Wake();
 114142        _deltaTorque += accelerationDelta;
 114143        Context.Diagnostics.EmitTorqueDelta(this, torque);
 114144    }
 145
 146    /// <summary>
 147    /// Queues a force in mass-distance-per-time-squared units for integration
 148    /// during the next fixed step.
 149    /// </summary>
 150    /// <param name="force">The world-space force to apply.</param>
 151    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 152    public void AddForce(Vector3d force)
 153    {
 2159154        Vector3d accelerationDelta = ProjectLinearMotion(force * EffectiveInverseMass);
 2159155        if (accelerationDelta == Vector3d.Zero)
 2156            return;
 157
 2157158        Wake();
 2157159        _deltaAcceleration += accelerationDelta;
 2157160        Context.Diagnostics.EmitForceDelta(this, force, accelerationDelta);
 2157161    }
 162
 163    /// <summary>
 164    /// Applies a world-space linear impulse immediately as a velocity change.
 165    /// The impulse is expressed in mass-distance-per-time units and does not
 166    /// advance the fixed-step simulation or apply a time-step factor.
 167    /// </summary>
 168    /// <param name="impulse">The world-space linear impulse to apply.</param>
 169    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 170    public void AddLinearImpulse(Vector3d impulse)
 171    {
 9169172        Vector3d velocityDelta = ProjectLinearMotion(impulse * EffectiveInverseMass);
 9169173        if (velocityDelta == Vector3d.Zero)
 511174            return;
 175
 8658176        Wake();
 8658177        Vector3d lastVelocity = _linearVelocity;
 8658178        _linearVelocity += velocityDelta;
 8658179        RefreshLinearMotionState(lastVelocity);
 8658180    }
 181
 182    /// <summary>
 183    /// Applies a world-space angular impulse immediately as an angular-velocity
 184    /// change. The impulse is expressed in mass-distance-squared-per-time units
 185    /// and does not apply a time-step factor.
 186    /// </summary>
 187    /// <param name="impulse">The world-space angular impulse to apply.</param>
 188    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 189    public void AddAngularImpulse(Vector3d impulse)
 190    {
 702191        Vector3d velocityDelta = ProjectAngularMotion(impulse * EffectiveInverseInertiaTensor);
 702192        if (velocityDelta == Vector3d.Zero)
 8193            return;
 194
 694195        Wake();
 694196        _angularVelocity += velocityDelta;
 694197        RefreshAngularMotionState(_angularVelocity - velocityDelta);
 694198    }
 199
 200    internal void ApplyCollisionLinearVelocityDelta(Vector3d velocityDelta)
 201    {
 207996202        velocityDelta = ProjectLinearMotion(velocityDelta);
 207996203        if (!CanTranslate || velocityDelta == Vector3d.Zero)
 191818204            return;
 205
 16178206        WakeFromCollision();
 16178207        Vector3d lastVelocity = _linearVelocity;
 16178208        _linearVelocity += velocityDelta;
 16178209        RefreshLinearMotionState(lastVelocity);
 16178210        Context.Diagnostics.EmitLinearVelocityDelta(this, lastVelocity, _linearVelocity);
 16178211    }
 212
 213    internal void ApplyCollisionAngularVelocityDelta(Vector3d velocityDelta)
 214    {
 207863215        velocityDelta = ProjectAngularMotion(velocityDelta);
 207863216        if (!CanRotate || velocityDelta == Vector3d.Zero)
 198495217            return;
 218
 9368219        WakeFromCollision();
 9368220        Vector3d lastVelocity = _angularVelocity;
 9368221        _angularVelocity += velocityDelta;
 9368222        RefreshAngularMotionState(lastVelocity);
 9368223        Context.Diagnostics.EmitAngularVelocityDelta(this, lastVelocity, _angularVelocity);
 9368224    }
 225
 226    internal bool CanApplyCollisionVelocityDeltas(
 227        Vector3d linearVelocityDelta,
 228        Vector3d angularVelocityDelta) =>
 106638229        TryPrepareCollisionVelocityState(
 106638230            linearVelocityDelta,
 106638231            angularVelocityDelta,
 106638232            out _,
 106638233            out _);
 234
 235    internal bool TryPrepareCollisionVelocityState(
 236        Vector3d linearVelocityDelta,
 237        Vector3d angularVelocityDelta,
 238        out Vector3d linearVelocity,
 239        out Vector3d angularVelocity)
 240    {
 588327241        linearVelocityDelta = ProjectLinearMotion(linearVelocityDelta);
 588327242        angularVelocityDelta = ProjectAngularMotion(angularVelocityDelta);
 588327243        linearVelocity = _linearVelocity;
 588327244        angularVelocity = _angularVelocity;
 588327245        bool linearVelocityFits = !CanTranslate
 588327246            || Vector3d.TryAdd(
 588327247                _linearVelocity,
 588327248                linearVelocityDelta,
 588327249                out linearVelocity);
 588327250        bool angularVelocityFits = !CanRotate
 588327251            || Vector3d.TryAdd(
 588327252                _angularVelocity,
 588327253                angularVelocityDelta,
 588327254                out angularVelocity);
 588327255        return linearVelocityFits & angularVelocityFits;
 256    }
 257
 258    internal void ApplyCollisionVelocityState(
 259        Vector3d linearVelocity,
 260        Vector3d angularVelocity)
 261    {
 476893262        linearVelocity = ProjectLinearMotion(linearVelocity);
 476893263        angularVelocity = ProjectAngularMotion(angularVelocity);
 476893264        if (_linearVelocity == linearVelocity && _angularVelocity == angularVelocity)
 21151265            return;
 266
 455742267        WakeFromCollision();
 455742268        if (_linearVelocity != linearVelocity)
 269        {
 434188270            Vector3d lastLinearVelocity = _linearVelocity;
 434188271            _linearVelocity = linearVelocity;
 434188272            RefreshLinearMotionState(lastLinearVelocity);
 434188273            Context.Diagnostics.EmitLinearVelocityDelta(this, lastLinearVelocity, _linearVelocity);
 274        }
 275
 455742276        if (_angularVelocity != angularVelocity)
 277        {
 369326278            Vector3d lastAngularVelocity = _angularVelocity;
 369326279            _angularVelocity = angularVelocity;
 369326280            RefreshAngularMotionState(lastAngularVelocity);
 369326281            Context.Diagnostics.EmitAngularVelocityDelta(this, lastAngularVelocity, _angularVelocity);
 282        }
 455742283    }
 284
 285    internal Vector3d PrepareCollisionPositionCorrection(Vector3d positionCorrection) =>
 63537286        CanTranslate
 63537287            ? AddPositionDisplacement(Position3d, ProjectLinearMotion(positionCorrection))
 63537288            : Position3d;
 289
 290    private bool _collisionPositionPublicationPending;
 291
 292    internal void ApplyCollisionPositionState(Vector3d position)
 293    {
 64499294        if (Position3d == position)
 9129295            return;
 55370296        Position3d = position;
 55370297        _collisionPositionPublicationPending = true;
 55370298        Context.Physics.MarkCollisionPositionPublicationPending();
 55370299    }
 300
 301    internal void PublishCollisionPositionState()
 302    {
 56258303        if (!_collisionPositionPublicationPending)
 2897304            return;
 53361305        _collisionPositionPublicationPending = false;
 306        // Publish after response bookkeeping; translation alone preserves inertia.
 53361307        Collider.Simulate(skipUnchangedMassProperties: true);
 53361308        if (Context.Settings.RuntimeMode.RunsMixedContacts())
 8719309            Context.MixedCollisions.Refresh3DColliderPartition(Collider);
 53361310    }
 311
 312    // Callers project frozen axes first; numeric exhaustion is not a physical wall.
 313    private static Vector3d AddPositionDisplacement(Vector3d start, Vector3d displacement)
 314    {
 256859315        if (!Vector3d.TryAdd(start, displacement, out Vector3d position))
 14316            throw new OverflowException("The requested body position is outside the representable coordinate domain.");
 256845317        return position;
 318    }
 319
 320
 321    /// <summary>Sets the authoritative world position and refreshes collider state.</summary>
 322    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 323    public void SetPosition(Vector3d position)
 324    {
 1352325        PreflightStaticPoseChange();
 1349326        bool preparedPose = PrepareExplicitBodyPose(
 1349327            position,
 1349328            Rotation,
 1349329            nameof(position),
 1349330            "The requested body position produces collider geometry outside the representable coordinate domain.");
 1348331        bool poseChanged = Position3d != position;
 1348332        if (poseChanged)
 1331333            Wake();
 334
 1348335        Position3d = position;
 1348336        PublishExplicitBodyPose(preparedPose, poseChanged);
 1348337    }
 338
 339    /// <summary>Sets the authoritative world-space Y coordinate.</summary>
 340    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 341    public void SetHeight(Fixed64 height)
 342    {
 4343        PreflightStaticPoseChange();
 3344        Vector3d position = Position3d;
 3345        position.Y = height;
 3346        bool preparedPose = PrepareExplicitBodyPose(
 3347            position,
 3348            Rotation,
 3349            nameof(height),
 3350            "The requested body height produces collider geometry outside the representable coordinate domain.");
 3351        bool poseChanged = HeightPos != height;
 3352        if (poseChanged)
 1353            Wake();
 354
 3355        HeightPos = height;
 3356        PublishExplicitBodyPose(preparedPose, poseChanged);
 3357    }
 358
 359    /// <summary>Sets the authoritative world rotation and refreshes collider state.</summary>
 360    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 361    public void SetRotation(FixedQuaternion quaternion)
 362    {
 104363        FixedQuaternion normalizedRotation = quaternion.Normalized;
 104364        PreflightStaticPoseChange();
 103365        bool preparedPose = PrepareExplicitBodyPose(
 103366            Position3d,
 103367            normalizedRotation,
 103368            nameof(quaternion),
 103369            "The requested body rotation produces collider geometry outside the representable coordinate domain.");
 102370        bool poseChanged = Rotation != normalizedRotation;
 102371        if (poseChanged)
 94372            Wake();
 373
 102374        Rotation = normalizedRotation;
 102375        PublishExplicitBodyPose(preparedPose, poseChanged);
 102376    }
 377
 378    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 379    private bool PrepareExplicitBodyPose(
 380        Vector3d position,
 381        FixedQuaternion rotation,
 382        string parameterName,
 383        string message)
 384    {
 1464385        if (!Active)
 1386            return false;
 387
 1463388        ChronicleGuard.ThrowIfTrue(
 1463389            !Collider.TryPrepareBodyPose(position, rotation),
 1463390            parameterName,
 1463391            message);
 1461392        return true;
 393    }
 394
 395    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 396    private void PublishExplicitBodyPose(bool preparedPose, bool poseChanged)
 397    {
 8968398        if (!preparedPose)
 2399            return;
 400
 8966401        Collider.PublishPreparedExplicitBodyPose(poseChanged);
 8966402        RefreshPartitionMobility();
 8966403    }
 404
 405    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 406    private void PreflightStaticPoseChange()
 407    {
 1470408        if (Active && IsStatic)
 409        {
 35410            ThrowIfRuntimeRegistrationMissing();
 34411            Context.ThrowIfFixedStepMutationNotAllowed();
 30412            Collider.ValidateCurrentRuntimeTransform();
 413        }
 1464414    }
 415
 416    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 417    private void PreflightResetPoseChange()
 418    {
 7513419        if (!Active)
 1420            return;
 421
 7512422        ThrowIfRuntimeRegistrationMissing();
 7511423        if (!IsStatic)
 7505424            return;
 425
 6426        Context.ThrowIfFixedStepMutationNotAllowed();
 5427        Collider.ValidateCurrentRuntimeTransform();
 5428    }
 429
 430    private void ProcessMovable()
 431    {
 65212432        EnsureContinuousCollisionFramePrepared(Context.LateSimulateToken);
 433
 434        // Non-kinematic bodies position is calculated from the motion prepared
 435        // for every body before ordered body processing begins.
 65212436        NonKinematicUpdate(_continuousCollisionAngularVelocityStepStart);
 437
 65211438    }
 439
 440    private void ApplyLinearForces()
 441    {
 65361442        _linearAccelerationStore = ProjectLinearMotion(_deltaAcceleration);
 65361443        _deltaAcceleration = Vector3d.Zero;
 65361444    }
 445
 446    private Vector3d ApplyDrag(Vector3d velocity, Fixed64 coefficient, Vector3d previousVelocity, Vector3d previousDirec
 447    {
 88612448        Fixed64 density = Context.Environment.AirDensity;
 88612449        if (coefficient == Fixed64.Zero || density == Fixed64.Zero || velocity == Vector3d.Zero)
 87927450            return velocity;
 451
 685452        Vector3d direction = velocity == previousVelocity ? previousDirection : velocity.Normalized;
 685453        Fixed64 fraction = coefficient * density * Collider.GetFrontalArea(direction) * Context.DeltaTime;
 454        // The existing drag law is linear in speed. Bound its radial decay before it reverses motion.
 685455        return fraction >= Fixed64.One ? Vector3d.Zero : velocity * (Fixed64.One - fraction);
 456    }
 457
 458    private void ApplyGroundLinearFriction()
 459    {
 65361460        if (!HasCurrentGroundSupport || Collider.Material.DynamicFriction == Fixed64.Zero)
 63723461            return;
 462
 463        // Project within the permitted axes so friction cannot consume separating normal motion.
 1638464        Vector3d direction = Vector3d.GetNormalizedProjectionOnPlane(
 1638465            _linearVelocity, ProjectLinearMotion(_groundNormal));
 1638466        if (direction == Vector3d.Zero)
 353467            return;
 468
 1285469        Fixed64 speed = Vector3d.Dot(_linearVelocity, direction);
 1285470        _linearVelocity = ApplyBoundedDissipation(
 1285471            _linearVelocity, direction, direction, ResolveGroundFrictionImpulse(speed) * InverseMass);
 1285472    }
 473
 474    private void UpdateLinearVelocity()
 475    {
 65361476        Fixed64 deltaTime = Context.DeltaTime;
 65361477        PhysicsEnvironment environment = Context.Environment;
 65361478        Vector3d lastVelocity = _linearVelocity;
 479
 65361480        _linearVelocity += ProjectLinearMotion(_linearAccelerationStore * deltaTime);
 65361481        _linearVelocity = ApplyDrag(_linearVelocity, LinearDragCoefficient, lastVelocity, _linearDirection);
 65361482        ApplyGroundLinearFriction();
 483
 484        // Reset stores for the next frame
 65361485        _linearAccelerationStore = Vector3d.Zero;
 486
 487        // Apply gravity only if not grounded
 65361488        if (!HasCurrentGroundSupport && (_freezeAxes & BodyFreezeAxes3D.PositionY) != BodyFreezeAxes3D.PositionY)
 55807489            _linearVelocity.Y -= environment.Gravity * _gravityScale * deltaTime;
 490
 491        // Make sure we don't fall any faster than maxFallSpeed. This gives our character a terminal velocity
 65361492        _linearVelocity.Y = FixedMath.Max(_linearVelocity.Y, -environment.MaxFallSpeed);
 65361493        _linearVelocity = ProjectLinearMotion(_linearVelocity);
 494
 65361495        RefreshLinearMotionState(lastVelocity);
 65361496    }
 497
 498    private void RefreshLinearMotionState(Vector3d lastVelocity)
 499    {
 525958500        _linearVelocity = ResolveLimitedVelocity(_linearVelocity, _linearVelocity.Magnitude, out _linearSpeed);
 501
 502        // Update the direction of the linear velocity, if we're not moving maintain previous direction
 525958503        _linearDirection = _linearSpeed > Fixed64.Zero
 525958504            ? _linearVelocity.Normalized
 525958505            : _linearDirection;
 525958506        _linearAcceleration = _linearSpeed > Fixed64.Zero
 525958507            ? (_linearVelocity - lastVelocity) / Context.DeltaTime
 525958508            : Vector3d.Zero;
 525958509    }
 510
 511    private void ApplyAngularTorques()
 512    {
 23251513        _angularAccelerationStore = ProjectAngularMotion(_deltaTorque);
 23251514        _deltaTorque = Vector3d.Zero;
 23251515    }
 516
 517    private Vector3d ApplyGroundAngularFriction(Vector3d velocity)
 518    {
 23251519        if (!HasCurrentGroundSupport || Collider.Material.DynamicFriction == Fixed64.Zero)
 21661520            return velocity;
 521
 1590522        Fixed64 speed = velocity == _angularVelocity ? _angularSpeed : velocity.Magnitude;
 1590523        if (speed <= Fixed64.Zero)
 27524            return velocity;
 525
 1563526        Vector3d direction = velocity == _angularVelocity ? _angularDirection : velocity.Normalized;
 1563527        Fixed64 impulse = ResolveGroundFrictionImpulse(speed);
 1563528        return ApplyAngularDissipation(velocity, direction, impulse);
 529    }
 530
 531    private Vector3d ApplyAngularDamping(Vector3d velocity)
 532    {
 23248533        Fixed64 damping = Context.Environment.DampingFactor;
 23248534        if (damping == Fixed64.Zero || velocity == Vector3d.Zero)
 22955535            return velocity;
 536
 293537        Vector3d direction = velocity;
 538        Fixed64 speed;
 293539        if (velocity == _angularVelocity)
 540        {
 8541            direction = _angularDirection;
 8542            speed = _angularSpeed;
 543        }
 544        else
 285545            direction.NormalizeInPlace(out speed);
 293546        return ApplyAngularDissipation(velocity, direction, damping * speed * Context.DeltaTime);
 547    }
 548
 549    private Vector3d ApplyAngularDissipation(Vector3d velocity, Vector3d direction, Fixed64 impulse)
 550    {
 1856551        Vector3d response = ProjectAngularMotion(_inverseInertiaTensor * direction);
 1856552        return (_freezeAxes & BodyFreezeAxes3D.Rotation) != BodyFreezeAxes3D.None
 1856553            ? ApplyConstrainedAngularDissipation(velocity, response, impulse)
 1856554            : ApplyBoundedDissipation(velocity, direction, response, impulse);
 555    }
 556
 557    private Vector3d ApplyConstrainedAngularDissipation(Vector3d velocity, Vector3d response, Fixed64 impulse)
 558    {
 559        // Projection and rotated inertia do not commute. Keep M*r exact: supported
 560        // slender bodies can overflow both this intermediate and its energy dot products.
 1659561        Signed192 x = Signed192.Raw(response.X), y = Signed192.Raw(response.Y), z = Signed192.Raw(response.Z);
 553562        Signed192 mx = Signed192.NarrowProven(WideArithmetic.GetDotProduct3D(
 553563            Signed192.Raw(_worldInertiaTensor.M11), Signed192.Raw(_worldInertiaTensor.M12), Signed192.Raw(_worldInertiaT
 553564        Signed192 my = Signed192.NarrowProven(WideArithmetic.GetDotProduct3D(
 553565            Signed192.Raw(_worldInertiaTensor.M21), Signed192.Raw(_worldInertiaTensor.M22), Signed192.Raw(_worldInertiaT
 553566        Signed192 mz = Signed192.NarrowProven(WideArithmetic.GetDotProduct3D(
 553567            Signed192.Raw(_worldInertiaTensor.M31), Signed192.Raw(_worldInertiaTensor.M32), Signed192.Raw(_worldInertiaT
 553568        Signed320 work = WideArithmetic.GetDotProduct3D(
 553569            Signed192.Raw(velocity.X), Signed192.Raw(velocity.Y), Signed192.Raw(velocity.Z), mx, my, mz);
 553570        Signed320 curvature = WideArithmetic.GetDotProduct3D(x, y, z, mx, my, mz);
 553571        if ((work.Sign <= 0) | (curvature.Sign <= 0))
 1572            return velocity;
 573
 574        // Two-factor sums fit 130 bits; energy products fit 196 and completed
 575        // velocity numerators fit 260. Existing fixed-width math owns all limbs/rounding.
 552576        Signed576 budget = WideArithmetic.MultiplySigned320(curvature, Signed192.Raw(impulse));
 552577        Signed576 minimum = WideArithmetic.MultiplySigned320(work, Signed192.One);
 552578        if (WideArithmetic.SubtractSigned576(budget, minimum).Sign >= 0)
 579        {
 279580            return new Vector3d(
 279581                ResolveConstrainedDissipativeVelocity(velocity.X, response.X, work, curvature),
 279582                ResolveConstrainedDissipativeVelocity(velocity.Y, response.Y, work, curvature),
 279583                ResolveConstrainedDissipativeVelocity(velocity.Z, response.Z, work, curvature));
 584        }
 273585        return new Vector3d(
 273586            ResolveDissipativeVelocity(velocity.X, response.X, impulse, Fixed64.One),
 273587            ResolveDissipativeVelocity(velocity.Y, response.Y, impulse, Fixed64.One),
 273588            ResolveDissipativeVelocity(velocity.Z, response.Z, impulse, Fixed64.One));
 589    }
 590
 591    private static Fixed64 ResolveConstrainedDissipativeVelocity(
 592        Fixed64 velocity, Fixed64 response, Signed320 work, Signed320 curvature)
 593    {
 837594        Signed576 numerator = WideArithmetic.SubtractSigned576(
 837595            WideArithmetic.MultiplySigned320(curvature, Signed192.Raw(velocity)),
 837596            WideArithmetic.MultiplySigned320(work, Signed192.Raw(response)));
 837597        if (!Fixed64.TryGetSignedRawRatio(numerator, Signed576.ExtendValue(curvature), out Fixed64 result))
 1598            throw new OverflowException("Dissipation produces an unrepresentable velocity.");
 836599        return result;
 600    }
 601
 602    private static Vector3d ApplyBoundedDissipation(
 603        Vector3d velocity, Vector3d energyResponse, Vector3d response, Fixed64 impulse)
 604    {
 2588605        Fixed64 curvature = Vector3d.Dot(energyResponse, response);
 2588606        Fixed64 work = Vector3d.Dot(velocity, energyResponse);
 2588607        if ((curvature <= Fixed64.Zero) | (work <= Fixed64.Zero))
 1608            return velocity;
 609
 610        // For response r and energy metric M, energy is minimal at j = (v . M r) / (r . M r).
 611        // Retain the tensor response: anisotropic inertia need not stop every component at once.
 2587612        Fixed64 divisor = Fixed64.One;
 2587613        if (impulse * curvature >= work)
 614        {
 2487615            impulse = work;
 2487616            divisor = curvature;
 617        }
 2587618        return new Vector3d(
 2587619            ResolveDissipativeVelocity(velocity.X, response.X, impulse, divisor),
 2587620            ResolveDissipativeVelocity(velocity.Y, response.Y, impulse, divisor),
 2587621            ResolveDissipativeVelocity(velocity.Z, response.Z, impulse, divisor));
 622    }
 623
 624    private static Fixed64 ResolveDissipativeVelocity(
 625        Fixed64 velocity, Fixed64 response, Fixed64 impulse, Fixed64 divisor)
 626    {
 627        Fixed64 result;
 8580628        if (divisor == Fixed64.One)
 629        {
 4782630            if (Fixed64.TrySubtractProducts(velocity, Fixed64.One, response, impulse, out result))
 4781631                return result;
 632        }
 633        else
 634        {
 635            // The correction may exceed Fixed64 even when the completed velocity fits.
 3798636            Signed192 numerator = WideArithmetic.SubtractSigned192(
 3798637                Fixed64.GetExactRawProduct(velocity, divisor), Fixed64.GetExactRawProduct(response, impulse));
 3798638            if (Fixed64.TryGetSignedRawRatio(numerator, Signed192.Raw(divisor), out result))
 3797639                return result;
 640        }
 2641        throw new OverflowException("Dissipation produces an unrepresentable velocity.");
 642    }
 643
 644    private void UpdateAngularVelocity()
 645    {
 23251646        Vector3d lastVelocity = _angularVelocity;
 23251647        _angularVelocity = ResolveAngularVelocityForStep(_angularVelocity, _angularAccelerationStore);
 23248648        _angularAccelerationStore = Vector3d.Zero;
 23248649        RefreshAngularMotionState(lastVelocity);
 23248650    }
 651
 652    private Vector3d ResolveAngularVelocityForStep(
 653        Vector3d startVelocity,
 654        Vector3d angularAcceleration)
 655    {
 23251656        Fixed64 deltaTime = Context.DeltaTime;
 23251657        Vector3d velocity = startVelocity
 23251658            + ProjectAngularMotion(angularAcceleration * deltaTime);
 23251659        velocity = ApplyDrag(velocity, AngularDragCoefficient, startVelocity, _angularDirection);
 23251660        velocity = ApplyGroundAngularFriction(velocity);
 23248661        velocity = ApplyAngularDamping(velocity);
 23248662        velocity = ProjectAngularMotion(velocity);
 663
 23248664        Fixed64 desiredSpeed = velocity.Magnitude;
 23248665        if (desiredSpeed <= Context.Environment.MinSpeed)
 14244666            return Vector3d.Zero;
 667
 9004668        return desiredSpeed > Context.Environment.MaxSpeed
 9004669            ? velocity.Normalized * Context.Environment.MaxSpeed
 9004670            : velocity;
 671    }
 672
 673    private void RefreshAngularMotionState(Vector3d lastVelocity)
 674    {
 404418675        Fixed64 desiredSpeed = _angularVelocity.Magnitude;
 404418676        _angularVelocity = ResolveLimitedVelocity(_angularVelocity, desiredSpeed, out _angularSpeed);
 404418677        if (desiredSpeed > Context.Environment.MinSpeed)
 358122678            _angularDirection = _angularVelocity.Normalized;
 679
 404418680        _angularAcceleration = _angularSpeed > Fixed64.Zero
 404418681            ? (_angularVelocity - lastVelocity) / Context.DeltaTime
 404418682            : Vector3d.Zero;
 404418683    }
 684
 685    private Vector3d ResolveLimitedVelocity(Vector3d velocity, Fixed64 desiredSpeed, out Fixed64 speed)
 686    {
 930962687        PhysicsEnvironment environment = Context.Environment;
 930962688        if (desiredSpeed <= environment.MinSpeed)
 689        {
 63272690            speed = Fixed64.Zero;
 63272691            return Vector3d.Zero;
 692        }
 867690693        if (desiredSpeed > environment.MaxSpeed)
 694        {
 193695            speed = environment.MaxSpeed;
 193696            return velocity.Normalized * speed;
 697        }
 867497698        speed = desiredSpeed;
 867497699        return velocity;
 700    }
 701
 702    private Fixed64 ResolveGroundFrictionImpulse(Fixed64 speed)
 703    {
 2848704        Fixed64 friction = Collider.Material.DynamicFriction;
 2848705        Fixed64 transition = Context.Environment.FrictionTransitionSpeed;
 2848706        if (speed < transition)
 2357707            friction *= speed / transition;
 2848708        return friction * _normalForce.Magnitude * Context.DeltaTime;
 709    }
 710
 711    private void NonKinematicUpdate(Vector3d angularVelocityStateStart)
 712    {
 65212713        Vector3d rotationalCcdStartPosition = Position3d;
 65212714        Vector3d rotationalCcdProposedPosition = AddPositionDisplacement(
 65212715            rotationalCcdStartPosition, ProjectLinearMotion(_linearVelocity) * Context.DeltaTime);
 65212716        FixedQuaternion rotationalCcdStartRotation = Rotation;
 65212717        FixedQuaternion rotationalCcdProposedRotation = IntegrateAngularRotation(
 65212718            rotationalCcdStartRotation,
 65212719            Context.DeltaTime);
 65212720        if (TryResolveRotationalContinuousCollision(
 65212721                rotationalCcdStartPosition,
 65212722                ref rotationalCcdProposedPosition,
 65212723                rotationalCcdStartRotation,
 65212724                ref rotationalCcdProposedRotation))
 725        {
 135726            ApplyResolvedPosition(rotationalCcdStartPosition, rotationalCcdProposedPosition);
 135727            Rotation = rotationalCcdProposedRotation;
 135728            Collider.RebuildRuntimeShapeOnly();
 729        }
 730        else
 731        {
 65077732            PositionBasedOnForce();
 65076733            rotationalCcdProposedPosition = Position3d;
 65076734            if (CanRotate && _angularSpeed > Fixed64.Zero)
 8857735                RotationBasedOnTorque(rotationalCcdStartPosition, rotationalCcdProposedPosition);
 736        }
 737
 65211738        CheckGroundForSimulation();
 65211739        ApplyGroundedHeightOrReset();
 740
 65211741        CheckChangedValues();
 65211742        if (!CanRotate && !RotationChangePending)
 42106743            return;
 744
 23105745        UpdateInertiaTensorOrientation();
 23105746        ApplyGyroscopicPrecession(angularVelocityStateStart);
 23105747    }
 748
 749    private void PositionBasedOnForce()
 750    {
 65077751        Vector3d startPosition = Position3d;
 65077752        Vector3d velocityVector = AddPositionDisplacement(
 65077753            startPosition, ProjectLinearMotion(_linearVelocity) * Context.DeltaTime);
 65077754        TryResolveContinuousCollision(startPosition, ref velocityVector);
 65076755        ApplyResolvedPosition(startPosition, velocityVector);
 65076756    }
 757
 758    private void ApplyResolvedPosition(Vector3d startPosition, Vector3d velocityVector)
 759    {
 65211760        velocityVector = ProjectLinearEndpoint(startPosition, velocityVector);
 761
 65211762        Vector2d velocityAxis = velocityVector.ToVector2d();
 763
 764        // Find out how much we need to push towards the ground to avoid loosing grounding
 765        // when walking down a step or over a sharp change in slope.
 65211766        if (HasCurrentGroundSupport && (_freezeAxes & BodyFreezeAxes3D.PositionY) != BodyFreezeAxes3D.PositionY)
 767        {
 2818768            velocityVector.Y -= FixedMath.Max(StepOffset, velocityVector.Magnitude);
 2818769            _lastGroundedPosition = Position3d;
 770        }
 62393771        else if ((_freezeAxes & BodyFreezeAxes3D.PositionY) != BodyFreezeAxes3D.PositionY)
 55660772            HeightPos = velocityVector.Y;
 773
 774        //  Apply the force
 65211775        SetPosition2d(velocityAxis);
 65211776    }
 777
 778
 779    private void RotationBasedOnTorque(Vector3d startPosition, Vector3d proposedPosition)
 780    {
 8857781        FixedQuaternion startRotation = Rotation;
 8857782        FixedQuaternion proposedRotation = IntegrateAngularRotation(startRotation, Context.DeltaTime);
 8857783        TryResolveRotationalContinuousCollision(startPosition, ref proposedPosition, startRotation, ref proposedRotation
 8857784        Position3d = proposedPosition;
 8857785        Rotation = proposedRotation;
 8857786        Collider.RebuildRuntimeShapeOnly();
 8857787    }
 788
 789    //  gyroscopic precession is a correction to the object's angular velocity based on its rotation
 790    private void ApplyGyroscopicPrecession(Vector3d angularVelocityStateStart)
 791    {
 23105792        if (!CanRotate)
 1793            return;
 794
 23104795        Vector3d gyroscopicCorrection = ProjectAngularMotion(
 23104796            _inverseInertiaTensor
 23104797            * Vector3d.Cross(_angularVelocity, _worldInertiaTensor * _angularVelocity)
 23104798            * Context.DeltaTime);
 23104799        if (gyroscopicCorrection == Vector3d.Zero)
 22535800            return;
 801
 569802        _angularVelocity -= gyroscopicCorrection;
 569803        _angularVelocity = ProjectAngularMotion(_angularVelocity);
 569804        RefreshAngularMotionState(angularVelocityStateStart);
 569805    }
 806
 807}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.ReplayHash.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody.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 spatial body state and continuous collision trajectories to the replay hash.</content>
 14public partial class SolidBody
 15{
 16    internal void ContributeReplayHash(
 17        ref ChronicleHashWriter writer,
 18        GravitasReplayHashMode mode)
 19    {
 465820        writer.WriteSection("body.3d", 7);
 465821        writer.WriteInt32(_dynamicId);
 465822        writer.WriteBool(Debug);
 465823        writer.WriteBool(Active);
 465824        writer.WriteEnum(_freezeAxes);
 465825        writer.WriteEnum(_motionType);
 465826        writer.WriteVector2d(_position2dUnmarked);
 465827        writer.WriteFixed64(_heightPosUnmarked);
 465828        writer.WriteVector3d(_lastPosition);
 465829        writer.WriteFixed64(GroundOriginOffset);
 465830        writer.WriteFixed64(GroundedDistanceRay);
 465831        writer.WriteFixed64(GroundDownDistanceOnAir);
 465832        writer.WriteEnum(GroundingMode);
 465833        writer.WriteEnum(GroundProbeMode);
 465834        writer.WriteFixed64(GroundProbeRadius);
 465835        writer.WriteFixed64(GroundMinNormalDot);
 465836        writer.WriteBool(_skipGroundingCheck);
 465837        writer.WriteInt64(_lastGroundCheckFrame);
 465838        writer.WriteFixed64(StepOffset);
 465839        writer.WriteVector3d(_groundNormal);
 465840        writer.WriteVector3d(_hitPlatformPosition);
 465841        writer.WriteVector3d(_hitPoint);
 465842        writer.WriteBool(_hasHitPoint);
 465843        writer.WriteBool(_isGrounded);
 465844        writer.WriteBool(_wasGrounded);
 465845        writer.WriteVector3d(_lastGroundedPosition);
 465846        writer.WriteQuaternion(_rotation);
 465847        writer.WriteVector3d(_linearVelocity);
 465848        writer.WriteVector3d(_linearDirection);
 465849        writer.WriteVector3d(_angularVelocity);
 465850        writer.WriteVector3d(_angularDirection);
 465851        writer.WriteVector3d(_deltaTorque);
 465852        writer.WriteVector3d(_localCenterOfMassOffset);
 465853        writer.WriteBool(_centerOfMassOffsetExplicit);
 465854        writer.WriteFixed64(_gravityScale);
 465855        writer.WriteBool(_isSleeping);
 465856        writer.WriteInt32(_sleepFrameCount);
 465857        writer.WriteBool(_sleepEnabled);
 465858        writer.WriteInt32(_sleepFrameThreshold);
 465859        writer.WriteFixed64(_sleepLinearSpeedThreshold);
 465860        writer.WriteFixed64(_sleepAngularSpeedThreshold);
 465861        writer.WriteEnum(_continuousCollisionMode);
 465862        writer.WriteFixed64(_linearSpeed);
 465863        writer.WriteVector3d(_linearAccelerationStore);
 465864        writer.WriteVector3d(_deltaAcceleration);
 465865        writer.WriteVector3d(_linearAcceleration);
 465866        writer.WriteFixed64(_angularSpeed);
 465867        writer.WriteVector3d(_angularAccelerationStore);
 465868        writer.WriteVector3d(_angularAcceleration);
 465869        writer.WriteFixed64(LinearDragCoefficient);
 465870        writer.WriteFixed64(AngularDragCoefficient);
 465871        writer.WriteVector3d(_normalForce);
 465872        writer.WriteFixed64(Mass);
 73
 465874        writer.WriteSection("body.3d.ccd-authoritative", 4);
 465875        writer.WriteBool(_continuousCollisionHandoffPending);
 465876        if (_continuousCollisionHandoffPending)
 77        {
 3478            writer.WriteInt64(_continuousCollisionHandoffToken);
 3479            writer.WriteFixed64(_continuousCollisionHandoffRemainingTime);
 3480            writer.WriteInt32(_continuousCollisionHandoffIgnoredCollider3D?.ReplayOrdinal ?? -1);
 3481            writer.WriteInt32(_continuousCollisionHandoffIgnoredCollider2D?.ReplayOrdinal ?? -1);
 3482            WriteContinuousCollisionTrajectory(ref writer);
 83        }
 84
 465885        if (mode != GravitasReplayHashMode.AuthoritativeWithSolverCaches)
 195686            return;
 87
 270288        writer.WriteSection("body.3d.solver-caches", 4);
 270289        writer.WriteInt64(_continuousCollisionFrameToken);
 270290        writer.WriteVector3d(_continuousCollisionAngularVelocityStepStart);
 270291        WriteContinuousCollisionTrajectory(ref writer);
 270292        writer.WriteInt64(_continuousCollisionHandoffToken);
 270293        writer.WriteFixed64(_continuousCollisionHandoffRemainingTime);
 270294        writer.WriteInt32(LastContinuousCollisionToiIterationCount);
 270295        writer.WriteBool(LastContinuousCollisionToiIterationLimitReached);
 270296        writer.WriteFixed3x3(_inertiaTensor);
 270297        writer.WriteFixed3x3(_worldInertiaTensor);
 270298        writer.WriteFixed3x3(_inverseLocalInertiaTensor);
 270299        writer.WriteFixed3x3(_inverseInertiaTensor);
 2702100        writer.WriteInt32(_continuousCollisionHits.Count);
 2702101        writer.WriteInt32(_continuousMixedCollisionHits.Count);
 2702102        writer.WriteInt32(_rotationalContinuousCollisionManifold.Count);
 2702103    }
 104
 105    private void WriteContinuousCollisionTrajectory(ref ChronicleHashWriter writer)
 106    {
 2736107        writer.WriteInt32(_continuousCollisionTrajectory.Count);
 10194108        for (int i = 0; i < _continuousCollisionTrajectory.Count; i++)
 109        {
 2361110            Gravitas.CollisionHandling.ContinuousCollisionMotionSegment3D segment =
 2361111                _continuousCollisionTrajectory[i];
 2361112            writer.WriteFixed64(segment.StartFraction);
 2361113            writer.WriteFixed64(segment.EndFraction);
 2361114            writer.WriteVector3d(segment.StartPosition);
 2361115            writer.WriteVector3d(segment.EndPosition);
 2361116            writer.WriteVector3d(segment.Displacement);
 2361117            writer.WriteQuaternion(segment.StartRotation);
 2361118            writer.WriteVector3d(segment.AngularVelocity);
 2361119            writer.WriteQuaternion(segment.TargetRotation);
 2361120            writer.WriteFixed64(segment.AngularDistance);
 2361121            writer.WriteEnum(segment.RotationPath);
 122        }
 2736123    }
 124}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.Serialization.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody.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 spatial body state and rebuilds its dependent state after loading.</content>
 15public partial class SolidBody
 16{
 17    /// <summary>Records or restores deterministic body state through Chronicler.</summary>
 18    public void RecordData(IChronicler chronicler)
 19    {
 20        // Validate timing before publishing any body fields. Numeric coercion must not
 21        // silently admit an obsolete narrow-frame snapshot in one transport only.
 16822        int bodySchemaVersion = 1;
 16823        RecordValues.Look(chronicler, ref bodySchemaVersion, "BodySchemaVersion", 0);
 16824        if (chronicler.Mode == SerializationMode.Loading)
 10125            ChronicleGuard.ThrowIfTrue(bodySchemaVersion != 1, nameof(SolidBody),
 10126                "The body schema is missing or unsupported.");
 16227        long lastGroundCheckFrame = _lastGroundCheckFrame;
 16228        RecordValues.Look(chronicler, ref lastGroundCheckFrame, "LastGroundCheckFrame", -1L);
 16229        if (chronicler.Mode == SerializationMode.Loading)
 9530            ChronicleGuard.ThrowIfTrue(lastGroundCheckFrame < -1, nameof(SolidBody),
 9531                "The grounding frame must be nonnegative or the unset value -1.");
 32
 16033        GroundingMode groundingMode = GroundingMode;
 16034        GroundProbeMode groundProbeMode = GroundProbeMode;
 16035        Fixed64 groundProbeRadius = GroundProbeRadius;
 16036        Fixed64 groundMinNormalDot = GroundMinNormalDot;
 16037        RecordValues.Look(chronicler, ref groundMinNormalDot, "GroundMinNormalDot", Fixed64.Half);
 16038        if (chronicler.Mode == SerializationMode.Loading)
 9339            ValidateGroundMinNormalDot(groundMinNormalDot);
 15840        bool active = Active;
 15841        BodyFreezeAxes3D freezeAxes = FreezeAxes;
 15842        PhysicsMobilityType motionType = MotionType;
 15843        Fixed64 gravityScale = GravityScale;
 15844        ContinuousCollisionMode continuousCollisionMode = ContinuousCollisionMode;
 45
 15846        RecordValues.Look(chronicler, ref active, "Active");
 15847        RecordValues.Look(chronicler, ref freezeAxes, "FreezeAxes", BodyFreezeAxes3D.None);
 15848        RecordValues.Look(chronicler, ref motionType, "MotionType", PhysicsMobilityType.Dynamic);
 15849        if (chronicler.Mode == SerializationMode.Loading)
 50        {
 9151            PreflightLoadedMotionType(motionType);
 8852            ChronicleGuard.ThrowIfArgument(
 8853                (freezeAxes & ~BodyFreezeAxes3D.All) != BodyFreezeAxes3D.None,
 8854                nameof(freezeAxes),
 8855                "Unsupported 3D freeze axis bits.");
 56        }
 57
 15458        RecordValues.Look(chronicler, ref Debug, "Debug");
 15459        RecordValues.Look(chronicler, ref _position2dUnmarked, "Position2d");
 15460        RecordValues.Look(chronicler, ref _heightPosUnmarked, "HeightPos");
 15461        RecordValues.Look(chronicler, ref _lastPosition, "LastPosition");
 15462        RecordValues.Look(chronicler, ref GroundOriginOffset, "GroundOriginOffset");
 15463        RecordValues.Look(chronicler, ref GroundedDistanceRay, "GroundedDistanceRay");
 15464        RecordValues.Look(chronicler, ref GroundDownDistanceOnAir, "GroundDownDistanceOnAir");
 15465        RecordValues.Look(chronicler, ref groundingMode, "GroundingMode", GroundingMode.Automatic);
 15466        RecordValues.Look(chronicler, ref groundProbeMode, "GroundProbeMode", GroundProbeMode.Auto);
 15467        RecordValues.Look(chronicler, ref groundProbeRadius, "GroundProbeRadius");
 15468        if (chronicler.Mode == SerializationMode.Loading)
 8769            ChronicleGuard.ThrowIfArgument(groundProbeRadius > ColliderGeometryPolicy.MaximumGeometryReach,
 8770                nameof(GroundProbeRadius), "Ground probe radius must not exceed 4,096 world units.");
 15371        RecordValues.Look(chronicler, ref _skipGroundingCheck, "SkipGroundingCheck", false);
 15372        RecordValues.Look(chronicler, ref StepOffset, "StepOffset");
 15373        RecordValues.Look(chronicler, ref _groundNormal, "GroundNormal");
 15374        RecordValues.Look(chronicler, ref _hitPlatformPosition, "HitPlatformPosition");
 15375        RecordValues.Look(chronicler, ref _hitPoint, "HitPoint");
 15376        RecordValues.Look(chronicler, ref _hasHitPoint, "HasHitPoint");
 15377        RecordValues.Look(chronicler, ref _isGrounded, "IsGrounded");
 15378        RecordValues.Look(chronicler, ref _wasGrounded, "WasGrounded");
 15379        RecordValues.Look(chronicler, ref _lastGroundedPosition, "LastGroundedPosition");
 15380        RecordValues.Look(chronicler, ref _rotation, "Rotation");
 15381        RecordValues.Look(chronicler, ref _linearVelocity, "LinearVelocity");
 15382        RecordValues.Look(chronicler, ref _linearDirection, "LinearDirection");
 15383        RecordValues.Look(chronicler, ref _angularVelocity, "AngularVelocity");
 15384        RecordValues.Look(chronicler, ref _angularDirection, "AngularDirection");
 15385        RecordValues.Look(chronicler, ref _deltaTorque, "DeltaTorque");
 15386        RecordValues.Look(chronicler, ref _localCenterOfMassOffset, "LocalCenterOfMassOffset");
 15387        RecordValues.Look(chronicler, ref _centerOfMassOffsetExplicit, "CenterOfMassOffsetExplicit", false);
 15388        RecordValues.Look(chronicler, ref gravityScale, "GravityScale", Fixed64.One);
 15389        RecordValues.Look(chronicler, ref _isSleeping, "IsSleeping");
 15390        RecordValues.Look(chronicler, ref _sleepFrameCount, "SleepFrameCount");
 15391        RecordValues.Look(chronicler, ref _sleepEnabled, "SleepEnabled", true);
 15392        RecordValues.Look(chronicler, ref _sleepFrameThreshold, "SleepFrameThreshold", 16);
 15393        RecordValues.Look(chronicler, ref _sleepLinearSpeedThreshold, "SleepLinearSpeedThreshold", (Fixed64)0.001f);
 15394        RecordValues.Look(chronicler, ref _sleepAngularSpeedThreshold, "SleepAngularSpeedThreshold", (Fixed64)0.001f);
 15395        RecordValues.Look(chronicler, ref continuousCollisionMode, "ContinuousCollisionMode", ContinuousCollisionMode.In
 15396        RecordValues.Look(chronicler, ref _linearSpeed, "LinearSpeed");
 15397        RecordValues.Look(chronicler, ref _linearAccelerationStore, "LinearAccelerationStore");
 15398        RecordValues.Look(chronicler, ref _deltaAcceleration, "DeltaAcceleration");
 15399        RecordValues.Look(chronicler, ref _linearAcceleration, "LinearAcceleration");
 153100        RecordValues.Look(chronicler, ref _angularSpeed, "AngularSpeed");
 153101        RecordValues.Look(chronicler, ref _angularAccelerationStore, "AngularAccelerationStore");
 153102        RecordValues.Look(chronicler, ref _angularAcceleration, "AngularAcceleration");
 153103        RecordValues.Look(chronicler, ref LinearDragCoefficient, "LinearDragCoefficient");
 153104        RecordValues.Look(chronicler, ref AngularDragCoefficient, "AngularDragCoefficient");
 153105        RecordValues.Look(chronicler, ref _normalForce, "NormalForce");
 106        // Restore backing state without the public setter's host-mutation wake policy.
 153107        RecordValues.Look(chronicler, ref _mass, "Mass");
 108
 153109        if (chronicler.Mode == SerializationMode.Loading)
 110        {
 86111            ApplyLoadedMotionType(motionType);
 86112            ContinuousCollisionMode = continuousCollisionMode;
 84113            _rotation = _rotation.Normalized;
 84114            _freezeAxes = freezeAxes;
 84115            GroundingMode = groundingMode;
 84116            GroundProbeMode = groundProbeMode;
 84117            GroundProbeRadius = groundProbeRadius;
 84118            _lastGroundCheckFrame = lastGroundCheckFrame;
 119            // Restore the validated policy without waking or invalidating saved probe timing.
 84120            _groundMinNormalDot = groundMinNormalDot;
 84121            GravityScale = gravityScale;
 84122            _hitPlatform = null;
 84123            _groundSupport = default;
 84124            _groundSupportVersion = 0;
 125        }
 126
 151127        Collider.RecordData(chronicler);
 128
 149129        if (chronicler.Mode == SerializationMode.Loading)
 130        {
 82131            ApplyLoadedState();
 82132            if (!active || Collider.Id < 0)
 8133                Deactivate();
 82134            _groundingStateVersion++;
 135        }
 149136    }
 137
 138    private void ApplyLoadedState()
 139    {
 82140        DiscardContinuousCollisionHandoff();
 82141        InvalidateContinuousCollisionTrajectory();
 82142        SetPositionTransformWorldPosition(Position3d);
 82143        SetRotationTransformWorldRotation(Rotation);
 144
 82145        _positionMutated = false;
 82146        _positionChangedBuffer = false;
 82147        _rotationMutated = false;
 82148        _rotationChangedBuffer = false;
 82149        _settingVisualsCounter = 0;
 82150        _rotationSpeed = DefaultRotationSpeed;
 82151        _rotationInterpoleSpeed = Fixed64.Zero;
 82152        _visualPosition = Position3d;
 82153        _lastVisualPosition = Position3d;
 82154        _visualRotation = Rotation;
 82155        _lastVisualRotation = Rotation;
 156
 82157        if (Collider.HasHostBinding)
 76158            RefreshMassPropertiesFromColliderShape();
 82159        ApplyFreezeConstraintsToMotion();
 160
 82161        if (Active)
 162        {
 76163            InvalidateMassDependentSolverState();
 76164            Collider.Simulate();
 76165            RefreshPartitionAwakeState();
 166        }
 82167    }
 168
 169}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Core/3D/Bodies/SolidBody.Visualization.cs

#LineLine coverage
 1//=======================================================================
 2// SolidBody.Visualization.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;
 10
 11namespace Gravitas;
 12
 13/// <content>Tracks presentation pose changes and publishes interpolated or authoritative transforms.</content>
 14public partial class SolidBody
 15{
 16    internal void OnVisualize()
 17    {
 846218        if (!HasSolverMobility || !SettingVisuals)
 841619            return;
 20
 4621        if (Context.ResetAccumulationThisVisualize)
 22        {
 4423            if (CanSetVisualPosition)
 1624                SetVisualPosition(Position3d);
 4425            if (CanSetVisualRotation)
 326                StoreVisualRotation(_rotation);
 27        }
 28
 4629        if (CanSetVisualPosition)
 30        {
 1731            Vector3d expectedPosition = Vector3d.SpeedLerp(
 1732                _lastVisualPosition,
 1733                _visualPosition,
 1734                Fixed64.One,
 1735                Context.ExpectedAccumulation);
 1736            SetPositionTransformWorldPosition(expectedPosition);
 37        }
 38
 4639        if (!CanSetVisualRotation)
 4240            return;
 41
 442        Fixed64 targetSpeed = ResolveVisualRotationStep();
 443        FixedQuaternion expectedRotation = _rotationInterpoleSpeed > Fixed64.Zero
 444            ? FixedQuaternion.Slerp(
 445                _rotationTransform.WorldRotation,
 446                _visualRotation,
 447                targetSpeed)
 448            : FixedQuaternion.Slerp(
 449                _lastVisualRotation,
 450                _visualRotation,
 451                targetSpeed);
 452        SetRotationTransformWorldRotation(expectedRotation);
 453    }
 54
 55    private Fixed64 ResolveVisualRotationStep()
 56    {
 457        if (_rotationInterpoleSpeed <= Fixed64.Zero)
 158            return Context.ExpectedAccumulation;
 59
 360        return FixedMath.Clamp01(
 361            Context.DeltaTime
 362            * _rotationInterpoleSpeed
 363            * _rotationSpeed);
 64    }
 65
 66    private void SetPositionTransformWorldPosition(Vector3d position)
 67    {
 649968        ChronicleGuard.ThrowIfTrue(
 649969            !_positionTransform.TrySetWorldPosition(position),
 649970            nameof(FixedTransform),
 649971            "Position transform cannot represent the requested world position.");
 649972    }
 73
 74    private void SetRotationTransformWorldRotation(FixedQuaternion rotation)
 75    {
 17576        ChronicleGuard.ThrowIfTrue(
 17577            !_rotationTransform.TrySetWorldPose(
 17578                _rotationTransform.WorldPosition,
 17579                rotation),
 17580            nameof(FixedTransform),
 17581            "Rotation transform cannot represent the requested world rotation.");
 17582    }
 83
 84    private void PublishAuthoritativePose()
 85    {
 4186        SetPositionTransformWorldPosition(Position3d);
 4187        SetRotationTransformWorldRotation(Rotation);
 4188    }
 89
 90    /// <summary>Advances authoritative pose-change buffers used by presentation and culling.</summary>
 91    public void CheckChangedValues()
 92    {
 93        // Keep each buffer set until the next visualization pass so the host
 94        // observes at least one presentation update after a state change.
 6538095        if (_positionMutated)
 96        {
 6176797            _positionChangedBuffer = _positionMutated;
 6176798            _positionMutated = false;
 6176799            _settingVisualsCounter = Context.FrameRate;
 100        }
 101        else
 102        {
 3613103            _positionChangedBuffer = false;
 104        }
 105
 65380106        if (_rotationMutated)
 107        {
 8686108            _rotationChangedBuffer = _rotationMutated;
 8686109            _rotationMutated = false;
 8686110            _settingVisualsCounter = Context.FrameRate;
 111        }
 112        else
 113        {
 56694114            _rotationChangedBuffer = false;
 115        }
 56694116    }
 117
 118    /// <summary>Advances the position interpolation endpoints.</summary>
 119    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 120    public void SetVisualPosition(Vector3d position)
 121    {
 6528122        _lastVisualPosition = _visualPosition;
 6528123        _visualPosition = position;
 6528124    }
 125
 126    /// <summary>Advances the rotation interpolation endpoints.</summary>
 127    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 128    public void SetVisualRotation(FixedQuaternion rotation) =>
 5129        StoreVisualRotation(rotation.Normalized);
 130
 131    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 132    private void StoreVisualRotation(FixedQuaternion rotation)
 133    {
 6516134        _lastVisualRotation = _visualRotation;
 6516135        _visualRotation = rotation;
 6516136    }
 137}

Methods/Properties

get_LocalCenterOfMassOffset()
set_LocalCenterOfMassOffset(FixedMathSharp.Vector3d)
get_WorldCenterOfMass()
TryGetWorldCenterOfMass(FixedMathSharp.Vector3d&)
TryGetOffsetFromCenterOfMass(Gravitas.CollisionHandling.ContactAnchor,FixedMathSharp.Vector3d&)
GetCenterOfMassAnchor()
ResetCenterOfMassFromCollider()
TryReconfigureCollider(Gravitas.Colliders.ColliderShapeDefinition,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,Gravitas.Colliders.LSCollider&,System.Exception&,System.Action)
TryFindColliderReconfigurationBlocker(Gravitas.Colliders.LSCollider,Gravitas.Colliders.LSCollider&)
CommitColliderReconfiguration(Gravitas.Colliders.ColliderShapeDefinition,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,System.Action)
CaptureSynchronizedStatePublication(System.Action,SwiftCollections.SwiftList`1<System.Exception>&)
CanPublishRootPosition(FixedMathSharp.Vector3d)
GetSupportedRuntimeShape(Gravitas.Colliders.ColliderShapeDefinitionKind)
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)
InvalidateContinuousCollisionTrajectory()
SetSingleContinuousCollisionSegment(Gravitas.CollisionHandling.ContinuousCollisionMotionSegment3D)
AppendContinuousCollisionSegment(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64)
AppendContinuousCollisionSegment(FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64)
ValidateContinuousCollisionPosition(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64)
ResolveCommittedLinearVelocity(FixedMathSharp.Vector3d)
CanAppendContinuousCollisionSegment(FixedMathSharp.Fixed64)
TryResolveContinuousCollisionMotionBound(FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&)
ResolveContinuousCollisionTrajectoryBounds(FixedMathSharp.Fixed64)
RemoveSupersededContinuousCollisionSegments(FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion)
ResolveContinuousCollisionSegment(FixedMathSharp.Fixed64)
GetContinuousCollisionTrajectorySegmentIndex(FixedMathSharp.Fixed64)
GetContinuousCollisionTrajectorySegmentRange(FixedMathSharp.Fixed64,System.Int32&,System.Int32&)
ApplyContinuousCollisionHandoff(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,Gravitas.Colliders.LSCollider,Gravitas.Colliders.LSCollider2D)
ApplyContinuousCollisionHandoff(FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,Gravitas.Colliders.LSCollider,Gravitas.Colliders.LSCollider2D)
ApplyContinuousCollisionHandoffReserved(FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,Gravitas.Colliders.LSCollider,Gravitas.Colliders.LSCollider2D)
ValidateContinuousCollisionHandoffPosition(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64)
HasRemainingHandoffMotion(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64)
CanApplyContinuousCollisionHandoff(FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d&)
TryConsumeContinuousCollisionHandoff(System.Boolean,System.Boolean)
TryConsumeQueuedContinuousCollisionHandoff(System.Boolean,System.Boolean,System.Boolean&)
TryConsumeContinuousCollisionHandoff(System.Boolean,System.Boolean,System.Boolean,System.Boolean&)
NotifyAuthoritativeMovement()
DiscardContinuousCollisionHandoff()
TryResolveContinuousCollision(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d&)
TryResolveContinuousCollision(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d&,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Boolean)
TryApplyContinuousCollisionDynamicResponse(FixedMathSharp.Vector3d,Gravitas.CollisionHandling.ContinuousCollisionTargetKind,Gravitas.Colliders.LSCollider,Gravitas.Colliders.LSCollider2D,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
TryApplyContinuousCollisionDynamicResponse(Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
TryApplyContinuousCollisionMixed2DResponse(Gravitas.SolidBody2D,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
UpdateContinuousCollisionFrameTrajectory(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64)
ResolveContinuousCollisionFrameFraction(FixedMathSharp.Fixed64)
ResolveContinuousCollisionRestitution(Gravitas.SolidBody,FixedMathSharp.Fixed64)
ResolveContinuousCollisionRestitution(Gravitas.SolidBody2D,FixedMathSharp.Fixed64)
RemoveClosingContinuousCollisionVelocity(FixedMathSharp.Vector3d)
IsEligibleDynamicContinuousCollisionTarget(Gravitas.SolidBody)
IsEligibleDynamicMixed2DTarget(Gravitas.SolidBody2D)
ShouldUseContinuousCollision(Gravitas.ContinuousCollisionMode&)
ResolveContinuousCollisionMode()
ResolveContinuousCollisionProxyRadius()
IsClosingContinuousCollisionHit(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
ResolveShapeExactContinuousClosingNormal(Gravitas.Queries.Physics3DHit)
IsValidContinuousCollisionTarget(Gravitas.Colliders.LSCollider)
IsValidMixedContinuousCollisionHit(Gravitas.Colliders.LSCollider2D)
TryGetFirstContinuousCollisionHit(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d&,FixedMathSharp.Fixed64&,Gravitas.CollisionHandling.ContinuousCollisionTargetKind&,Gravitas.Colliders.LSCollider&,Gravitas.Colliders.LSCollider2D&)
TryGetFirstValidContinuousCollisionHit(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,System.Int32,System.Boolean,Gravitas.Queries.Physics3DHit&)
AdmitContinuousSurfaceTriangle(System.Int32,Gravitas.Queries.Physics3DHit&,System.Nullable`1<FixedMathSharp.Geometry.WideDirection3d>)
QueryStaticContinuousCollisionHits(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,System.Boolean&)
TryGetFirstValidMixedContinuousCollisionHit(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,System.Int32,Gravitas.Queries.PhysicsMixedHit&)
UsesMixedStaticManifold(Gravitas.Colliders.LSCollider2D)
TryBuildStaticMixedContinuousCollisionManifold(Gravitas.Colliders.LSCollider2D,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion)
TryApplyStaticMixedContinuousCollisionResponse(Gravitas.Colliders.LSCollider2D,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
TryRefineShapeExactContinuousCollisionHit(Gravitas.Queries.Physics3DHit,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,Gravitas.Queries.Physics3DHit&,System.Boolean&)
TryRefineContinuousCollisionAgainstTargetSphere(Gravitas.Colliders.LSSphereCollider,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,Gravitas.Queries.Physics3DHit&,System.Boolean&)
ApplyShapeExactContinuousContactSlop(Gravitas.Queries.Physics3DHit)
PrepareExactConvexSourceSweep(FixedMathSharp.Vector3d)
IsExactConvexSourceSupported(Gravitas.Colliders.LSCollider)
ContinuousCollisionHitComesBefore(Gravitas.Queries.Physics3DHit,Gravitas.Queries.Physics3DHit)
TryGetFirstDynamicContinuousCollisionHit(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.Queries.Physics3DHit&,FixedMathSharp.Fixed64&)
TryGetDynamicRelativeContinuousCollisionHit(Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.Queries.Physics3DHit&,FixedMathSharp.Fixed64&)
TrySweepRelativeSourceSphere(Gravitas.Colliders.LSSphereCollider,Gravitas.Colliders.LSCollider,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,Gravitas.Queries.Physics3DHit&)
TryGetFirstDynamicMixedContinuousCollisionHit(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.DynamicMixedIntervalHit&)
TryGetDynamicMixed2DContinuousCollisionHit(Gravitas.SolidBody2D,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.DynamicMixedIntervalHit&)
TryResolveKinematicContinuousCollision(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d&)
TryGetFirstKinematicStaticContinuousCollisionHit(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&)
TryApplyKinematicDynamicContinuousCollisionPushes(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Boolean&,FixedMathSharp.Fixed64&)
GatherKinematicDynamic3DContinuousCollisionHits(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
GatherKinematicDynamicMixed2DContinuousCollisionHits(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Boolean&,FixedMathSharp.Fixed64&)
ApplyKinematicContinuousCollisionHandoff(Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
ApplyKinematicContinuousCollisionHandoff(Gravitas.SolidBody2D,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
get_HasRotationInvariantCollider()
TryResolveRotationalContinuousCollision(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d&,FixedMathSharp.FixedQuaternion,FixedMathSharp.FixedQuaternion&)
TryResolveRotationalContinuousCollision(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d&,FixedMathSharp.FixedQuaternion,FixedMathSharp.FixedQuaternion&,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Boolean)
TryResolveKinematicRotationalContinuousCollision(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d&,FixedMathSharp.FixedQuaternion,FixedMathSharp.FixedQuaternion&)
IsRotationalIntervalSeparated(Gravitas.Colliders.LSCollider,FixedMathSharp.Fixed64)
TryGetSpherePairSeparationGap(Gravitas.Colliders.LSCollider,FixedMathSharp.Fixed64&)
TryGetSphereSeparationGap(Gravitas.Colliders.LSSphereCollider,Gravitas.Colliders.LSCollider,FixedMathSharp.Fixed64&)
SampleDynamicRotationalContinuousPose(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
SampleKinematicRotationalContinuousPose(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.FixedQuaternion,FixedMathSharp.Fixed64)
ResolveKinematicAngularDistanceRadians(FixedMathSharp.FixedQuaternion,FixedMathSharp.FixedQuaternion)
TrySampleRotationalContinuousCollision(Gravitas.Colliders.LSCollider,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ManifoldContact&,System.Boolean&)
TryBuildContinuousCollisionManifold(Gravitas.Colliders.LSCollider)
TrySelectRotationalClosingContact(Gravitas.Colliders.LSCollider,Gravitas.CollisionHandling.ContactManifold,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ManifoldContact&)
ResolveSourceOutwardContactNormal(Gravitas.Colliders.LSCollider,Gravitas.CollisionHandling.ManifoldContact)
IsMovingRotationalContinuousCollisionTarget(Gravitas.SolidBody)
ResolveRotationalFrameFraction(FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
OrderRotationalContinuousCollisionPair(Gravitas.Colliders.LSCollider,Gravitas.Colliders.LSCollider&,Gravitas.Colliders.LSCollider&,System.Boolean&)
IntegrateAngularRotation(FixedMathSharp.FixedQuaternion,FixedMathSharp.Fixed64)
IntegrateAngularRotation(FixedMathSharp.FixedQuaternion,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64)
.ctor(Gravitas.IMatterAgent,Gravitas.Colliders.LSCollider)
SetMixedContinuousCollisionSamplePose(FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion)
.ctor(FixedMathSharp.Fixed64,Gravitas.CollisionHandling.MixedContact,System.Boolean,System.Boolean,FixedMathSharp.Fixed64,Gravitas.Colliders.LSCollider2D,Gravitas.CollisionHandling.ContactManifold)
TryFindEarliestMixedRotationalContinuousCollision(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.FixedQuaternion,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Boolean,System.Boolean,Gravitas.SolidBody/RotationalMixed2DHit&)
GatherMixedRotationalContinuousCollisionCandidates(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64)
ConsiderMixedRotationalContinuousCollisionCandidate(Gravitas.Colliders.LSCollider2D,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.FixedQuaternion,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Boolean,System.Boolean&,FixedMathSharp.Fixed64&,System.Int32&,Gravitas.CollisionHandling.MixedContact&,System.Boolean&,FixedMathSharp.Fixed64&,Gravitas.Colliders.LSCollider2D&)
TryGetInvariantMixedPlane(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.FixedQuaternion,System.Boolean,FixedMathSharp.Vector3d&,FixedMathSharp.Geometry.RationalPoint3d&)
TryFindEarliestMixedRotationalContinuousCollisionAgainstTarget(Gravitas.Colliders.LSCollider2D,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.FixedQuaternion,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Boolean,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Boolean,FixedMathSharp.Fixed64&,Gravitas.CollisionHandling.MixedContact&,System.Boolean&,FixedMathSharp.Fixed64&)
SampleMixedRotationalContinuousPairPose(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.FixedQuaternion,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Boolean,Gravitas.SolidBody2D,System.Boolean)
TryGetExactSphereCircleTranslationalContact(Gravitas.SolidBody2D,Gravitas.CollisionHandling.ContinuousCollisionMotionSegment2D,FixedMathSharp.Geometry.FixedSegment2d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Geometry.FixedSegment,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.FixedQuaternion,FixedMathSharp.Fixed64,System.Boolean&,FixedMathSharp.Fixed64&,FixedMathSharp.Fixed64&,FixedMathSharp.Fixed64&,Gravitas.CollisionHandling.MixedContact&)
BuildMixedRotationalContinuousContact(Gravitas.Colliders.LSCollider2D,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.MixedContact&,System.Boolean&,System.Nullable`1<FixedMathSharp.Vector3d>,FixedMathSharp.Geometry.RationalPoint3d)
IsValidMixedRotationalContinuousCollisionTarget(Gravitas.Colliders.LSCollider2D)
IsMovingMixedRotationalContinuousCollisionTarget(Gravitas.SolidBody2D)
TryApplyMixedTranslationalManifoldResponse(Gravitas.SolidBody2D,FixedMathSharp.Vector3d,FixedMathSharp.Vector2d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
TryApplyMixedRotationalContinuousCollisionResponse(Gravitas.Colliders.LSCollider2D,Gravitas.CollisionHandling.MixedContact,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.FixedQuaternion,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Boolean,Gravitas.CollisionHandling.ContactManifold)
TryApplyRotationalContinuousCollisionResponse(Gravitas.SolidBody,Gravitas.CollisionHandling.ManifoldContact,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Boolean)
TryCalculateExactRotationalVelocityDeltas(Gravitas.CollisionHandling.ManifoldContact,Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,Gravitas.SolidBody,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,Gravitas.CollisionHandling.ContactNormalVelocityDeltaResult3D&)
ResolveContinuousCollisionInverseInertia(FixedMathSharp.FixedQuaternion)
StopRotationalContinuousCollision(FixedMathSharp.Vector3d)
HasNearbyRotationalContinuousCollisionTarget(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64)
IsRotatingContinuousCollisionTarget(Gravitas.SolidBody)
IsRotatingContinuousCollisionTarget(Gravitas.SolidBody2D)
GatherRotationalContinuousCollisionCandidates(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64)
TryFindEarliestRotationalContinuousCollision(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.FixedQuaternion,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Boolean,Gravitas.Colliders.LSCollider,FixedMathSharp.Fixed64&,Gravitas.CollisionHandling.ManifoldContact&,System.Boolean&,FixedMathSharp.Fixed64&,Gravitas.Colliders.LSCollider&)
CanExcludeInvariantTangentialContact(Gravitas.Queries.Physics3DHit,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.FixedQuaternion,System.Boolean)
HasInvariantContinuousProjection(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.FixedQuaternion,System.Boolean)
TryGetFullColliderSupportPoint(Gravitas.Colliders.LSCollider,FixedMathSharp.Vector3d,FixedMathSharp.Geometry.RationalPoint3d&)
TryGetLeafColliderSupportPoint(Gravitas.Colliders.LSCollider,FixedMathSharp.Vector3d,FixedMathSharp.Geometry.RationalPoint3d&)
GetExactMeshSupportPoint(Gravitas.Colliders.LSMeshCollider,FixedMathSharp.Vector3d)
TryFindEarliestRotationalContinuousCollisionAgainstTarget(Gravitas.Colliders.LSCollider,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.FixedQuaternion,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Boolean,FixedMathSharp.Fixed64&,Gravitas.CollisionHandling.ManifoldContact&,System.Boolean&,FixedMathSharp.Fixed64&)
SampleRotationalContinuousPairPose(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.FixedQuaternion,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Boolean,Gravitas.SolidBody,System.Boolean)
.ctor(Gravitas.IMatterAgent,Gravitas.Colliders.LSCollider)
get_DynamicId()
get_FreezeAxes()
set_FreezeAxes(Gravitas.BodyFreezeAxes3D)
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)
SetDynamicId(System.Int32)
get_ContinuousCollisionMode()
set_ContinuousCollisionMode(Gravitas.ContinuousCollisionMode)
get_PositionTransform()
get_RotationTransform()
get_PositionChangePending()
SetPosition2d(FixedMathSharp.Vector2d)
get_Position3d()
set_Position3d(FixedMathSharp.Vector3d)
get_HeightPos()
set_HeightPos(FixedMathSharp.Fixed64)
.cctor()
get_VisualPosition()
get_LastVisualPosition()
get_RotationChangePending()
get_Rotation()
set_Rotation(FixedMathSharp.FixedQuaternion)
get_Forward()
get_Up()
get_Right()
get_VisualRotation()
get_LastVisualRotation()
get_IsRotationFullyFrozen()
get_SettingVisuals()
get_LinearVelocity()
get_AngularVelocity()
GetConstrainedInverseMass(FixedMathSharp.Vector3d)
ApplyConstrainedInverseInertia(FixedMathSharp.Vector3d)
GetConstrainedInverseInertiaTensor()
get_GravityScale()
set_GravityScale(FixedMathSharp.Fixed64)
get_IsAtRest()
get_IsSleeping()
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_IsAwakeForCollision()
get_LinearSpeed()
get_LinearAcceleration()
get_AngularSpeed()
get_AngularAcceleration()
get_Weight()
get_World()
Initialize(FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,Gravitas.PhysicsMobilityType)
LateSimulate()
LateSimulate(System.Boolean,System.Boolean)
UpdateKinematicPositionAndRotation()
Sleep()
Wake()
WakeFromCollision()
get_CanSleep()
SetTransformWorldPose(FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion)
Deactivate()
UpdateRotation(FixedMathSharp.FixedQuaternion,FixedMathSharp.Fixed64)
GetWorldPoint(FixedMathSharp.Vector3d)
TryGetWorldPoint(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d&)
GetLocalPoint(FixedMathSharp.Vector3d)
TryGetLocalPoint(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d&)
ResetPosition(FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion)
.ctor(Gravitas.IMatterAgent,Gravitas.Colliders.LSCollider)
get_GroundProbeMode()
set_GroundProbeMode(Gravitas.GroundProbeMode)
get_GroundProbeRadius()
set_GroundProbeRadius(FixedMathSharp.Fixed64)
get_GroundMinNormalDot()
set_GroundMinNormalDot(FixedMathSharp.Fixed64)
get_GroundNormal()
get_HitPlatform()
get_HasHitPoint()
get_HitPoint()
TryGetHitPoint(FixedMathSharp.Vector3d&)
get_IsGrounded()
set_IsGrounded(System.Boolean)
get_WasGrounded()
get_LastGroundedPosition()
SkipGrounding(FixedMathSharp.Fixed64)
UseManualGrounding(System.Boolean)
UseAutomaticGrounding(System.Boolean)
SetManualGrounding(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.FixedTransform)
ClearManualGrounding()
SkipGroundingCoroutine()
CheckGround()
ValidateGroundMinNormalDot(FixedMathSharp.Fixed64)
CheckGroundForSimulation()
get_HasCurrentGroundSupport()
WakeForContactChange()
RefreshAutomaticGroundSupport()
HasCurrentGroundSupportDependency()
CaptureGroundedTransitionState()
CaptureGroundedStepState()
CompleteGroundedStepState()
CheckGround(System.Boolean)
TryFindGroundHit(Gravitas.GroundProbeMode,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,Gravitas.Queries.Physics3DHit&)
TryFindGroundHitWithRay(FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,Gravitas.Queries.Physics3DHit&)
TryFindGroundHitWithSweptSphere(FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,Gravitas.Queries.Physics3DHit&)
IsValidGroundHit(Gravitas.Queries.Physics3DHit)
IsValidGroundCollider(Gravitas.Colliders.LSCollider)
ResolveGroundProbeMode()
ResolveGroundProbeRadius()
ClearGrounding()
ApplyGroundedHeightOrReset()
SetGroundingState(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.FixedTransform)
SetGroundingState(Gravitas.CollisionHandling.ContactAnchor,FixedMathSharp.Vector3d,FixedMathSharp.FixedTransform)
RefreshGroundNormalForce()
ResetGroundCalculations()
get_CanUseAngularInertia()
get_Mass()
set_Mass(FixedMathSharp.Fixed64)
InvalidateMassDependentSolverState()
get_InverseMass()
get_InverseInertiaTensor()
get_CanTranslate()
get_CanRotate()
get_HasSolverMobility()
get_EffectiveInverseMass()
get_EffectiveInverseInertiaTensor()
RefreshMassPropertiesFromColliderShape()
UpdateInertiaTensorOrientation()
CalculateInertiaTensorOrientation(FixedMathSharp.Fixed3x3,FixedMathSharp.Fixed3x3,FixedMathSharp.Fixed3x3&,FixedMathSharp.Fixed3x3&)
RefreshInertiaTensor(FixedMathSharp.Fixed64)
ProjectLinearMotion(FixedMathSharp.Vector3d)
ProjectLinearEndpoint(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
ProjectAngularMotion(FixedMathSharp.Vector3d)
UpdateSleepState()
get_CanCountTowardSleep()
get_ReadyForAutomaticSleep()
AdvanceSleepCounter()
ClearMotionForSleep()
ApplyFreezeConstraintsToMotion()
RefreshPartitionAwakeState()
RefreshPartitionMobility()
AddTorque(FixedMathSharp.Vector3d)
AddForce(FixedMathSharp.Vector3d)
AddLinearImpulse(FixedMathSharp.Vector3d)
AddAngularImpulse(FixedMathSharp.Vector3d)
ApplyCollisionLinearVelocityDelta(FixedMathSharp.Vector3d)
ApplyCollisionAngularVelocityDelta(FixedMathSharp.Vector3d)
CanApplyCollisionVelocityDeltas(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
TryPrepareCollisionVelocityState(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d&,FixedMathSharp.Vector3d&)
ApplyCollisionVelocityState(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
PrepareCollisionPositionCorrection(FixedMathSharp.Vector3d)
ApplyCollisionPositionState(FixedMathSharp.Vector3d)
PublishCollisionPositionState()
AddPositionDisplacement(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
SetPosition(FixedMathSharp.Vector3d)
SetHeight(FixedMathSharp.Fixed64)
SetRotation(FixedMathSharp.FixedQuaternion)
PrepareExplicitBodyPose(FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,System.String,System.String)
PublishExplicitBodyPose(System.Boolean,System.Boolean)
PreflightStaticPoseChange()
PreflightResetPoseChange()
ProcessMovable()
ApplyLinearForces()
ApplyDrag(FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
ApplyGroundLinearFriction()
UpdateLinearVelocity()
RefreshLinearMotionState(FixedMathSharp.Vector3d)
ApplyAngularTorques()
ApplyGroundAngularFriction(FixedMathSharp.Vector3d)
ApplyAngularDamping(FixedMathSharp.Vector3d)
ApplyAngularDissipation(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64)
ApplyConstrainedAngularDissipation(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64)
ResolveConstrainedDissipativeVelocity(FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Signed320,FixedMathSharp.Signed320)
ApplyBoundedDissipation(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64)
ResolveDissipativeVelocity(FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
UpdateAngularVelocity()
ResolveAngularVelocityForStep(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
RefreshAngularMotionState(FixedMathSharp.Vector3d)
ResolveLimitedVelocity(FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64&)
ResolveGroundFrictionImpulse(FixedMathSharp.Fixed64)
NonKinematicUpdate(FixedMathSharp.Vector3d)
PositionBasedOnForce()
ApplyResolvedPosition(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
RotationBasedOnTorque(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
ApplyGyroscopicPrecession(FixedMathSharp.Vector3d)
ContributeReplayHash(Chronicler.Hashing.ChronicleHashWriter&,Gravitas.GravitasReplayHashMode)
WriteContinuousCollisionTrajectory(Chronicler.Hashing.ChronicleHashWriter&)
RecordData(Chronicler.IChronicler)
ApplyLoadedState()
OnVisualize()
ResolveVisualRotationStep()
SetPositionTransformWorldPosition(FixedMathSharp.Vector3d)
SetRotationTransformWorldRotation(FixedMathSharp.FixedQuaternion)
PublishAuthoritativePose()
CheckChangedValues()
SetVisualPosition(FixedMathSharp.Vector3d)
SetVisualRotation(FixedMathSharp.FixedQuaternion)
StoreVisualRotation(FixedMathSharp.FixedQuaternion)