| | | 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 | | |
| | | 8 | | using Chronicler; |
| | | 9 | | using FixedMathSharp; |
| | | 10 | | using Gravitas.Colliders; |
| | | 11 | | using Gravitas.CollisionHandling; |
| | | 12 | | using Gravitas.Queries; |
| | | 13 | | using GridForge.Grids; |
| | | 14 | | using SwiftCollections; |
| | | 15 | | using System; |
| | | 16 | | using System.Runtime.CompilerServices; |
| | | 17 | | |
| | | 18 | | namespace Gravitas; |
| | | 19 | | |
| | | 20 | | /// <summary>Represents deterministic 3D rigid-body state owned by one world context.</summary> |
| | | 21 | | public partial class SolidBody : IRecordable |
| | | 22 | | { |
| | | 23 | | /// <summary>Stores the host-controlled body diagnostic flag.</summary> |
| | | 24 | | public bool Debug = false; |
| | | 25 | | |
| | | 26 | | /// <summary>Gets whether the body is initialized and registered.</summary> |
| | | 27 | | public bool Active { get; private set; } |
| | | 28 | | |
| | 5076 | 29 | | private int _dynamicId = -1; |
| | | 30 | | |
| | | 31 | | /// <summary> |
| | | 32 | | /// Gets the ephemeral simulated-body slot, or <c>-1</c> for a static or |
| | | 33 | | /// unregistered body. |
| | | 34 | | /// </summary> |
| | 87134 | 35 | | public int DynamicId => _dynamicId; |
| | | 36 | | |
| | | 37 | | private BodyFreezeAxes3D _freezeAxes; |
| | | 38 | | |
| | | 39 | | /// <summary> |
| | | 40 | | /// Gets or sets the 3D translational and rotational degrees of freedom |
| | | 41 | | /// frozen for solver response, integration, and CCD. |
| | | 42 | | /// </summary> |
| | | 43 | | public BodyFreezeAxes3D FreezeAxes |
| | | 44 | | { |
| | | 45 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 94 | 46 | | get => _freezeAxes; |
| | | 47 | | set |
| | | 48 | | { |
| | 2312 | 49 | | SwiftThrowHelper.ThrowIfArgument( |
| | 2312 | 50 | | (value & ~BodyFreezeAxes3D.All) != BodyFreezeAxes3D.None, |
| | 2312 | 51 | | nameof(value), |
| | 2312 | 52 | | "Unsupported 3D freeze axis bits."); |
| | | 53 | | |
| | 2312 | 54 | | if (_freezeAxes == value) |
| | 2089 | 55 | | return; |
| | | 56 | | |
| | 223 | 57 | | _freezeAxes = value; |
| | 223 | 58 | | ApplyFreezeConstraintsToMotion(); |
| | 223 | 59 | | if (Active) |
| | 159 | 60 | | RefreshInertiaTensor(); |
| | 223 | 61 | | RefreshPartitionMobility(); |
| | 223 | 62 | | } |
| | | 63 | | } |
| | | 64 | | |
| | | 65 | | /// <summary> |
| | | 66 | | /// Gets whether all translation axes are frozen. |
| | | 67 | | /// </summary> |
| | | 68 | | public bool IsPositionFullyFrozen |
| | | 69 | | { |
| | | 70 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 2155660 | 71 | | get => (_freezeAxes & BodyFreezeAxes3D.Position) == BodyFreezeAxes3D.Position; |
| | | 72 | | } |
| | | 73 | | |
| | | 74 | | private BodyMotionType _motionType; |
| | | 75 | | |
| | | 76 | | /// <summary> |
| | | 77 | | /// Gets this body's explicit solver-controlled, host-controlled, or static |
| | | 78 | | /// runtime role. |
| | | 79 | | /// </summary> |
| | 282 | 80 | | public BodyMotionType MotionType => _motionType; |
| | | 81 | | |
| | | 82 | | /// <summary> |
| | | 83 | | /// Gets whether the solver controls this body. |
| | | 84 | | /// </summary> |
| | 2400703 | 85 | | public bool IsDynamic => _motionType == BodyMotionType.Dynamic; |
| | | 86 | | |
| | | 87 | | /// <summary> |
| | | 88 | | /// Gets whether this body is excluded from per-frame motion. |
| | | 89 | | /// </summary> |
| | 106102 | 90 | | public bool IsStatic => _motionType == BodyMotionType.Static; |
| | | 91 | | |
| | | 92 | | /// <summary> |
| | | 93 | | /// Gets whether the host controls this body's pose. |
| | | 94 | | /// </summary> |
| | 217414 | 95 | | public bool IsKinematic => _motionType == BodyMotionType.Kinematic; |
| | | 96 | | |
| | | 97 | | /// <summary> |
| | | 98 | | /// Changes this registered body's runtime role between fixed-step |
| | | 99 | | /// transactions. |
| | | 100 | | /// </summary> |
| | | 101 | | /// <remarks> |
| | | 102 | | /// The transition preserves body, collider, pair, joint, and host identity, |
| | | 103 | | /// but clears incompatible motion, sleep, CCD, and solver-cache state before |
| | | 104 | | /// repartitioning. Freeze axes remain independent of the selected role. |
| | | 105 | | /// </remarks> |
| | | 106 | | /// <exception cref="ArgumentOutOfRangeException"> |
| | | 107 | | /// <paramref name="motionType"/> is undefined. |
| | | 108 | | /// </exception> |
| | | 109 | | /// <exception cref="InvalidOperationException"> |
| | | 110 | | /// The body is not currently registered, its context registration has been |
| | | 111 | | /// reset, or a simulation transaction or callback is active. |
| | | 112 | | /// </exception> |
| | | 113 | | public void SetMotionType(BodyMotionType motionType) |
| | | 114 | | { |
| | 43 | 115 | | if (!PrepareMotionTypeTransition(motionType)) |
| | 1 | 116 | | return; |
| | | 117 | | |
| | 34 | 118 | | CommitMotionTypeTransition(motionType); |
| | 34 | 119 | | } |
| | | 120 | | |
| | | 121 | | internal bool PrepareMotionTypeTransition(BodyMotionType motionType) |
| | | 122 | | { |
| | 153 | 123 | | motionType.ThrowIfInvalid(nameof(motionType)); |
| | 152 | 124 | | ThrowIfRuntimeRegistrationMissing(); |
| | 148 | 125 | | if (_motionType == motionType) |
| | 93 | 126 | | return false; |
| | | 127 | | |
| | 55 | 128 | | Context.ThrowIfFixedStepMutationNotAllowed(); |
| | 53 | 129 | | Collider.ValidateCurrentRuntimeTransform(); |
| | 51 | 130 | | if (motionType != BodyMotionType.Dynamic) |
| | 37 | 131 | | PublishAuthoritativePose(); |
| | | 132 | | |
| | 51 | 133 | | return true; |
| | | 134 | | } |
| | | 135 | | |
| | | 136 | | internal void CommitMotionTypeTransition(BodyMotionType motionType) |
| | | 137 | | { |
| | 50 | 138 | | ReconcileMotionTypeRegistration(motionType); |
| | | 139 | | |
| | 50 | 140 | | ClearMotionForSleep(); |
| | 50 | 141 | | _isSleeping = false; |
| | 50 | 142 | | _sleepFrameCount = 0; |
| | 50 | 143 | | InvalidateContinuousCollisionTrajectory(); |
| | 50 | 144 | | ApplyFreezeConstraintsToMotion(); |
| | 50 | 145 | | RefreshInertiaTensor(); |
| | 50 | 146 | | RefreshPartitionMobility(); |
| | 50 | 147 | | } |
| | | 148 | | |
| | | 149 | | internal void ApplyLoadedMotionType(BodyMotionType motionType) |
| | | 150 | | { |
| | 51 | 151 | | motionType.ThrowIfInvalid(nameof(motionType)); |
| | 51 | 152 | | if (_motionType == motionType) |
| | 33 | 153 | | return; |
| | | 154 | | |
| | 18 | 155 | | if (!Active) |
| | | 156 | | { |
| | 4 | 157 | | _motionType = motionType; |
| | 4 | 158 | | return; |
| | | 159 | | } |
| | | 160 | | |
| | 14 | 161 | | ReconcileMotionTypeRegistration(motionType); |
| | 14 | 162 | | InvalidateContinuousCollisionTrajectory(); |
| | 14 | 163 | | } |
| | | 164 | | |
| | | 165 | | internal void PreflightLoadedMotionType(BodyMotionType motionType) |
| | | 166 | | { |
| | 55 | 167 | | motionType.ThrowIfInvalid(nameof(motionType)); |
| | 54 | 168 | | if (!Active) |
| | 6 | 169 | | return; |
| | | 170 | | |
| | 48 | 171 | | ThrowIfRuntimeRegistrationMissing(); |
| | 46 | 172 | | Context.ThrowIfFixedStepMutationNotAllowed(); |
| | 46 | 173 | | Collider.ValidateCurrentRuntimeTransform(); |
| | 46 | 174 | | } |
| | | 175 | | |
| | | 176 | | private void ThrowIfRuntimeRegistrationMissing() |
| | | 177 | | { |
| | 6698 | 178 | | SwiftThrowHelper.ThrowIfTrue( |
| | 6698 | 179 | | !Active, |
| | 6698 | 180 | | nameof(SolidBody), |
| | 6698 | 181 | | "Body runtime state cannot change before initialization or after deactivation."); |
| | 6696 | 182 | | SwiftThrowHelper.ThrowIfTrue( |
| | 6696 | 183 | | !Context.Physics.TryGetColliderById(Collider.Id, out LSCollider? registeredCollider) |
| | 6696 | 184 | | || !ReferenceEquals(registeredCollider, Collider), |
| | 6696 | 185 | | nameof(SolidBody), |
| | 6696 | 186 | | "Body runtime state cannot change after its registration has been reset or replaced."); |
| | 6690 | 187 | | } |
| | | 188 | | |
| | | 189 | | private void ReconcileMotionTypeRegistration(BodyMotionType motionType) |
| | | 190 | | { |
| | 64 | 191 | | BodyMotionType previousMotionType = _motionType; |
| | 64 | 192 | | Context.Physics.ClearWarmStartCachesForCollider(Collider); |
| | 64 | 193 | | Context.Constraints3D.ClearSolverCachesForBody(this); |
| | 64 | 194 | | Context.Physics.InvalidateContinuousCollisionStateForMotionTypeChange(this, DynamicId); |
| | | 195 | | |
| | 64 | 196 | | _motionType = motionType; |
| | 64 | 197 | | Context.Physics.RefreshBodyMotionTypeRegistration(this, previousMotionType); |
| | 64 | 198 | | Context.Physics.RefreshColliderServiceRefreshRegistration(Collider); |
| | 64 | 199 | | } |
| | | 200 | | |
| | 29 | 201 | | internal void SetDynamicId(int dynamicId) => _dynamicId = dynamicId; |
| | | 202 | | |
| | | 203 | | private ContinuousCollisionMode _continuousCollisionMode = ContinuousCollisionMode.Inherit; |
| | 5076 | 204 | | private int _continuousCollisionFrameToken = int.MinValue; |
| | 5076 | 205 | | private readonly SwiftList<ContinuousCollisionMotionSegment3D> _continuousCollisionTrajectory = |
| | 5076 | 206 | | new(PhysicsSettings.DefaultContinuousCollisionMaxToiIterations + 1); |
| | | 207 | | private Vector3d _continuousCollisionAngularVelocityStepStart; |
| | | 208 | | private bool _continuousCollisionHandoffPending; |
| | 5076 | 209 | | private int _continuousCollisionHandoffToken = int.MinValue; |
| | | 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)] |
| | 94 | 220 | | get => _continuousCollisionMode; |
| | | 221 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 222 | | set |
| | | 223 | | { |
| | 673 | 224 | | value.ThrowIfInvalid(nameof(value)); |
| | 669 | 225 | | _continuousCollisionMode = value; |
| | 669 | 226 | | } |
| | | 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 the most recent late simulation step reached the configured continuous-collision TOI iteration limi |
| | | 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> |
| | 96 | 241 | | public FixedTransform PositionTransform => _positionTransform; |
| | | 242 | | |
| | | 243 | | private FixedTransform _rotationTransform = null!; |
| | | 244 | | /// <summary>Gets the host transform that receives authoritative or visual rotation updates.</summary> |
| | 24 | 245 | | 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> |
| | 13644 | 252 | | public bool PositionChangePending => _positionMutated || _positionChangedBuffer; |
| | | 253 | | |
| | | 254 | | private Vector2d _position2dUnmarked; |
| | | 255 | | private void SetPosition2d(Vector2d value) |
| | | 256 | | { |
| | 19330 | 257 | | if (_position2dUnmarked == value) |
| | 1645 | 258 | | return; |
| | | 259 | | |
| | 17685 | 260 | | _position2dUnmarked = value; |
| | 17685 | 261 | | _positionMutated = true; |
| | 17685 | 262 | | } |
| | | 263 | | |
| | | 264 | | /// <summary>Gets the authoritative world-space position.</summary> |
| | | 265 | | public Vector3d Position3d |
| | | 266 | | { |
| | 510197 | 267 | | get => _position2dUnmarked.ToVector3d(_heightPosUnmarked); |
| | | 268 | | private set |
| | | 269 | | { |
| | 27444 | 270 | | if (Position3d == value) |
| | 25354 | 271 | | return; |
| | 2090 | 272 | | _position2dUnmarked.Set(value.X, value.Z); |
| | 2090 | 273 | | _heightPosUnmarked = value.Y; |
| | 2090 | 274 | | _positionMutated = true; |
| | 2090 | 275 | | } |
| | | 276 | | } |
| | | 277 | | |
| | 5076 | 278 | | 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 | | { |
| | 7 | 282 | | get => _heightPosUnmarked; |
| | | 283 | | private set |
| | | 284 | | { |
| | 19361 | 285 | | if (_heightPosUnmarked == value) |
| | 16039 | 286 | | return; |
| | 3322 | 287 | | _heightPosUnmarked = value; |
| | 3322 | 288 | | _positionMutated = true; |
| | 3322 | 289 | | } |
| | | 290 | | } |
| | | 291 | | |
| | | 292 | | private Vector3d _lastPosition; |
| | | 293 | | |
| | | 294 | | private const int DefaultBodyHitBufferCapacity = 16; |
| | 5076 | 295 | | private readonly SwiftList<Physics3DHit> _continuousCollisionHits = new(DefaultBodyHitBufferCapacity); |
| | 5076 | 296 | | private readonly SwiftList<PhysicsMixedHit> _continuousMixedCollisionHits = new(DefaultBodyHitBufferCapacity); |
| | 5076 | 297 | | private readonly SwiftList<int> _rotationalContinuousCollisionCandidateIds = new(DefaultBodyHitBufferCapacity); |
| | 5076 | 298 | | private readonly ContactManifold _rotationalContinuousCollisionManifold = new(); |
| | 5076 | 299 | | private readonly SweptSphereQueryWorker _shapeExactContinuousSweepWorker = new(); |
| | 5076 | 300 | | 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. |
| | 1 | 303 | | private static readonly Fixed64 ShapeExactContinuousContactSlop = |
| | 1 | 304 | | 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> |
| | 1 | 312 | | public Vector3d VisualPosition => _visualPosition; |
| | | 313 | | |
| | | 314 | | private Vector3d _lastVisualPosition; |
| | | 315 | | /// <summary>Gets the previous position interpolation endpoint.</summary> |
| | 1 | 316 | | 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> |
| | 1146 | 325 | | 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 | | { |
| | 447670 | 332 | | get => _rotation; |
| | | 333 | | private set |
| | | 334 | | { |
| | 26011 | 335 | | if (_rotation == value) |
| | 15757 | 336 | | return; |
| | 10254 | 337 | | _rotation = value; |
| | 10254 | 338 | | _rotationMutated = true; |
| | 10254 | 339 | | } |
| | | 340 | | } |
| | | 341 | | |
| | | 342 | | /// <summary>Gets the body's world-space forward direction.</summary> |
| | 1 | 343 | | public Vector3d Forward => _rotation.Rotate(Vector3d.Forward); |
| | | 344 | | /// <summary>Gets the body's world-space up direction.</summary> |
| | 1 | 345 | | public Vector3d Up => _rotation.Rotate(Vector3d.Up); |
| | | 346 | | /// <summary>Gets the body's world-space right direction.</summary> |
| | 4 | 347 | | 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> |
| | 13 | 354 | | public FixedQuaternion VisualRotation => _visualRotation; |
| | | 355 | | |
| | | 356 | | private FixedQuaternion _lastVisualRotation; |
| | | 357 | | /// <summary>Gets the previous rotation interpolation endpoint.</summary> |
| | 1 | 358 | | 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)] |
| | 2090182 | 366 | | get => (_freezeAxes & BodyFreezeAxes3D.Rotation) == BodyFreezeAxes3D.Rotation; |
| | | 367 | | } |
| | | 368 | | |
| | | 369 | | /// <summary>Controls visual rotation interpolation when the host agent is not interacting.</summary> |
| | 5076 | 370 | | public Fixed64 DefaultRotationSpeed = (Fixed64)30; // 1 for NPC... |
| | | 371 | | |
| | | 372 | | /// <summary>Controls visual rotation interpolation while the host agent is interacting.</summary> |
| | 5076 | 373 | | 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; |
| | 13522 | 381 | | 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> |
| | 337812 | 389 | | 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> |
| | 357795 | 398 | | 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 | | private Fixed3x3 _inertiaTensor; |
| | | 408 | | private Fixed3x3 _worldInertiaTensor; |
| | | 409 | | private Fixed3x3 _inverseLocalInertiaTensor; |
| | | 410 | | private Fixed3x3 _inverseInertiaTensor; |
| | | 411 | | /// <summary>Gets the constrained world-space inverse inertia tensor.</summary> |
| | 9 | 412 | | public Fixed3x3 InverseInertiaTensor => _inverseInertiaTensor; |
| | | 413 | | |
| | | 414 | | /// <summary> |
| | | 415 | | /// Gets whether solver-side response may translate this body. |
| | | 416 | | /// </summary> |
| | 1299539 | 417 | | public bool CanTranslate => Active && _dynamicId >= 0 && IsDynamic && !IsPositionFullyFrozen && InverseMass > Fixed6 |
| | | 418 | | |
| | | 419 | | /// <summary> |
| | | 420 | | /// Gets whether solver-side response may rotate this body. |
| | | 421 | | /// </summary> |
| | 1131850 | 422 | | public bool CanRotate => Active |
| | 1131850 | 423 | | && _dynamicId >= 0 |
| | 1131850 | 424 | | && IsDynamic |
| | 1131850 | 425 | | && !IsRotationFullyFrozen |
| | 1131850 | 426 | | && _inverseInertiaTensor != Fixed3x3.Zero; |
| | | 427 | | |
| | 432717 | 428 | | internal bool HasSolverMobility => CanTranslate || CanRotate; |
| | | 429 | | |
| | | 430 | | /// <summary> |
| | | 431 | | /// Gets the inverse mass that should be used by collision response. |
| | | 432 | | /// Translation-frozen, static, and kinematic bodies expose their raw mass |
| | | 433 | | /// but contribute zero constrained inverse mass. |
| | | 434 | | /// </summary> |
| | 34688 | 435 | | public Fixed64 EffectiveInverseMass => CanTranslate ? InverseMass : Fixed64.Zero; |
| | | 436 | | |
| | | 437 | | /// <summary> |
| | | 438 | | /// Gets the inverse inertia tensor that should be used by collision response. |
| | | 439 | | /// Bodies that cannot rotate expose a zero tensor even when raw inertia is available. |
| | | 440 | | /// </summary> |
| | 184 | 441 | | public Fixed3x3 EffectiveInverseInertiaTensor => CanRotate ? _inverseInertiaTensor : Fixed3x3.Zero; |
| | | 442 | | |
| | | 443 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 444 | | internal Vector3d ProjectLinearMotion(Vector3d value) |
| | | 445 | | { |
| | 924643 | 446 | | if (value == Vector3d.Zero || IsPositionFullyFrozen) |
| | 26308 | 447 | | return Vector3d.Zero; |
| | | 448 | | |
| | 898335 | 449 | | Fixed64 x = (_freezeAxes & BodyFreezeAxes3D.PositionX) == BodyFreezeAxes3D.PositionX ? Fixed64.Zero : value.X; |
| | 898335 | 450 | | Fixed64 y = (_freezeAxes & BodyFreezeAxes3D.PositionY) == BodyFreezeAxes3D.PositionY ? Fixed64.Zero : value.Y; |
| | 898335 | 451 | | Fixed64 z = (_freezeAxes & BodyFreezeAxes3D.PositionZ) == BodyFreezeAxes3D.PositionZ ? Fixed64.Zero : value.Z; |
| | 898335 | 452 | | return new Vector3d(x, y, z); |
| | | 453 | | } |
| | | 454 | | |
| | | 455 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 456 | | private Vector3d ProjectLinearEndpoint(Vector3d start, Vector3d end) |
| | | 457 | | { |
| | 12690 | 458 | | Fixed64 x = (_freezeAxes & BodyFreezeAxes3D.PositionX) == BodyFreezeAxes3D.PositionX ? start.X : end.X; |
| | 12690 | 459 | | Fixed64 y = (_freezeAxes & BodyFreezeAxes3D.PositionY) == BodyFreezeAxes3D.PositionY ? start.Y : end.Y; |
| | 12690 | 460 | | Fixed64 z = (_freezeAxes & BodyFreezeAxes3D.PositionZ) == BodyFreezeAxes3D.PositionZ ? start.Z : end.Z; |
| | 12690 | 461 | | return new Vector3d(x, y, z); |
| | | 462 | | } |
| | | 463 | | |
| | | 464 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 465 | | internal Vector3d ProjectAngularMotion(Vector3d value) |
| | | 466 | | { |
| | 1062879 | 467 | | if (value == Vector3d.Zero || IsRotationFullyFrozen) |
| | 69011 | 468 | | return Vector3d.Zero; |
| | | 469 | | |
| | 993868 | 470 | | Fixed64 x = (_freezeAxes & BodyFreezeAxes3D.RotationX) == BodyFreezeAxes3D.RotationX ? Fixed64.Zero : value.X; |
| | 993868 | 471 | | Fixed64 y = (_freezeAxes & BodyFreezeAxes3D.RotationY) == BodyFreezeAxes3D.RotationY ? Fixed64.Zero : value.Y; |
| | 993868 | 472 | | Fixed64 z = (_freezeAxes & BodyFreezeAxes3D.RotationZ) == BodyFreezeAxes3D.RotationZ ? Fixed64.Zero : value.Z; |
| | 993868 | 473 | | return new Vector3d(x, y, z); |
| | | 474 | | } |
| | | 475 | | |
| | | 476 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 477 | | internal Fixed64 GetConstrainedInverseMass(Vector3d axis) |
| | | 478 | | { |
| | 556070 | 479 | | if (!CanTranslate || axis == Vector3d.Zero) |
| | 15753 | 480 | | return Fixed64.Zero; |
| | | 481 | | |
| | 540317 | 482 | | Fixed64 axisMagnitudeSquared = axis.MagnitudeSquared; |
| | 540317 | 483 | | if (axisMagnitudeSquared <= Fixed64.Epsilon) |
| | 1 | 484 | | return Fixed64.Zero; |
| | | 485 | | |
| | 540316 | 486 | | Vector3d allowedAxis = ProjectLinearMotion(axis); |
| | 540316 | 487 | | Fixed64 allowedScale = Vector3d.Dot(allowedAxis, axis) / axisMagnitudeSquared; |
| | 540316 | 488 | | return allowedScale > Fixed64.Zero ? InverseMass * allowedScale : Fixed64.Zero; |
| | | 489 | | } |
| | | 490 | | |
| | | 491 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 492 | | internal Vector3d ApplyConstrainedInverseInertia(Vector3d torqueAxis) |
| | | 493 | | { |
| | 751005 | 494 | | if (!CanRotate || torqueAxis == Vector3d.Zero) |
| | 63875 | 495 | | return Vector3d.Zero; |
| | | 496 | | |
| | 687130 | 497 | | return ProjectAngularMotion(_inverseInertiaTensor * torqueAxis); |
| | | 498 | | } |
| | | 499 | | |
| | | 500 | | internal Fixed3x3 GetConstrainedInverseInertiaTensor() => |
| | 58716 | 501 | | new( |
| | 58716 | 502 | | ApplyConstrainedInverseInertia(Vector3d.Right), |
| | 58716 | 503 | | ApplyConstrainedInverseInertia(Vector3d.Up), |
| | 58716 | 504 | | ApplyConstrainedInverseInertia(Vector3d.Forward)); |
| | | 505 | | |
| | 5076 | 506 | | private Fixed64 _gravityScale = Fixed64.One; |
| | | 507 | | |
| | | 508 | | /// <summary> |
| | | 509 | | /// Multiplies context environment gravity for this body. Zero disables gravity-derived acceleration and grounded we |
| | | 510 | | /// </summary> |
| | | 511 | | public Fixed64 GravityScale |
| | | 512 | | { |
| | | 513 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | 92 | 514 | | get => _gravityScale; |
| | | 515 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 516 | | set |
| | | 517 | | { |
| | 56 | 518 | | SwiftThrowHelper.ThrowIfArgument( |
| | 56 | 519 | | value < Fixed64.Zero, |
| | 56 | 520 | | nameof(value), |
| | 56 | 521 | | "Gravity scale cannot be negative."); |
| | 55 | 522 | | _gravityScale = value; |
| | 55 | 523 | | RefreshGroundNormalForce(); |
| | 55 | 524 | | } |
| | | 525 | | } |
| | | 526 | | |
| | | 527 | | /// <summary>Gets whether both linear and angular velocity are exactly zero.</summary> |
| | 3 | 528 | | public bool IsAtRest => _linearVelocity.IsZero && _angularVelocity.IsZero; |
| | | 529 | | |
| | | 530 | | private bool _isSleeping; |
| | | 531 | | private int _sleepFrameCount; |
| | 5076 | 532 | | private bool _sleepEnabled = true; |
| | 5076 | 533 | | private int _sleepFrameThreshold = 16; |
| | 5076 | 534 | | private Fixed64 _sleepLinearSpeedThreshold = (Fixed64)0.001f; |
| | 5076 | 535 | | private Fixed64 _sleepAngularSpeedThreshold = (Fixed64)0.001f; |
| | | 536 | | |
| | | 537 | | /// <summary> |
| | | 538 | | /// Gets whether this dynamic body is currently excluded from solver work until a deterministic wake event occurs. |
| | | 539 | | /// </summary> |
| | 343393 | 540 | | public bool IsSleeping => _isSleeping; |
| | | 541 | | |
| | | 542 | | /// <summary> |
| | | 543 | | /// Enables deterministic sleep evaluation for this body. |
| | | 544 | | /// </summary> |
| | | 545 | | public bool SleepEnabled |
| | | 546 | | { |
| | 13028 | 547 | | get => _sleepEnabled; |
| | | 548 | | set |
| | | 549 | | { |
| | 27 | 550 | | if (_sleepEnabled == value) |
| | 7 | 551 | | return; |
| | | 552 | | |
| | 20 | 553 | | _sleepEnabled = value; |
| | 20 | 554 | | if (!value) |
| | 19 | 555 | | Wake(); |
| | 20 | 556 | | } |
| | | 557 | | } |
| | | 558 | | |
| | | 559 | | /// <summary> |
| | | 560 | | /// Number of consecutive fixed frames below sleep thresholds required before the body sleeps. |
| | | 561 | | /// </summary> |
| | | 562 | | public int SleepFrameThreshold |
| | | 563 | | { |
| | 2 | 564 | | get => _sleepFrameThreshold; |
| | | 565 | | set |
| | | 566 | | { |
| | 11 | 567 | | SwiftThrowHelper.ThrowIfNegative(value, nameof(value)); |
| | 11 | 568 | | _sleepFrameThreshold = value; |
| | 11 | 569 | | } |
| | | 570 | | } |
| | | 571 | | |
| | | 572 | | /// <summary> |
| | | 573 | | /// Linear speed at or below which the body can count toward sleeping. |
| | | 574 | | /// </summary> |
| | | 575 | | public Fixed64 SleepLinearSpeedThreshold |
| | | 576 | | { |
| | 4656 | 577 | | get => _sleepLinearSpeedThreshold; |
| | | 578 | | set |
| | | 579 | | { |
| | 4 | 580 | | SwiftThrowHelper.ThrowIfArgument( |
| | 4 | 581 | | value < Fixed64.Zero, |
| | 4 | 582 | | nameof(value), |
| | 4 | 583 | | "Sleep linear speed threshold cannot be negative."); |
| | 4 | 584 | | _sleepLinearSpeedThreshold = value; |
| | 4 | 585 | | } |
| | | 586 | | } |
| | | 587 | | |
| | | 588 | | /// <summary> |
| | | 589 | | /// Angular speed at or below which the body can count toward sleeping. |
| | | 590 | | /// </summary> |
| | | 591 | | public Fixed64 SleepAngularSpeedThreshold |
| | | 592 | | { |
| | 926 | 593 | | get => _sleepAngularSpeedThreshold; |
| | | 594 | | set |
| | | 595 | | { |
| | 2 | 596 | | SwiftThrowHelper.ThrowIfArgument( |
| | 2 | 597 | | value < Fixed64.Zero, |
| | 2 | 598 | | nameof(value), |
| | 2 | 599 | | "Sleep angular speed threshold cannot be negative."); |
| | 2 | 600 | | _sleepAngularSpeedThreshold = value; |
| | 2 | 601 | | } |
| | | 602 | | } |
| | | 603 | | |
| | 318970 | 604 | | internal bool IsAwakeForCollision => HasSolverMobility && !IsSleeping; |
| | | 605 | | |
| | | 606 | | // LinearVelocity magnitude |
| | | 607 | | private Fixed64 _linearSpeed; |
| | | 608 | | /// <summary>Gets the magnitude of <see cref="LinearVelocity"/>.</summary> |
| | 861 | 609 | | public Fixed64 LinearSpeed => _linearSpeed; |
| | | 610 | | |
| | | 611 | | /// <summary> |
| | | 612 | | /// Represents the total accumulated force on the object. This can be the sum of all external forces acting on the o |
| | | 613 | | /// Changing this value directly affects the object's acceleration and, subsequently, its velocity and position. |
| | | 614 | | /// </summary> |
| | | 615 | | private Vector3d _linearAccelerationStore; |
| | | 616 | | private Vector3d _deltaAcceleration; |
| | | 617 | | |
| | | 618 | | private Vector3d _linearAcceleration; |
| | | 619 | | /// <summary>Gets the linear acceleration measured during the latest integration step.</summary> |
| | 3 | 620 | | public Vector3d LinearAcceleration => _linearAcceleration; |
| | | 621 | | |
| | | 622 | | private Fixed64 _angularSpeed; |
| | | 623 | | /// <summary>Gets the magnitude of <see cref="AngularVelocity"/>.</summary> |
| | 11 | 624 | | public Fixed64 AngularSpeed => _angularSpeed; |
| | | 625 | | |
| | | 626 | | private Vector3d _angularAccelerationStore; |
| | | 627 | | |
| | | 628 | | private Vector3d _angularAcceleration; |
| | | 629 | | /// <summary>Gets the angular acceleration measured during the latest integration step.</summary> |
| | 9 | 630 | | public Vector3d AngularAcceleration => _angularAcceleration; |
| | | 631 | | |
| | | 632 | | |
| | | 633 | | /// <summary> |
| | | 634 | | /// Represents a body's resistance to movement, akin to air resistance. |
| | | 635 | | /// Higher values slow down the body more quickly in absence of other forces. |
| | | 636 | | /// The effect is significant when bodies are expected to slow down or stop without sustained forces. |
| | | 637 | | /// It's not constrained between 0 and 1, depends on the object's shape and the flow conditions. |
| | | 638 | | /// </summary> |
| | 5076 | 639 | | public Fixed64 LinearDragCoefficient = (Fixed64)0.75f; |
| | | 640 | | |
| | 5076 | 641 | | private Fixed64 AngularDragCoefficient = (Fixed64)0.75f; |
| | | 642 | | |
| | | 643 | | /// <summary> |
| | | 644 | | /// Represents the normal force on the object. |
| | | 645 | | /// It's usually perpendicular to the contact surface and prevents the object from "falling" into the surface. |
| | | 646 | | /// Can be updated to simulate changes in terrain or surface inclination. |
| | | 647 | | /// </summary> |
| | | 648 | | private Vector3d _normalForce; |
| | | 649 | | |
| | | 650 | | // Mass (in kilograms) is the measure of the amount of matter in a body |
| | | 651 | | // Divide the weight (in Newtons) by the acceleration of gravity to determine the mass of an object (measured in Ki |
| | | 652 | | // On Earth, gravity accelerates at 9.8 meters per second squared (9.8 m/s^2) |
| | | 653 | | // ex: 150 Pounds x PhysicsEnvironment.PoundToNewton = 667 Newtons / 9.8 m/s^2 = 68 kilograms * PhysicsEnvironment. |
| | | 654 | | /// <summary>Mass used by integration and collision response.</summary> |
| | | 655 | | public Fixed64 Mass; |
| | | 656 | | |
| | | 657 | | // InverseMass is the reciprocal of mass, which is useful for performance reasons |
| | | 658 | | // when mass is used in calculations. |
| | | 659 | | /// <summary>Gets the reciprocal mass, or zero when <see cref="Mass"/> is zero.</summary> |
| | 1832899 | 660 | | public Fixed64 InverseMass => Mass != Fixed64.Zero |
| | 1832899 | 661 | | ? Fixed64.One / Mass |
| | 1832899 | 662 | | : Fixed64.Zero; |
| | | 663 | | |
| | | 664 | | // Weight is a measure of how the force of gravity acts upon the mass. |
| | | 665 | | // Weight (in Newtons) is mass (in Kilograms) multiplied by the acceleration of gravity (g). |
| | | 666 | | // ex: 68 kg * 9.8 m/s^2 = 667 Newtons / PhysicsEnvironment.PoundToNewton = 150 Pounds |
| | 306 | 667 | | private Fixed64 Weight => Mass * Context.Environment.Gravity * _gravityScale; |
| | | 668 | | |
| | | 669 | | /// <summary>Gets the host agent that owns this body.</summary> |
| | | 670 | | public IMatterAgent Agent { get; private set; } = null!; |
| | | 671 | | |
| | | 672 | | /// <summary>Gets the world context supplied by the host agent.</summary> |
| | | 673 | | public GravitasWorldContext Context { get; private set; } = null!; |
| | | 674 | | |
| | | 675 | | /// <summary>Gets the GridForge world owned by <see cref="Context"/>.</summary> |
| | 11597 | 676 | | public GridWorld World => Context.World; |
| | | 677 | | |
| | | 678 | | /// <summary>Gets the runtime collider attached to this body.</summary> |
| | | 679 | | public LSCollider Collider { get; private set; } = null!; |
| | | 680 | | |
| | | 681 | | /// <summary> |
| | | 682 | | /// Called after authoritative position or rotation changes have been |
| | | 683 | | /// committed during simulation. |
| | | 684 | | /// </summary> |
| | | 685 | | public Action? OnMoved; |
| | | 686 | | |
| | | 687 | | /// <summary>Creates a 3D body bound to a host agent and collider.</summary> |
| | 5076 | 688 | | public SolidBody(IMatterAgent agent, LSCollider collider) |
| | | 689 | | { |
| | 5076 | 690 | | SwiftThrowHelper.ThrowIfNull(agent, nameof(agent)); |
| | 5076 | 691 | | SwiftThrowHelper.ThrowIfNull(collider, nameof(collider)); |
| | | 692 | | |
| | 5076 | 693 | | GravitasWorldContext context = agent.Context; |
| | 5076 | 694 | | SwiftThrowHelper.ThrowIfNull(context, nameof(agent.Context)); |
| | 5076 | 695 | | SwiftThrowHelper.ThrowIfArgument( |
| | 5076 | 696 | | collider.TryGetBoundContext(out GravitasWorldContext? colliderContext) |
| | 5076 | 697 | | && !ReferenceEquals(colliderContext, context), |
| | 5076 | 698 | | nameof(collider), |
| | 5076 | 699 | | "Agent and collider must be bound to the same context."); |
| | | 700 | | |
| | 5075 | 701 | | Agent = agent; |
| | 5075 | 702 | | Collider = collider; |
| | 5075 | 703 | | Context = context; |
| | 5075 | 704 | | Collider.BindContext(context); |
| | | 705 | | |
| | 5075 | 706 | | _positionTransform = agent.Transform; |
| | 5075 | 707 | | _rotationTransform = agent.Transform; |
| | | 708 | | |
| | 5075 | 709 | | _rotationSpeed = DefaultRotationSpeed; |
| | 5075 | 710 | | _rotationInterpoleSpeed = Fixed64.Zero; |
| | 5075 | 711 | | } |
| | | 712 | | |
| | | 713 | | /// <summary>Initializes and registers the body at an authoritative world pose.</summary> |
| | | 714 | | public void Initialize( |
| | | 715 | | Vector3d startPosition, |
| | | 716 | | FixedQuaternion startRotation, |
| | | 717 | | BodyMotionType motionType = BodyMotionType.Dynamic) |
| | | 718 | | { |
| | 5091 | 719 | | motionType.ThrowIfInvalid(nameof(motionType)); |
| | 5091 | 720 | | SwiftThrowHelper.ThrowIfArgument( |
| | 5091 | 721 | | Collider.Id >= 0 |
| | 5091 | 722 | | || (Collider.HasHostBinding && !ReferenceEquals(Collider.Body, this)), |
| | 5091 | 723 | | nameof(Collider), |
| | 5091 | 724 | | "Body collider must be unregistered and free of another host binding before initialization."); |
| | 5089 | 725 | | FixedQuaternion normalizedRotation = startRotation.Normalized; |
| | 5089 | 726 | | Collider.PreflightBodyInitialization(this, startPosition, normalizedRotation); |
| | | 727 | | |
| | 5082 | 728 | | _motionType = motionType; |
| | 5082 | 729 | | Active = true; |
| | | 730 | | |
| | 5082 | 731 | | InvalidateContinuousCollisionTrajectory(); |
| | 5082 | 732 | | ClearMotionForSleep(); |
| | 5082 | 733 | | _normalForce = Vector3d.Zero; |
| | 5082 | 734 | | _isSleeping = false; |
| | 5082 | 735 | | _sleepFrameCount = 0; |
| | | 736 | | |
| | 5082 | 737 | | _isGrounded = false; |
| | 5082 | 738 | | _wasGrounded = false; |
| | 5082 | 739 | | _groundedTransitionCapturedForStep = false; |
| | 5082 | 740 | | _skipGroundingCheck = false; |
| | 5082 | 741 | | _lastGroundCheckFrame = int.MinValue; |
| | 5082 | 742 | | ResetGroundCalculations(); |
| | | 743 | | |
| | 5082 | 744 | | _positionChangedBuffer = true; |
| | 5082 | 745 | | _position2dUnmarked = startPosition.ToVector2d(); |
| | 5082 | 746 | | _lastGroundedPosition = _lastPosition = startPosition; |
| | 5082 | 747 | | _heightPosUnmarked = startPosition.Y; |
| | | 748 | | |
| | 5082 | 749 | | _rotationChangedBuffer = true; |
| | 5082 | 750 | | _rotation = normalizedRotation; |
| | | 751 | | |
| | 5082 | 752 | | if (!IsKinematic) |
| | | 753 | | { |
| | 4670 | 754 | | _lastVisualPosition = _visualPosition = Position3d; |
| | 4670 | 755 | | _visualRotation = normalizedRotation; |
| | 4670 | 756 | | _lastVisualRotation = _visualRotation; |
| | | 757 | | } |
| | | 758 | | |
| | 5082 | 759 | | OnVisualize(); |
| | | 760 | | |
| | 5082 | 761 | | _dynamicId = Context.Physics.AssimilateBody(this, motionType); |
| | 5082 | 762 | | Collider!.Initialize(this); |
| | 5082 | 763 | | RefreshMassPropertiesFromColliderShape(); |
| | 5081 | 764 | | CheckGround(force: true); |
| | 5081 | 765 | | } |
| | | 766 | | |
| | | 767 | | |
| | | 768 | | /// <summary>Completes deferred body integration and post-step bookkeeping.</summary> |
| | | 769 | | public void LateSimulate() |
| | | 770 | | { |
| | 7 | 771 | | Context.EnterSimulationPhase(); |
| | | 772 | | try |
| | | 773 | | { |
| | 7 | 774 | | LateSimulate(updateSleepState: true, updateColliderState: true); |
| | 7 | 775 | | } |
| | | 776 | | finally |
| | | 777 | | { |
| | 7 | 778 | | Context.ExitSimulationPhase(); |
| | 7 | 779 | | } |
| | 7 | 780 | | } |
| | | 781 | | |
| | | 782 | | internal void LateSimulate(bool updateSleepState, bool updateColliderState) |
| | | 783 | | { |
| | 13556 | 784 | | if (!Active) return; |
| | | 785 | | |
| | 13554 | 786 | | CaptureGroundedStepState(); |
| | | 787 | | try |
| | | 788 | | { |
| | 13554 | 789 | | LastContinuousCollisionToiIterationCount = 0; |
| | 13554 | 790 | | LastContinuousCollisionToiIterationLimitReached = false; |
| | | 791 | | |
| | 13554 | 792 | | _lastPosition = Position3d; |
| | 13554 | 793 | | if (TryConsumeContinuousCollisionHandoff(updateSleepState, updateColliderState)) |
| | 62 | 794 | | return; |
| | | 795 | | |
| | 13492 | 796 | | if (IsKinematic) |
| | 6337 | 797 | | UpdateKinematicPositionAndRotation(); |
| | | 798 | | |
| | 13492 | 799 | | if (HasSolverMobility) |
| | | 800 | | { |
| | 7148 | 801 | | if (!IsSleeping) |
| | | 802 | | { |
| | 6370 | 803 | | ProcessMovable(); |
| | 6369 | 804 | | if (updateSleepState) |
| | 4 | 805 | | UpdateSleepState(); |
| | | 806 | | } |
| | | 807 | | |
| | 7147 | 808 | | if (updateColliderState) |
| | 8 | 809 | | Collider!.Simulate(); |
| | | 810 | | } |
| | | 811 | | |
| | 13491 | 812 | | if (SettingVisuals) |
| | 6130 | 813 | | _settingVisualsCounter--; |
| | | 814 | | |
| | 13491 | 815 | | if (PositionChangePending || RotationChangePending) |
| | 12971 | 816 | | OnMoved?.Invoke(); |
| | 536 | 817 | | } |
| | | 818 | | finally |
| | | 819 | | { |
| | 13554 | 820 | | CompleteGroundedStepState(); |
| | 13554 | 821 | | } |
| | 13553 | 822 | | } |
| | | 823 | | |
| | | 824 | | internal void UpdateSleepStateAfterPhysicsStep() |
| | | 825 | | { |
| | 13501 | 826 | | if (!IsSleeping) |
| | 12787 | 827 | | UpdateSleepState(); |
| | 13501 | 828 | | } |
| | | 829 | | |
| | | 830 | | private void UpdateKinematicPositionAndRotation() |
| | | 831 | | { |
| | 6337 | 832 | | EnsureContinuousCollisionFramePrepared(Context.LateSimulateToken); |
| | 6337 | 833 | | Vector3d startPosition = Position3d; |
| | 6337 | 834 | | FixedQuaternion startRotation = Rotation; |
| | 6337 | 835 | | Vector3d requestedPosition = _positionTransform.WorldPosition; |
| | 6337 | 836 | | FixedQuaternion requestedRotation = _rotationTransform.WorldRotation; |
| | 6337 | 837 | | Vector3d kinematicPosition = ContinuousCollisionFrameEnd; |
| | 6337 | 838 | | FixedQuaternion kinematicRotation = ContinuousCollisionFrameTargetRotation; |
| | 6337 | 839 | | if (startPosition == kinematicPosition && startRotation == kinematicRotation) |
| | | 840 | | { |
| | 6 | 841 | | SetPositionTransformWorldPosition(kinematicPosition); |
| | 6 | 842 | | SetRotationTransformWorldRotation(kinematicRotation); |
| | 6 | 843 | | return; |
| | | 844 | | } |
| | | 845 | | |
| | 6331 | 846 | | Wake(); |
| | | 847 | | |
| | 6331 | 848 | | Vector3d resolvedPosition = ProjectLinearEndpoint(startPosition, kinematicPosition); |
| | 6331 | 849 | | if (ShouldUseContinuousCollision(out _)) |
| | 6318 | 850 | | _ = ContinuousCollisionSweepRange.ValidateEndpoint(startPosition, resolvedPosition, out _); |
| | 6331 | 851 | | FixedQuaternion resolvedRotation = kinematicRotation; |
| | 6331 | 852 | | if (!TryResolveKinematicRotationalContinuousCollision( |
| | 6331 | 853 | | startPosition, |
| | 6331 | 854 | | ref resolvedPosition, |
| | 6331 | 855 | | startRotation, |
| | 6331 | 856 | | ref resolvedRotation)) |
| | | 857 | | { |
| | 6221 | 858 | | TryResolveKinematicContinuousCollision(startPosition, ref resolvedPosition); |
| | | 859 | | } |
| | | 860 | | |
| | 6331 | 861 | | if (resolvedPosition != requestedPosition) |
| | 6182 | 862 | | SetPositionTransformWorldPosition(resolvedPosition); |
| | 6331 | 863 | | if (resolvedRotation != requestedRotation) |
| | 23 | 864 | | SetRotationTransformWorldRotation(resolvedRotation); |
| | | 865 | | |
| | 6331 | 866 | | SetPosition2d(resolvedPosition.ToVector2d()); |
| | 6331 | 867 | | HeightPos = resolvedPosition.Y; |
| | 6331 | 868 | | SetVisualPosition(resolvedPosition); |
| | | 869 | | |
| | 6331 | 870 | | Rotation = resolvedRotation; |
| | 6331 | 871 | | StoreVisualRotation(resolvedRotation); |
| | 6331 | 872 | | } |
| | | 873 | | |
| | | 874 | | |
| | | 875 | | /// <summary> |
| | | 876 | | /// Puts the body to sleep and keeps it partitioned for queries and deterministic wake propagation. |
| | | 877 | | /// </summary> |
| | | 878 | | public void Sleep() |
| | | 879 | | { |
| | 234 | 880 | | if (!CanSleep) |
| | 6 | 881 | | return; |
| | | 882 | | |
| | 228 | 883 | | _sleepFrameCount = _sleepFrameThreshold; |
| | 228 | 884 | | ClearMotionForSleep(); |
| | 228 | 885 | | if (_isSleeping) |
| | 1 | 886 | | return; |
| | | 887 | | |
| | 227 | 888 | | _isSleeping = true; |
| | 227 | 889 | | RefreshPartitionAwakeState(); |
| | 227 | 890 | | } |
| | | 891 | | |
| | | 892 | | /// <summary> |
| | | 893 | | /// Wakes a sleeping body because a deterministic simulation or host stimulus changed its state. |
| | | 894 | | /// </summary> |
| | | 895 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 896 | | public void Wake() |
| | | 897 | | { |
| | 7448 | 898 | | _sleepFrameCount = 0; |
| | 7448 | 899 | | if (!_isSleeping) |
| | 7430 | 900 | | return; |
| | | 901 | | |
| | 18 | 902 | | _isSleeping = false; |
| | 18 | 903 | | RefreshPartitionAwakeState(); |
| | 18 | 904 | | } |
| | | 905 | | |
| | | 906 | | internal void WakeFromCollision() |
| | | 907 | | { |
| | 481092 | 908 | | if (!_isSleeping) |
| | 480974 | 909 | | return; |
| | | 910 | | |
| | 118 | 911 | | _sleepFrameCount = 0; |
| | 118 | 912 | | _isSleeping = false; |
| | 118 | 913 | | RefreshPartitionAwakeState(); |
| | 118 | 914 | | } |
| | | 915 | | |
| | 13026 | 916 | | private bool CanSleep => SleepEnabled && HasSolverMobility; |
| | | 917 | | |
| | | 918 | | |
| | | 919 | | private void SetTransformWorldPose(Vector3d position, FixedQuaternion rotation) |
| | | 920 | | { |
| | 6484 | 921 | | Vector3d originalLocalPosition = _positionTransform.LocalPosition; |
| | 6484 | 922 | | FixedQuaternion originalLocalRotation = _positionTransform.LocalRotation; |
| | 6484 | 923 | | SwiftThrowHelper.ThrowIfTrue( |
| | 6484 | 924 | | !_positionTransform.TrySetWorldPose(position, rotation), |
| | 6484 | 925 | | nameof(FixedTransform), |
| | 6484 | 926 | | "Host transform cannot represent the requested world pose."); |
| | | 927 | | |
| | | 928 | | try |
| | | 929 | | { |
| | 6483 | 930 | | if (Active) |
| | | 931 | | { |
| | 6482 | 932 | | SwiftThrowHelper.ThrowIfTrue( |
| | 6482 | 933 | | !Collider.TryPrepareBodyPose(position, rotation), |
| | 6482 | 934 | | nameof(position), |
| | 6482 | 935 | | "The requested body pose produces collider geometry outside the representable coordinate domain."); |
| | | 936 | | } |
| | 6479 | 937 | | } |
| | 4 | 938 | | catch |
| | | 939 | | { |
| | 4 | 940 | | _positionTransform.LocalPosition = originalLocalPosition; |
| | 4 | 941 | | _positionTransform.LocalRotation = originalLocalRotation; |
| | 4 | 942 | | throw; |
| | | 943 | | } |
| | 6479 | 944 | | } |
| | | 945 | | |
| | | 946 | | /// <summary>Releases the body's collider and context-local registration.</summary> |
| | | 947 | | public void Deactivate() |
| | | 948 | | { |
| | 103 | 949 | | if (!ReferenceEquals(Collider.Body, this)) |
| | 11 | 950 | | return; |
| | | 951 | | |
| | 92 | 952 | | DiscardContinuousCollisionHandoff(); |
| | 92 | 953 | | InvalidateContinuousCollisionTrajectory(); |
| | 92 | 954 | | Collider.DeactivateRuntimeRegistration(); |
| | 91 | 955 | | Context.Physics.DessimilateBody(this); |
| | 91 | 956 | | _dynamicId = -1; |
| | 91 | 957 | | Active = false; |
| | 91 | 958 | | } |
| | | 959 | | |
| | | 960 | | /// <summary>Sets an authoritative target rotation and visual interpolation rate.</summary> |
| | | 961 | | public void UpdateRotation(FixedQuaternion targetRotation, Fixed64 bufferInterpolation) |
| | | 962 | | { |
| | 9 | 963 | | FixedQuaternion normalizedRotation = targetRotation.Normalized; |
| | 9 | 964 | | PreflightStaticPoseChange(); |
| | 8 | 965 | | bool preparedPose = PrepareExplicitBodyPose( |
| | 8 | 966 | | Position3d, |
| | 8 | 967 | | normalizedRotation, |
| | 8 | 968 | | nameof(targetRotation), |
| | 8 | 969 | | "The requested target rotation produces collider geometry outside the representable coordinate domain."); |
| | 8 | 970 | | _rotationInterpoleSpeed = bufferInterpolation; |
| | 8 | 971 | | _rotationSpeed = Agent.IsInteracting |
| | 8 | 972 | | ? InteractionRotationSpeed |
| | 8 | 973 | | : DefaultRotationSpeed; |
| | 8 | 974 | | Rotation = normalizedRotation; |
| | 8 | 975 | | PublishExplicitBodyPose(preparedPose); |
| | 8 | 976 | | } |
| | | 977 | | |
| | | 978 | | /// <summary> |
| | | 979 | | /// Gets a world-space point from the authoritative body pose and committed collider scale. |
| | | 980 | | /// </summary> |
| | | 981 | | /// <param name="point">The body-local point.</param> |
| | | 982 | | /// <returns>The corresponding world-space point.</returns> |
| | | 983 | | /// <exception cref="InvalidOperationException"> |
| | | 984 | | /// The collider has no committed scale or the final world-space point is not representable. |
| | | 985 | | /// </exception> |
| | | 986 | | public Vector3d GetWorldPoint(Vector3d point) |
| | | 987 | | { |
| | 7 | 988 | | bool transformed = TryGetWorldPoint(point, out Vector3d result); |
| | 7 | 989 | | SwiftThrowHelper.ThrowIfTrue( |
| | 7 | 990 | | !transformed, |
| | 7 | 991 | | nameof(Collider), |
| | 7 | 992 | | "Cannot get the world point because the collider has no committed scale or the final coordinate is not repre |
| | 5 | 993 | | return result; |
| | | 994 | | } |
| | | 995 | | |
| | | 996 | | /// <summary> |
| | | 997 | | /// Attempts to get a world-space point from the authoritative body pose and committed collider scale. |
| | | 998 | | /// </summary> |
| | | 999 | | /// <param name="point">The body-local point.</param> |
| | | 1000 | | /// <param name="result">The world-space point on success; otherwise zero.</param> |
| | | 1001 | | /// <returns><see langword="true"/> when a committed scale exists and the final point is representable.</returns> |
| | | 1002 | | public bool TryGetWorldPoint(Vector3d point, out Vector3d result) |
| | | 1003 | | { |
| | 221 | 1004 | | if (!Collider.TryGetCommittedOwnerScale(out Vector3d scale)) |
| | | 1005 | | { |
| | 2 | 1006 | | result = Vector3d.Zero; |
| | 2 | 1007 | | return false; |
| | | 1008 | | } |
| | | 1009 | | |
| | 219 | 1010 | | return Rotation.TryTransformScaledPoint(Position3d, point, scale, out result); |
| | | 1011 | | } |
| | | 1012 | | |
| | | 1013 | | /// <summary> |
| | | 1014 | | /// Gets a body-local point from the authoritative body pose and committed collider scale. |
| | | 1015 | | /// </summary> |
| | | 1016 | | /// <param name="point">The world-space point.</param> |
| | | 1017 | | /// <returns>The corresponding body-local point.</returns> |
| | | 1018 | | /// <exception cref="InvalidOperationException"> |
| | | 1019 | | /// The collider has no committed scale, its scale is singular, or the final body-local point is not representable. |
| | | 1020 | | /// </exception> |
| | | 1021 | | public Vector3d GetLocalPoint(Vector3d point) |
| | | 1022 | | { |
| | 5 | 1023 | | bool transformed = TryGetLocalPoint(point, out Vector3d result); |
| | 5 | 1024 | | SwiftThrowHelper.ThrowIfTrue( |
| | 5 | 1025 | | !transformed, |
| | 5 | 1026 | | nameof(Collider), |
| | 5 | 1027 | | "Cannot get the local point because the collider has no committed scale, its scale is singular, or the final |
| | 3 | 1028 | | return result; |
| | | 1029 | | } |
| | | 1030 | | |
| | | 1031 | | /// <summary> |
| | | 1032 | | /// Attempts to get a body-local point from the authoritative body pose and committed collider scale. |
| | | 1033 | | /// </summary> |
| | | 1034 | | /// <param name="point">The world-space point.</param> |
| | | 1035 | | /// <param name="result">The body-local point on success; otherwise zero.</param> |
| | | 1036 | | /// <returns> |
| | | 1037 | | /// <see langword="true"/> when a committed nonsingular scale exists and the final point is representable. |
| | | 1038 | | /// </returns> |
| | | 1039 | | public bool TryGetLocalPoint(Vector3d point, out Vector3d result) |
| | | 1040 | | { |
| | 219 | 1041 | | if (!Collider.TryGetCommittedOwnerScale(out Vector3d scale)) |
| | | 1042 | | { |
| | 2 | 1043 | | result = Vector3d.Zero; |
| | 2 | 1044 | | return false; |
| | | 1045 | | } |
| | | 1046 | | |
| | 217 | 1047 | | return Rotation.TryInverseTransformScaledPoint(Position3d, point, scale, out result); |
| | | 1048 | | } |
| | | 1049 | | |
| | | 1050 | | /// <summary>Resets the authoritative pose, motion, grounding, and presentation state.</summary> |
| | | 1051 | | public void ResetPosition(Vector3d position = default, FixedQuaternion rotation = default) |
| | | 1052 | | { |
| | 6486 | 1053 | | FixedQuaternion normalizedRotation = rotation.Normalized; |
| | 6486 | 1054 | | PreflightResetPoseChange(); |
| | 6484 | 1055 | | SetTransformWorldPose(position, normalizedRotation); |
| | 6479 | 1056 | | InvalidateContinuousCollisionTrajectory(); |
| | 6479 | 1057 | | ClearMotionForSleep(); |
| | 6479 | 1058 | | bool wasSleeping = _isSleeping; |
| | 6479 | 1059 | | _isSleeping = false; |
| | 6479 | 1060 | | _sleepFrameCount = 0; |
| | 6479 | 1061 | | _normalForce = Vector3d.Zero; |
| | | 1062 | | |
| | 6479 | 1063 | | SetPosition2d(position.ToVector2d()); |
| | 6479 | 1064 | | HeightPos = position.Y; |
| | 6479 | 1065 | | _lastPosition = position; |
| | 6479 | 1066 | | _lastVisualPosition = _visualPosition = position; |
| | 6479 | 1067 | | Rotation = normalizedRotation; |
| | | 1068 | | |
| | 6479 | 1069 | | _visualRotation = normalizedRotation; |
| | | 1070 | | |
| | 6479 | 1071 | | PublishExplicitBodyPose(Active); |
| | 6479 | 1072 | | if (wasSleeping) |
| | 27 | 1073 | | RefreshPartitionAwakeState(); |
| | 6479 | 1074 | | } |
| | | 1075 | | |
| | | 1076 | | } |