| | | 1 | | //======================================================================= |
| | | 2 | | // SolverContact.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 | | using Gravitas.Materials; |
| | | 10 | | |
| | | 11 | | namespace Gravitas.CollisionHandling; |
| | | 12 | | |
| | | 13 | | internal readonly struct ContactLever3D |
| | | 14 | | { |
| | | 15 | | private ContactLever3D(Vector3d vector, bool isExact) |
| | | 16 | | { |
| | 18286 | 17 | | Vector = vector; |
| | 18286 | 18 | | IsExact = isExact; |
| | 18286 | 19 | | } |
| | | 20 | | |
| | | 21 | | internal bool IsExact { get; } |
| | | 22 | | |
| | | 23 | | internal Vector3d Vector { get; } |
| | | 24 | | |
| | | 25 | | internal static ContactLever3D Create( |
| | | 26 | | in ContactAnchor point, |
| | | 27 | | in ContactAnchor origin) |
| | | 28 | | { |
| | 18286 | 29 | | if (point.TryGetOffsetFrom(origin, out Vector3d vector)) |
| | 18246 | 30 | | return new ContactLever3D(vector, isExact: false); |
| | | 31 | | |
| | 40 | 32 | | return new ContactLever3D(default, isExact: true); |
| | | 33 | | } |
| | | 34 | | } |
| | | 35 | | |
| | | 36 | | /// <summary> |
| | | 37 | | /// Solver-ready 3D contact data, including the deterministic tangent frame and cached impulses. |
| | | 38 | | /// </summary> |
| | | 39 | | internal readonly struct SolverContact |
| | | 40 | | { |
| | | 41 | | public SolverContact( |
| | | 42 | | int manifoldIndex, |
| | | 43 | | ulong contactId, |
| | | 44 | | ResponseBody bodyA, |
| | | 45 | | ResponseBody bodyB, |
| | | 46 | | ContactLever3D relativeA, |
| | | 47 | | ContactLever3D relativeB, |
| | | 48 | | Fixed64 depth, |
| | | 49 | | Vector3d normal, |
| | | 50 | | PhysicsMaterial materialA, |
| | | 51 | | PhysicsMaterial materialB, |
| | | 52 | | Fixed64 cachedNormalImpulse, |
| | | 53 | | Fixed64 cachedTangentImpulse, |
| | | 54 | | Fixed64 cachedSecondaryTangentImpulse) |
| | | 55 | | { |
| | | 56 | | ManifoldIndex = manifoldIndex; |
| | | 57 | | ContactId = contactId; |
| | | 58 | | A = bodyA; |
| | | 59 | | B = bodyB; |
| | | 60 | | RelativeA = relativeA; |
| | | 61 | | RelativeB = relativeB; |
| | | 62 | | Depth = depth; |
| | | 63 | | Normal = normal; |
| | | 64 | | MaterialA = materialA; |
| | | 65 | | MaterialB = materialB; |
| | | 66 | | PhysicsMaterial.CombineFriction(materialA, materialB, out Fixed64 staticFriction, out Fixed64 dynamicFriction); |
| | | 67 | | StaticFriction = staticFriction; |
| | | 68 | | DynamicFriction = dynamicFriction; |
| | | 69 | | Restitution = PhysicsMaterial.CombineRestitution(materialA, materialB); |
| | | 70 | | Tangent = CreateTangent(normal); |
| | | 71 | | SecondaryTangent = Vector3d.Cross(normal, Tangent).Normalized; |
| | | 72 | | CachedNormalImpulse = cachedNormalImpulse; |
| | | 73 | | CachedTangentImpulse = cachedTangentImpulse; |
| | | 74 | | CachedSecondaryTangentImpulse = cachedSecondaryTangentImpulse; |
| | | 75 | | } |
| | | 76 | | |
| | | 77 | | public int ManifoldIndex { get; } |
| | | 78 | | |
| | | 79 | | public ulong ContactId { get; } |
| | | 80 | | |
| | | 81 | | public ResponseBody A { get; } |
| | | 82 | | |
| | | 83 | | public ResponseBody B { get; } |
| | | 84 | | |
| | | 85 | | public ContactLever3D RelativeA { get; } |
| | | 86 | | |
| | | 87 | | public ContactLever3D RelativeB { get; } |
| | | 88 | | |
| | | 89 | | public Fixed64 Depth { get; } |
| | | 90 | | |
| | | 91 | | public Vector3d Normal { get; } |
| | | 92 | | |
| | | 93 | | public PhysicsMaterial MaterialA { get; } |
| | | 94 | | |
| | | 95 | | public PhysicsMaterial MaterialB { get; } |
| | | 96 | | |
| | | 97 | | public Fixed64 StaticFriction { get; } |
| | | 98 | | |
| | | 99 | | public Fixed64 DynamicFriction { get; } |
| | | 100 | | |
| | | 101 | | public Fixed64 Restitution { get; } |
| | | 102 | | |
| | | 103 | | public Vector3d Tangent { get; } |
| | | 104 | | |
| | | 105 | | public Vector3d SecondaryTangent { get; } |
| | | 106 | | |
| | | 107 | | public Fixed64 CachedNormalImpulse { get; } |
| | | 108 | | |
| | | 109 | | public Fixed64 CachedTangentImpulse { get; } |
| | | 110 | | |
| | | 111 | | public Fixed64 CachedSecondaryTangentImpulse { get; } |
| | | 112 | | |
| | | 113 | | internal static Vector3d CreateTangent(Vector3d normal) |
| | | 114 | | { |
| | | 115 | | Vector3d absolute = Vector3d.Abs(normal); |
| | | 116 | | Vector3d reference = absolute.X <= absolute.Y && absolute.X <= absolute.Z |
| | | 117 | | ? Vector3d.Right |
| | | 118 | | : absolute.Y <= absolute.Z |
| | | 119 | | ? Vector3d.Up |
| | | 120 | | : Vector3d.Forward; |
| | | 121 | | |
| | | 122 | | return Vector3d.Cross(reference, normal).Normalized; |
| | | 123 | | } |
| | | 124 | | } |