| | | 1 | | //======================================================================= |
| | | 2 | | // SolverContact3D.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 | | |
| | | 14 | | |
| | | 15 | | /// <summary> |
| | | 16 | | /// Solver-ready 3D contact data, including the deterministic tangent frame and cached impulses. |
| | | 17 | | /// </summary> |
| | | 18 | | internal readonly struct SolverContact3D |
| | | 19 | | { |
| | | 20 | | public SolverContact3D( |
| | | 21 | | int groupIndex, |
| | | 22 | | int pointIndex, |
| | | 23 | | ulong contactId, |
| | | 24 | | ResponseBody3D bodyA, |
| | | 25 | | ResponseBody3D bodyB, |
| | | 26 | | ContactLever3D relativeA, |
| | | 27 | | ContactLever3D relativeB, |
| | | 28 | | Fixed64 depth, |
| | | 29 | | Vector3d normal, |
| | | 30 | | PhysicsMaterial materialA, |
| | | 31 | | PhysicsMaterial materialB, |
| | | 32 | | Fixed64 cachedNormalImpulse, |
| | | 33 | | Fixed64 cachedTangentImpulse, |
| | | 34 | | Fixed64 cachedSecondaryTangentImpulse) |
| | | 35 | | { |
| | 714432 | 36 | | GroupIndex = groupIndex; |
| | 714432 | 37 | | PointIndex = pointIndex; |
| | 714432 | 38 | | ContactId = contactId; |
| | 714432 | 39 | | A = bodyA; |
| | 714432 | 40 | | B = bodyB; |
| | 714432 | 41 | | RelativeA = relativeA; |
| | 714432 | 42 | | RelativeB = relativeB; |
| | 714432 | 43 | | Depth = depth; |
| | 714432 | 44 | | Normal = normal; |
| | 714432 | 45 | | MaterialA = materialA; |
| | 714432 | 46 | | MaterialB = materialB; |
| | 714432 | 47 | | PhysicsMaterial.CombineFriction(materialA, materialB, out Fixed64 staticFriction, out Fixed64 dynamicFriction); |
| | 714432 | 48 | | StaticFriction = staticFriction; |
| | 714432 | 49 | | DynamicFriction = dynamicFriction; |
| | 714432 | 50 | | Restitution = PhysicsMaterial.CombineRestitution(materialA, materialB); |
| | 714432 | 51 | | Tangent = CreateTangent(normal); |
| | 714432 | 52 | | SecondaryTangent = Vector3d.Cross(normal, Tangent).Normalized; |
| | 714432 | 53 | | CachedNormalImpulse = cachedNormalImpulse; |
| | 714432 | 54 | | CachedTangentImpulse = cachedTangentImpulse; |
| | 714432 | 55 | | CachedSecondaryTangentImpulse = cachedSecondaryTangentImpulse; |
| | 714432 | 56 | | } |
| | | 57 | | |
| | | 58 | | internal int GroupIndex { get; } |
| | | 59 | | |
| | | 60 | | internal int PointIndex { get; } |
| | | 61 | | |
| | | 62 | | public ulong ContactId { get; } |
| | | 63 | | |
| | | 64 | | public ResponseBody3D A { get; } |
| | | 65 | | |
| | | 66 | | public ResponseBody3D B { get; } |
| | | 67 | | |
| | | 68 | | public ContactLever3D RelativeA { get; } |
| | | 69 | | |
| | | 70 | | public ContactLever3D RelativeB { get; } |
| | | 71 | | |
| | | 72 | | public Fixed64 Depth { get; } |
| | | 73 | | |
| | | 74 | | public Vector3d Normal { get; } |
| | | 75 | | |
| | | 76 | | public PhysicsMaterial MaterialA { get; } |
| | | 77 | | |
| | | 78 | | public PhysicsMaterial MaterialB { get; } |
| | | 79 | | |
| | | 80 | | public Fixed64 StaticFriction { get; } |
| | | 81 | | |
| | | 82 | | public Fixed64 DynamicFriction { get; } |
| | | 83 | | |
| | | 84 | | public Fixed64 Restitution { get; } |
| | | 85 | | |
| | | 86 | | public Vector3d Tangent { get; } |
| | | 87 | | |
| | | 88 | | public Vector3d SecondaryTangent { get; } |
| | | 89 | | |
| | | 90 | | public Fixed64 CachedNormalImpulse { get; } |
| | | 91 | | |
| | | 92 | | public Fixed64 CachedTangentImpulse { get; } |
| | | 93 | | |
| | | 94 | | public Fixed64 CachedSecondaryTangentImpulse { get; } |
| | | 95 | | |
| | | 96 | | internal static Vector3d CreateTangent(Vector3d normal) |
| | | 97 | | { |
| | 714712 | 98 | | Vector3d absolute = Vector3d.Abs(normal); |
| | 714712 | 99 | | Vector3d reference = absolute.X <= absolute.Y && absolute.X <= absolute.Z |
| | 714712 | 100 | | ? Vector3d.Right |
| | 714712 | 101 | | : absolute.Y <= absolute.Z |
| | 714712 | 102 | | ? Vector3d.Up |
| | 714712 | 103 | | : Vector3d.Forward; |
| | | 104 | | |
| | 714712 | 105 | | return Vector3d.Cross(reference, normal).Normalized; |
| | | 106 | | } |
| | | 107 | | } |