| | | 1 | | //======================================================================= |
| | | 2 | | // ContinuousCollisionMotionSegment3D.cs |
| | | 3 | | //======================================================================= |
| | | 4 | | // MIT License, Copyright (c) 2026-present David Oravsky (mrdav30) |
| | | 5 | | // See LICENSE file in the project root for full license information. |
| | | 6 | | //======================================================================= |
| | | 7 | | |
| | | 8 | | using FixedMathSharp; |
| | | 9 | | |
| | | 10 | | namespace Gravitas.CollisionHandling; |
| | | 11 | | |
| | | 12 | | internal enum ContinuousCollisionRotationPath3D : byte |
| | | 13 | | { |
| | | 14 | | IntegratedAngularVelocity, |
| | | 15 | | KinematicSlerp |
| | | 16 | | } |
| | | 17 | | |
| | | 18 | | /// <summary> |
| | | 19 | | /// One immutable, normalized-time piece of a 3D body's prepared CCD trajectory. |
| | | 20 | | /// </summary> |
| | | 21 | | internal readonly struct ContinuousCollisionMotionSegment3D |
| | | 22 | | { |
| | | 23 | | public ContinuousCollisionMotionSegment3D( |
| | | 24 | | Fixed64 startFraction, |
| | | 25 | | Fixed64 endFraction, |
| | | 26 | | Vector3d startPosition, |
| | | 27 | | Vector3d endPosition, |
| | | 28 | | Vector3d displacement, |
| | | 29 | | FixedQuaternion startRotation, |
| | | 30 | | Vector3d angularVelocity, |
| | | 31 | | FixedQuaternion targetRotation, |
| | | 32 | | Fixed64 angularDistance, |
| | | 33 | | ContinuousCollisionRotationPath3D rotationPath) |
| | | 34 | | { |
| | 14564 | 35 | | StartFraction = startFraction; |
| | 14564 | 36 | | EndFraction = endFraction; |
| | 14564 | 37 | | StartPosition = startPosition; |
| | 14564 | 38 | | EndPosition = endPosition; |
| | 14564 | 39 | | Displacement = displacement; |
| | 14564 | 40 | | StartRotation = startRotation; |
| | 14564 | 41 | | AngularVelocity = angularVelocity; |
| | 14564 | 42 | | TargetRotation = targetRotation; |
| | 14564 | 43 | | AngularDistance = angularDistance; |
| | 14564 | 44 | | RotationPath = rotationPath; |
| | 14564 | 45 | | } |
| | | 46 | | |
| | | 47 | | public Fixed64 StartFraction { get; } |
| | | 48 | | public Fixed64 EndFraction { get; } |
| | | 49 | | public Vector3d StartPosition { get; } |
| | | 50 | | public Vector3d EndPosition { get; } |
| | | 51 | | public Vector3d Displacement { get; } |
| | | 52 | | public FixedQuaternion StartRotation { get; } |
| | | 53 | | public Vector3d AngularVelocity { get; } |
| | | 54 | | public FixedQuaternion TargetRotation { get; } |
| | | 55 | | public Fixed64 AngularDistance { get; } |
| | | 56 | | public ContinuousCollisionRotationPath3D RotationPath { get; } |
| | | 57 | | |
| | | 58 | | public Vector3d SamplePosition(Fixed64 frameFraction) |
| | | 59 | | { |
| | 4519 | 60 | | Fixed64 localFraction = ResolveLocalFraction(frameFraction); |
| | 4519 | 61 | | return Vector3d.Lerp(StartPosition, EndPosition, localFraction); |
| | | 62 | | } |
| | | 63 | | |
| | | 64 | | public FixedQuaternion SampleRotation(Fixed64 frameFraction, Fixed64 frameDeltaTime) |
| | | 65 | | { |
| | 4512 | 66 | | Fixed64 localFraction = ResolveLocalFraction(frameFraction); |
| | 4512 | 67 | | if (RotationPath == ContinuousCollisionRotationPath3D.KinematicSlerp) |
| | 371 | 68 | | return FixedQuaternion.Slerp(StartRotation, TargetRotation, localFraction).Normalized; |
| | | 69 | | |
| | 4141 | 70 | | Fixed64 elapsedTime = frameDeltaTime * (frameFraction - StartFraction); |
| | 4141 | 71 | | FixedQuaternion angularVelocityQuaternion = new( |
| | 4141 | 72 | | AngularVelocity.X, |
| | 4141 | 73 | | AngularVelocity.Y, |
| | 4141 | 74 | | AngularVelocity.Z, |
| | 4141 | 75 | | Fixed64.Zero); |
| | 4141 | 76 | | FixedQuaternion spin = angularVelocityQuaternion * StartRotation * Fixed64.Half * elapsedTime; |
| | 4141 | 77 | | return (StartRotation + spin).Normalized; |
| | | 78 | | } |
| | | 79 | | |
| | | 80 | | public bool TryResolveMotionBound( |
| | | 81 | | Fixed64 lowerFraction, |
| | | 82 | | Fixed64 upperFraction, |
| | | 83 | | Fixed64 pivotRadius, |
| | | 84 | | out Fixed64 motionBound) |
| | | 85 | | { |
| | 3960 | 86 | | Fixed64 segmentSpan = EndFraction - StartFraction; |
| | 3960 | 87 | | Fixed64 localSpan = (upperFraction - lowerFraction) / segmentSpan; |
| | 3960 | 88 | | if (!Vector3d.TrySubtract(EndPosition, StartPosition, out Vector3d exactDisplacement)) |
| | | 89 | | { |
| | 1 | 90 | | motionBound = default; |
| | 1 | 91 | | return false; |
| | | 92 | | } |
| | | 93 | | |
| | 3959 | 94 | | return ContinuousCollisionMath.TryResolveRotationalIntervalMotionBound( |
| | 3959 | 95 | | exactDisplacement, |
| | 3959 | 96 | | AngularDistance, |
| | 3959 | 97 | | pivotRadius, |
| | 3959 | 98 | | localSpan, |
| | 3959 | 99 | | out motionBound); |
| | | 100 | | } |
| | | 101 | | |
| | | 102 | | private Fixed64 ResolveLocalFraction(Fixed64 frameFraction) |
| | | 103 | | { |
| | 9031 | 104 | | Fixed64 span = EndFraction - StartFraction; |
| | 9031 | 105 | | return FixedMath.Clamp01((frameFraction - StartFraction) / span); |
| | | 106 | | } |
| | | 107 | | } |