| | | 1 | | //======================================================================= |
| | | 2 | | // CollisionResponse.SupportedIsland.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 System; |
| | | 9 | | using FixedMathSharp; |
| | | 10 | | using Gravitas.Colliders; |
| | | 11 | | |
| | | 12 | | namespace Gravitas.CollisionHandling; |
| | | 13 | | |
| | | 14 | | /// <content>Seeds coupled normal loads for cuboid support.</content> |
| | | 15 | | public static partial class CollisionResponse |
| | | 16 | | { |
| | | 17 | | private static bool TrySeedFlatSupportNormals(CollisionPair pair) |
| | | 18 | | { |
| | 38736 | 19 | | if (!IsFlatCuboidPatch(pair)) |
| | 38449 | 20 | | return false; |
| | | 21 | | |
| | 574 | 22 | | SolidBody? a = pair.ColliderA.Body, b = pair.ColliderB.Body; |
| | 287 | 23 | | bool firstIsSupport = IsFixedIslandSupport(a); |
| | 287 | 24 | | SolidBody? body = firstIsSupport ? b : IsFixedIslandSupport(b) ? a : null; |
| | 287 | 25 | | if (body == null || !HasUnconstrainedIslandMobility(body)) |
| | 264 | 26 | | return false; |
| | | 27 | | |
| | 23 | 28 | | Vector3d normal = pair.Manifold.GetGroup(0)[0].Normal; |
| | 23 | 29 | | Vector3d outward = firstIsSupport ? normal : -normal; |
| | 23 | 30 | | if (!HasOnlyNormalMotion(body, outward) |
| | 23 | 31 | | | !Vector3d.TryDot(body.LinearVelocity, outward, out Fixed64 approach) | approach >= Fixed64.Zero) |
| | 8 | 32 | | return false; |
| | | 33 | | |
| | 15 | 34 | | Span<IslandNormal> rows = stackalloc IslandNormal[ContactManifold.MaxContactsPerGroup]; |
| | 15 | 35 | | if (!TryReadIslandNormals(pair, body, null, rows) || !HasFlatNormalRows(rows, normal)) |
| | 4 | 36 | | return false; |
| | | 37 | | |
| | 11 | 38 | | Span<Fixed64> speeds = stackalloc Fixed64[ContactManifold.MaxContactsPerGroup]; |
| | 11 | 39 | | Span<Fixed64> impulses = stackalloc Fixed64[ContactManifold.MaxContactsPerGroup]; |
| | 11 | 40 | | if (!TryPrepareIslandNormalVelocities(rows, body, null, speeds, impulses, requireRest: true, |
| | 11 | 41 | | out IslandMotion state, out _)) |
| | 2 | 42 | | return false; |
| | | 43 | | |
| | | 44 | | // Equal depths make the first row the ordinary correction owner. |
| | 9 | 45 | | ref readonly ContactResponseSnapshot snapshot = ref pair.ResponseSnapshot; |
| | 9 | 46 | | _ = TryCreateContact(pair, ResponseBody3D.Create(pair.ColliderA), ResponseBody3D.Create(pair.ColliderB), |
| | 9 | 47 | | snapshot.CenterA, snapshot.CenterB, 0, 0, out SolverContact3D correction); |
| | 9 | 48 | | ApplyPositionCorrection(correction, Fixed64.One); |
| | 9 | 49 | | StoreIslandNormals(pair, rows, speeds, impulses); |
| | 9 | 50 | | body.ApplyCollisionVelocityState(state.Linear, state.Angular); |
| | 9 | 51 | | return true; |
| | | 52 | | } |
| | | 53 | | |
| | 38748 | 54 | | private static bool IsFlatCuboidPatch(CollisionPair pair) => pair.ColliderA.Shape == ColliderType.AABox |
| | 38748 | 55 | | & pair.ColliderB.Shape == ColliderType.AABox & pair.Manifold.GroupCount == 1 |
| | 38748 | 56 | | & pair.Manifold.Count == ContactManifold.MaxContactsPerGroup; |
| | | 57 | | |
| | 33 | 58 | | private static bool HasOnlyNormalMotion(SolidBody body, Vector3d normal) => body.AngularVelocity == Vector3d.Zero |
| | 33 | 59 | | & Vector3d.TryCross(body.LinearVelocity, normal, out Vector3d tangent) & tangent == Vector3d.Zero; |
| | | 60 | | |
| | | 61 | | private static bool HasFlatNormalRows(ReadOnlySpan<IslandNormal> rows, Vector3d normal) |
| | | 62 | | { |
| | 213 | 63 | | foreach (IslandNormal row in rows) |
| | | 64 | | { |
| | 85 | 65 | | if (row.Normal != normal | row.Depth != rows[0].Depth |
| | 85 | 66 | | | row.Tangent != Fixed64.Zero | row.SecondaryTangent != Fixed64.Zero) |
| | 1 | 67 | | return false; |
| | | 68 | | } |
| | 21 | 69 | | return true; |
| | | 70 | | } |
| | | 71 | | |
| | | 72 | | internal static bool TrySeedSupportedIslandNormals(CollisionPair first, CollisionPair second, out bool correctedPosi |
| | | 73 | | { |
| | 1173 | 74 | | correctedPositions = false; |
| | 1173 | 75 | | if (!TryGetSupportedBodies(first, second, out SolidBody lower, out SolidBody upper) |
| | 1173 | 76 | | && !TryGetSupportedBodies(second, first, out lower, out upper)) |
| | 437 | 77 | | return false; |
| | | 78 | | |
| | 736 | 79 | | Span<IslandNormal> rows = stackalloc IslandNormal[SmallContactNormalSolver.MaxRows]; |
| | 736 | 80 | | int firstCount = first.Manifold.Count; |
| | 736 | 81 | | int count = firstCount + second.Manifold.Count; |
| | | 82 | | System.Diagnostics.Debug.Assert(count <= rows.Length); |
| | 736 | 83 | | if (!(TryReadIslandNormals(first, lower, upper, rows[..firstCount]) |
| | 736 | 84 | | & TryReadIslandNormals(second, lower, upper, rows[firstCount..count]))) |
| | 243 | 85 | | return false; |
| | | 86 | | |
| | 493 | 87 | | bool requireRest = upper.Mass <= lower.Mass; |
| | 493 | 88 | | if (requireRest) |
| | | 89 | | { |
| | 5 | 90 | | bool floorFirst = rows[0].Upper.Linear == Vector3d.Zero; |
| | 5 | 91 | | Vector3d supportAxis = (floorFirst ? rows[0] : rows[firstCount]).Lower.Linear; |
| | 5 | 92 | | Vector3d loadAxis = (floorFirst ? rows[firstCount] : rows[0]).Upper.Linear; |
| | 5 | 93 | | if (!(supportAxis == loadAxis & HasOnlyNormalMotion(lower, supportAxis) & HasOnlyNormalMotion(upper, loadAxi |
| | 5 | 94 | | & HasFlatNormalRows(rows[..firstCount], rows[0].Normal) |
| | 5 | 95 | | & HasFlatNormalRows(rows[firstCount..count], rows[firstCount].Normal))) |
| | 1 | 96 | | return false; |
| | | 97 | | } |
| | | 98 | | |
| | 492 | 99 | | Span<Fixed64> speeds = stackalloc Fixed64[SmallContactNormalSolver.MaxRows]; |
| | 492 | 100 | | Span<Fixed64> impulses = stackalloc Fixed64[SmallContactNormalSolver.MaxRows]; |
| | 492 | 101 | | if (!TryPrepareIslandNormalVelocities(rows[..count], lower, upper, speeds[..count], impulses[..count], requireRe |
| | 492 | 102 | | out IslandMotion lowerState, out IslandMotion upperState)) |
| | 4 | 103 | | return false; |
| | | 104 | | |
| | 488 | 105 | | correctedPositions = TryPrepareIslandPositions(rows[..firstCount], rows[firstCount..count], lower, upper, |
| | 488 | 106 | | out Vector3d lowerPosition, out Vector3d upperPosition); |
| | 488 | 107 | | if (requireRest & !correctedPositions) |
| | 1 | 108 | | return false; |
| | | 109 | | |
| | | 110 | | // Commit only after both endpoints and every normal impulse were prepared. |
| | | 111 | | // The subsequent row sweep refines friction against the new normal loads. |
| | 487 | 112 | | if (correctedPositions) |
| | | 113 | | { |
| | 485 | 114 | | lower.ApplyCollisionPositionState(lowerPosition); |
| | 485 | 115 | | upper.ApplyCollisionPositionState(upperPosition); |
| | | 116 | | } |
| | 487 | 117 | | StoreIslandNormals(first, rows[..firstCount], speeds[..firstCount], impulses[..firstCount]); |
| | 487 | 118 | | StoreIslandNormals(second, rows[firstCount..count], speeds[firstCount..count], impulses[firstCount..count]); |
| | 487 | 119 | | lower.ApplyCollisionVelocityState(lowerState.Linear, lowerState.Angular); |
| | 487 | 120 | | upper.ApplyCollisionVelocityState(upperState.Linear, upperState.Angular); |
| | 487 | 121 | | return true; |
| | | 122 | | } |
| | | 123 | | |
| | | 124 | | private static bool TryPrepareIslandNormalVelocities(Span<IslandNormal> rows, SolidBody lower, |
| | | 125 | | SolidBody? upper, Span<Fixed64> speeds, Span<Fixed64> impulses, bool requireRest, |
| | | 126 | | out IslandMotion lowerState, out IslandMotion upperState) |
| | | 127 | | { |
| | 503 | 128 | | int count = rows.Length; |
| | 503 | 129 | | Span<Fixed64> matrix = stackalloc Fixed64[count * count]; |
| | 503 | 130 | | lowerState = upperState = default; |
| | 503 | 131 | | var lowerMobility = new IslandMobility(lower); |
| | 503 | 132 | | var upperMobility = upper == null ? default : new IslandMobility(upper); |
| | 503 | 133 | | bool preparedSystem = TryPrepareIslandFriction(rows, lowerMobility, upperMobility, |
| | 503 | 134 | | out IslandMotion lowerFriction, out IslandMotion upperFriction); |
| | 3858 | 135 | | for (int i = 0; i < count; i++) |
| | | 136 | | { |
| | 1426 | 137 | | ref IslandNormal row = ref rows[i]; |
| | 1426 | 138 | | bool prepared = row.Lower.TryDot(lowerFriction, out Fixed64 lowerSpeed) |
| | 1426 | 139 | | & row.Upper.TryDot(upperFriction, out Fixed64 upperSpeed) |
| | 1426 | 140 | | & lowerMobility.TryTransform(row.Lower, out row.LowerResponse) |
| | 1426 | 141 | | & upperMobility.TryTransform(row.Upper, out row.UpperResponse); |
| | 1426 | 142 | | prepared &= Fixed64.TryAdd(lowerSpeed, upperSpeed, out Fixed64 warmSpeed); |
| | 1426 | 143 | | preparedSystem &= prepared & Fixed64.TryAdd(row.Speed, warmSpeed, out speeds[i]); |
| | 10392 | 144 | | for (int j = 0; j <= i; j++) |
| | | 145 | | { |
| | 3770 | 146 | | bool terms = rows[j].Lower.TryDot(row.LowerResponse, out Fixed64 low) |
| | 3770 | 147 | | & rows[j].Upper.TryDot(row.UpperResponse, out Fixed64 high); |
| | 3770 | 148 | | preparedSystem &= terms & Fixed64.TryAdd(low, high, out Fixed64 coefficient); |
| | 3770 | 149 | | matrix[i * count + j] = matrix[j * count + i] = coefficient; |
| | | 150 | | } |
| | | 151 | | } |
| | | 152 | | |
| | 503 | 153 | | if (!preparedSystem) |
| | 1 | 154 | | return false; |
| | 502 | 155 | | if (!SmallContactNormalSolver.TrySolve(count, matrix, speeds, impulses)) |
| | 2 | 156 | | return false; |
| | 500 | 157 | | Fixed64 deadband = lower.Context.Environment.MinSpeed; |
| | 500 | 158 | | if (requireRest) |
| | 14 | 159 | | deadband = FixedMath.Min(deadband, PhysicsEnvironment.DefaultMinSpeed); |
| | 500 | 160 | | bool preparedVelocities = TryPrepareIslandVelocity(lower, rows, impulses, lowerFriction, |
| | 500 | 161 | | isLower: true, deadband, out lowerState) |
| | 500 | 162 | | & (upper == null || TryPrepareIslandVelocity(upper, rows, impulses, upperFriction, |
| | 500 | 163 | | isLower: false, deadband, out upperState)); |
| | 500 | 164 | | if (requireRest && (lowerState.Linear != Vector3d.Zero | lowerState.Angular != Vector3d.Zero |
| | 500 | 165 | | | upperState.Linear != Vector3d.Zero | upperState.Angular != Vector3d.Zero)) |
| | 1 | 166 | | return false; |
| | 499 | 167 | | return preparedVelocities && ArePreparedIslandNormalsFeasible(rows, impulses, lowerState, upperState); |
| | | 168 | | } |
| | | 169 | | |
| | | 170 | | private static bool TryGetSupportedBodies(CollisionPair support, CollisionPair load, |
| | | 171 | | out SolidBody lower, out SolidBody upper) |
| | | 172 | | { |
| | 1613 | 173 | | lower = null!; |
| | 1613 | 174 | | upper = null!; |
| | 1613 | 175 | | if (support.CollisionType != CollisionType.Cuboid_Cuboid | load.CollisionType != CollisionType.Cuboid_Cuboid |
| | 1613 | 176 | | | support.Manifold.GroupCount != 1 | load.Manifold.GroupCount != 1) |
| | 854 | 177 | | return false; |
| | | 178 | | |
| | 759 | 179 | | SolidBody? a = support.ColliderA.Body; |
| | 759 | 180 | | SolidBody? b = support.ColliderB.Body; |
| | 759 | 181 | | SolidBody? supported = IsFixedIslandSupport(a) ? b : IsFixedIslandSupport(b) ? a : null; |
| | 759 | 182 | | if (supported == null || !HasUnconstrainedIslandMobility(supported)) |
| | 19 | 183 | | return false; |
| | | 184 | | // Two contacts in this connected island must share the only moving support endpoint. |
| | | 185 | | System.Diagnostics.Debug.Assert(load.ColliderA.Body == supported || load.ColliderB.Body == supported); |
| | 740 | 186 | | SolidBody? loaded = load.ColliderA.Body == supported ? load.ColliderB.Body : load.ColliderA.Body; |
| | 740 | 187 | | if (loaded == null || !HasUnconstrainedIslandMobility(loaded)) |
| | 3 | 188 | | return false; |
| | 737 | 189 | | if (loaded.Mass <= supported.Mass && !(IsFlatCuboidPatch(support) & IsFlatCuboidPatch(load))) |
| | 1 | 190 | | return false; |
| | 736 | 191 | | lower = supported; |
| | 736 | 192 | | upper = loaded; |
| | 736 | 193 | | return true; |
| | | 194 | | } |
| | | 195 | | |
| | 1261 | 196 | | private static bool IsFixedIslandSupport(SolidBody? body) => body == null || body.IsStatic && body.IsAtRest; |
| | | 197 | | |
| | 1771 | 198 | | private static bool HasUnconstrainedIslandMobility(SolidBody body) => body.IsDynamic |
| | 1771 | 199 | | && body.FreezeAxes == BodyFreezeAxes3D.None && body.CanTranslate && body.CanRotate; |
| | | 200 | | |
| | | 201 | | private static bool TryReadIslandNormals(CollisionPair pair, SolidBody lower, SolidBody? upper, Span<IslandNormal> r |
| | | 202 | | { |
| | 1487 | 203 | | if (!pair.ResponseSnapshot.IsValid) |
| | 1 | 204 | | return false; |
| | 1486 | 205 | | ResponseBody3D a = ResponseBody3D.Create(pair.ColliderA); |
| | 1486 | 206 | | ResponseBody3D b = ResponseBody3D.Create(pair.ColliderB); |
| | 1486 | 207 | | ref readonly ContactResponseSnapshot snapshot = ref pair.ResponseSnapshot; |
| | 4458 | 208 | | bool lowerIsA = a.Body == lower, upperIsA = upper != null & a.Body == upper, upperIsB = upper != null & b.Body = |
| | 5852 | 209 | | for (int i = 0; i < rows.Length; i++) |
| | | 210 | | { |
| | 1926 | 211 | | bool contactPrepared = TryCreateContact(pair, a, b, snapshot.CenterA, snapshot.CenterB, 0, i, |
| | 1926 | 212 | | out SolverContact3D contact, requireMatchingTangentBasis: true); |
| | 1926 | 213 | | if (!(contactPrepared & !contact.RelativeA.IsExact & !contact.RelativeB.IsExact & (contact.Restitution == Fi |
| | 485 | 214 | | return false; |
| | 1441 | 215 | | bool prepared = ContactNormalImpulse3D.TryComputeNormalVelocity(snapshot.LinearA, snapshot.AngularA, |
| | 1441 | 216 | | contact.RelativeA.Vector, snapshot.LinearB, snapshot.AngularB, |
| | 1441 | 217 | | contact.RelativeB.Vector, contact.Normal, out Fixed64 speed) |
| | 1441 | 218 | | & ContactResponseArithmetic3D.TryCross(contact.RelativeA.Vector, -contact.Normal, out Vector3d torqueA) |
| | 1441 | 219 | | & ContactResponseArithmetic3D.TryCross(contact.RelativeB.Vector, contact.Normal, out Vector3d torqueB); |
| | 1441 | 220 | | var first = new IslandMotion(-contact.Normal, torqueA); |
| | 1441 | 221 | | var second = new IslandMotion(contact.Normal, torqueB); |
| | 1441 | 222 | | if (!(prepared & TryReadIslandFriction(contact, out IslandMotion firstFriction, out IslandMotion secondFrict |
| | 1 | 223 | | return false; |
| | 1440 | 224 | | rows[i] = new IslandNormal |
| | 1440 | 225 | | { |
| | 1440 | 226 | | Lower = lowerIsA ? first : second, |
| | 1440 | 227 | | Upper = upperIsA ? first : upperIsB ? second : default, |
| | 1440 | 228 | | LowerFriction = lowerIsA ? firstFriction : secondFriction, |
| | 1440 | 229 | | UpperFriction = upperIsA ? firstFriction : upperIsB ? secondFriction : default, |
| | 1440 | 230 | | Tangent = contact.CachedTangentImpulse, |
| | 1440 | 231 | | SecondaryTangent = contact.CachedSecondaryTangentImpulse, |
| | 1440 | 232 | | Speed = speed, |
| | 1440 | 233 | | Normal = contact.Normal, |
| | 1440 | 234 | | Depth = contact.Depth |
| | 1440 | 235 | | }; |
| | | 236 | | } |
| | 1000 | 237 | | return true; |
| | | 238 | | } |
| | | 239 | | |
| | | 240 | | private static bool TryReadIslandFriction(SolverContact3D contact, out IslandMotion first, out IslandMotion second) |
| | | 241 | | { |
| | 1441 | 242 | | first = second = default; |
| | 1441 | 243 | | if (contact.CachedTangentImpulse == Fixed64.Zero && contact.CachedSecondaryTangentImpulse == Fixed64.Zero) |
| | 1436 | 244 | | return true; |
| | 5 | 245 | | bool prepared = ContactResponseArithmetic3D.TryLinearCombination(contact.Tangent, contact.CachedTangentImpulse, |
| | 5 | 246 | | contact.SecondaryTangent, contact.CachedSecondaryTangentImpulse, Vector3d.Zero, Fixed64.Zero, out Vector3d f |
| | 5 | 247 | | prepared &= Vector3d.TrySubtract(Vector3d.Zero, force, out Vector3d opposite); |
| | 5 | 248 | | prepared &= ContactResponseArithmetic3D.TryCross(contact.RelativeA.Vector, opposite, out Vector3d firstTorque) |
| | 5 | 249 | | & ContactResponseArithmetic3D.TryCross(contact.RelativeB.Vector, force, out Vector3d secondTorque); |
| | 5 | 250 | | first = new IslandMotion(opposite, firstTorque); |
| | 5 | 251 | | second = new IslandMotion(force, secondTorque); |
| | 5 | 252 | | return prepared; |
| | | 253 | | } |
| | | 254 | | |
| | | 255 | | private static bool TryPrepareIslandFriction(ReadOnlySpan<IslandNormal> rows, |
| | | 256 | | IslandMobility lowerMobility, IslandMobility upperMobility, out IslandMotion lower, out IslandMotion upper) |
| | | 257 | | { |
| | 503 | 258 | | lower = upper = default; |
| | 1006 | 259 | | IslandMotion lowerImpulse = default, upperImpulse = default; |
| | 3855 | 260 | | foreach (IslandNormal row in rows) |
| | | 261 | | { |
| | 1425 | 262 | | if (!(lowerImpulse.TryAdd(row.LowerFriction, out lowerImpulse) |
| | 1425 | 263 | | & upperImpulse.TryAdd(row.UpperFriction, out upperImpulse))) |
| | 1 | 264 | | return false; |
| | | 265 | | } |
| | 502 | 266 | | return lowerMobility.TryTransform(lowerImpulse, out lower) & upperMobility.TryTransform(upperImpulse, out upper) |
| | | 267 | | } |
| | | 268 | | |
| | | 269 | | private static bool TryPrepareIslandVelocity(SolidBody body, ReadOnlySpan<IslandNormal> rows, |
| | | 270 | | ReadOnlySpan<Fixed64> impulses, IslandMotion friction, bool isLower, Fixed64 deadband, out IslandMotion state) |
| | | 271 | | { |
| | 990 | 272 | | state = default; |
| | 1980 | 273 | | Vector3d linear = friction.Linear, angular = friction.Angular; |
| | 990 | 274 | | bool prepared = true; |
| | 7568 | 275 | | for (int i = 0; i < rows.Length; i++) |
| | | 276 | | { |
| | 2794 | 277 | | IslandMotion response = isLower ? rows[i].LowerResponse : rows[i].UpperResponse; |
| | 2794 | 278 | | bool scaled = ContactResponseArithmetic3D.TryScale(response.Linear, impulses[i], out Vector3d linearDelta) |
| | 2794 | 279 | | & ContactResponseArithmetic3D.TryScale(response.Angular, impulses[i], out Vector3d angularDelta); |
| | 2794 | 280 | | prepared &= scaled & Vector3d.TryAdd(linear, linearDelta, out linear) |
| | 2794 | 281 | | & Vector3d.TryAdd(angular, angularDelta, out angular); |
| | | 282 | | } |
| | 990 | 283 | | prepared &= body.TryPrepareCollisionVelocityState(linear, angular, out linear, out angular); |
| | 990 | 284 | | PhysicsEnvironment environment = body.Context.Environment; |
| | 1980 | 285 | | Fixed64 linearSpeed = linear.Magnitude, angularSpeed = angular.Magnitude; |
| | 990 | 286 | | state = new IslandMotion(linearSpeed <= deadband ? Vector3d.Zero : linear, |
| | 990 | 287 | | angularSpeed <= deadband ? Vector3d.Zero : angular); |
| | 990 | 288 | | return prepared & (linearSpeed <= environment.MaxSpeed) & (angularSpeed <= environment.MaxSpeed); |
| | | 289 | | } |
| | | 290 | | |
| | | 291 | | private static bool ArePreparedIslandNormalsFeasible(ReadOnlySpan<IslandNormal> rows, ReadOnlySpan<Fixed64> impulses |
| | | 292 | | IslandMotion lowerState, IslandMotion upperState) |
| | | 293 | | { |
| | | 294 | | // Materializing the body deltas adds rounding beyond the matrix solve. |
| | | 295 | | // Use the ordinary motion deadband, independent of a host's configured limit. |
| | 498 | 296 | | Fixed64 tolerance = PhysicsEnvironment.DefaultMinSpeed; |
| | 3816 | 297 | | for (int i = 0; i < rows.Length; i++) |
| | | 298 | | { |
| | 1411 | 299 | | bool prepared = rows[i].Lower.TryGetPreparedSpeed(lowerState, out Fixed64 lowerSpeed) |
| | 1411 | 300 | | & rows[i].Upper.TryGetPreparedSpeed(upperState, out Fixed64 upperSpeed); |
| | 1411 | 301 | | prepared &= Fixed64.TryAdd(lowerSpeed, upperSpeed, out Fixed64 speed); |
| | 1411 | 302 | | if (!(prepared & (speed >= -tolerance) & ((impulses[i] == Fixed64.Zero) | (speed <= tolerance)))) |
| | 1 | 303 | | return false; |
| | | 304 | | } |
| | 497 | 305 | | return true; |
| | | 306 | | } |
| | | 307 | | |
| | | 308 | | private static bool TryPrepareIslandPositions(ReadOnlySpan<IslandNormal> first, ReadOnlySpan<IslandNormal> second, |
| | | 309 | | SolidBody lower, SolidBody upper, out Vector3d lowerPosition, out Vector3d upperPosition) |
| | | 310 | | { |
| | 488 | 311 | | lowerPosition = upperPosition = default; |
| | 488 | 312 | | if (!(TryGetTranslationDepth(first, out Fixed64 firstDepth) & TryGetTranslationDepth(second, out Fixed64 secondD |
| | 1 | 313 | | return false; |
| | | 314 | | |
| | | 315 | | // Project one depth per interface, without using angular inertia for a translation-only correction. |
| | 487 | 316 | | Span<Vector3d> lowerAxes = stackalloc Vector3d[] { first[0].Lower.Linear, second[0].Lower.Linear }; |
| | 487 | 317 | | Span<Vector3d> upperAxes = stackalloc Vector3d[] { first[0].Upper.Linear, second[0].Upper.Linear }; |
| | 487 | 318 | | Span<Fixed64> depths = stackalloc Fixed64[] { PenetrationSlop - firstDepth, PenetrationSlop - secondDepth }; |
| | 487 | 319 | | Span<Fixed64> impulses = stackalloc Fixed64[2]; |
| | 487 | 320 | | impulses.Clear(); |
| | 487 | 321 | | bool prepared = SmallContactNormalSolver.TrySolveTranslations(lowerAxes, upperAxes, |
| | 487 | 322 | | lower.EffectiveInverseMass, upper.EffectiveInverseMass, depths, impulses); |
| | 487 | 323 | | return prepared & TryPrepareIslandTranslation(lower, lowerAxes, impulses, out lowerPosition) |
| | 487 | 324 | | & TryPrepareIslandTranslation(upper, upperAxes, impulses, out upperPosition); |
| | | 325 | | } |
| | | 326 | | |
| | | 327 | | private static bool TryGetTranslationDepth(ReadOnlySpan<IslandNormal> rows, out Fixed64 depth) |
| | | 328 | | { |
| | 976 | 329 | | depth = Fixed64.Zero; |
| | 4697 | 330 | | foreach (IslandNormal row in rows) |
| | | 331 | | { |
| | 1373 | 332 | | if (row.Normal != rows[0].Normal) |
| | 1 | 333 | | return false; |
| | 1372 | 334 | | depth = FixedMath.Max(depth, row.Depth); |
| | | 335 | | } |
| | 975 | 336 | | return true; |
| | | 337 | | } |
| | | 338 | | |
| | | 339 | | private static bool TryPrepareIslandTranslation(SolidBody body, ReadOnlySpan<Vector3d> axes, |
| | | 340 | | ReadOnlySpan<Fixed64> impulses, out Vector3d position) |
| | | 341 | | { |
| | 974 | 342 | | position = default; |
| | 974 | 343 | | Fixed64 inverseMass = PenetrationCorrectionPercent * body.EffectiveInverseMass; |
| | 974 | 344 | | bool prepared = ContactResponseArithmetic3D.TryLinearCombination(axes[0], impulses[0], axes[1], impulses[1], |
| | 974 | 345 | | Vector3d.Zero, Fixed64.Zero, out Vector3d translation); |
| | 974 | 346 | | prepared &= ContactResponseArithmetic3D.TryScale(translation, inverseMass, out translation); |
| | 974 | 347 | | return prepared & (inverseMass > Fixed64.Zero) & Vector3d.TryAdd(body.Position3d, translation, out position); |
| | | 348 | | } |
| | | 349 | | |
| | | 350 | | private static void StoreIslandNormals(CollisionPair pair, ReadOnlySpan<IslandNormal> rows, |
| | | 351 | | ReadOnlySpan<Fixed64> speeds, ReadOnlySpan<Fixed64> impulses) |
| | | 352 | | { |
| | 983 | 353 | | ref ContactGroup group = ref pair.Manifold.GetGroup(0); |
| | 983 | 354 | | var diagnostics = pair.Context.Diagnostics; |
| | 4768 | 355 | | for (int i = 0; i < rows.Length; i++) |
| | | 356 | | { |
| | 1401 | 357 | | pair.StoreWarmStartImpulse(group.Key, group[i].ContactId, rows[i].Normal, impulses[i], |
| | 1401 | 358 | | rows[i].Tangent, rows[i].SecondaryTangent); |
| | 1401 | 359 | | if (diagnostics.Enabled && impulses[i] != Fixed64.Zero) |
| | 4 | 360 | | diagnostics.EmitResponseImpulse(pair, rows[i].Normal * impulses[i], speeds[i], group[i]); |
| | | 361 | | } |
| | 983 | 362 | | } |
| | | 363 | | |
| | | 364 | | private struct IslandNormal |
| | | 365 | | { |
| | | 366 | | internal IslandMotion Lower, Upper, LowerResponse, UpperResponse, LowerFriction, UpperFriction; |
| | | 367 | | internal Vector3d Normal; |
| | | 368 | | internal Fixed64 Speed, Depth, Tangent, SecondaryTangent; |
| | | 369 | | } |
| | | 370 | | |
| | | 371 | | private readonly struct IslandMotion |
| | | 372 | | { |
| | | 373 | | internal IslandMotion(Vector3d linear, Vector3d angular) |
| | | 374 | | { |
| | 10587 | 375 | | Linear = linear; |
| | 10587 | 376 | | Angular = angular; |
| | 10587 | 377 | | } |
| | | 378 | | |
| | | 379 | | internal readonly Vector3d Linear, Angular; |
| | | 380 | | |
| | | 381 | | internal bool TryAdd(IslandMotion other, out IslandMotion result) |
| | | 382 | | { |
| | 2850 | 383 | | if (!(Vector3d.TryAdd(Linear, other.Linear, out Vector3d linear) |
| | 2850 | 384 | | & Vector3d.TryAdd(Angular, other.Angular, out Vector3d angular))) |
| | | 385 | | { |
| | 1 | 386 | | result = default; |
| | 1 | 387 | | return false; |
| | | 388 | | } |
| | 2849 | 389 | | result = new IslandMotion(linear, angular); |
| | 2849 | 390 | | return true; |
| | | 391 | | } |
| | | 392 | | |
| | | 393 | | internal bool TryDot(IslandMotion other, out Fixed64 result) |
| | | 394 | | { |
| | 10392 | 395 | | result = default; |
| | 10392 | 396 | | return ContactResponseArithmetic3D.TryDot(Linear, other.Linear, out Fixed64 linear) |
| | 10392 | 397 | | & ContactResponseArithmetic3D.TryDot(Angular, other.Angular, out Fixed64 angular) |
| | 10392 | 398 | | & Fixed64.TryAdd(linear, angular, out result); |
| | | 399 | | } |
| | | 400 | | |
| | | 401 | | internal bool TryGetPreparedSpeed(IslandMotion other, out Fixed64 result) |
| | | 402 | | { |
| | 2822 | 403 | | result = default; |
| | | 404 | | // These are already-materialized velocities. Rounding a sub-LSB term |
| | | 405 | | // to zero here cannot amplify it through a subsequent impulse scale. |
| | 2822 | 406 | | return Vector3d.TryDot(Linear, other.Linear, out Fixed64 linear) |
| | 2822 | 407 | | & Vector3d.TryDot(Angular, other.Angular, out Fixed64 angular) |
| | 2822 | 408 | | & Fixed64.TryAdd(linear, angular, out result); |
| | | 409 | | } |
| | | 410 | | } |
| | | 411 | | |
| | | 412 | | private readonly struct IslandMobility |
| | | 413 | | { |
| | | 414 | | private readonly Fixed64 _inverseMass; |
| | | 415 | | private readonly Fixed3x3 _inverseInertia; |
| | | 416 | | |
| | | 417 | | internal IslandMobility(SolidBody body) |
| | | 418 | | { |
| | 995 | 419 | | _inverseMass = body.EffectiveInverseMass; |
| | 995 | 420 | | _inverseInertia = body.GetConstrainedInverseInertiaTensor(); |
| | 995 | 421 | | } |
| | | 422 | | |
| | | 423 | | internal bool TryTransform(IslandMotion jacobian, out IslandMotion response) |
| | | 424 | | { |
| | 3856 | 425 | | bool prepared = ContactResponseArithmetic3D.TryScale(jacobian.Linear, _inverseMass, out Vector3d linear) |
| | 3856 | 426 | | & ContactResponseArithmetic3D.TryTransformDirection(_inverseInertia, jacobian.Angular, out Vector3d angu |
| | 3856 | 427 | | response = new IslandMotion(linear, angular); |
| | 3856 | 428 | | return prepared; |
| | | 429 | | } |
| | | 430 | | } |
| | | 431 | | } |