| | | 1 | | //======================================================================= |
| | | 2 | | // GravitasDiagnosticSink.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 | | using Gravitas.CollisionHandling; |
| | | 12 | | using Gravitas.Constraints; |
| | | 13 | | using Gravitas.Queries; |
| | | 14 | | using SwiftCollections; |
| | | 15 | | |
| | | 16 | | namespace Gravitas.Diagnostics; |
| | | 17 | | |
| | | 18 | | /// <summary> |
| | | 19 | | /// Context-owned deterministic diagnostics buffer for physics events and debug draw commands. |
| | | 20 | | /// </summary> |
| | | 21 | | public sealed partial class GravitasDiagnosticSink |
| | | 22 | | { |
| | | 23 | | private readonly GravitasWorldContext _context; |
| | 10191 | 24 | | private readonly SwiftList<GravitasDiagnosticEvent> _events = new(); |
| | 10191 | 25 | | private readonly SwiftList<GravitasDebugDrawCommand> _drawCommands = new(); |
| | | 26 | | private int _eventSequence; |
| | | 27 | | private int _drawSequence; |
| | | 28 | | |
| | 10191 | 29 | | internal GravitasDiagnosticSink(GravitasWorldContext context) |
| | | 30 | | { |
| | 10191 | 31 | | ChronicleGuard.ThrowIfNull(context, nameof(context)); |
| | 10191 | 32 | | _context = context; |
| | 10191 | 33 | | } |
| | | 34 | | |
| | | 35 | | /// <summary> |
| | | 36 | | /// Gets whether this sink records events and draw commands. |
| | | 37 | | /// </summary> |
| | | 38 | | public bool Enabled { get; private set; } |
| | | 39 | | |
| | | 40 | | /// <summary> |
| | | 41 | | /// Gets captured diagnostic events for this context. |
| | | 42 | | /// </summary> |
| | | 43 | | /// <remarks> |
| | | 44 | | /// Borrows sink-owned storage. Consume before further capture, clear, |
| | | 45 | | /// disable, or context reset; copy the data when retaining it. |
| | | 46 | | /// </remarks> |
| | 281 | 47 | | public ReadOnlySpan<GravitasDiagnosticEvent> Events => _events.AsReadOnlySpan(); |
| | | 48 | | |
| | | 49 | | /// <summary> |
| | | 50 | | /// Gets captured engine-agnostic draw commands for this context. |
| | | 51 | | /// </summary> |
| | | 52 | | /// <remarks> |
| | | 53 | | /// Borrows sink-owned storage. Consume before further capture, clear, |
| | | 54 | | /// disable, or context reset; copy the data when retaining it. |
| | | 55 | | /// </remarks> |
| | 45 | 56 | | public ReadOnlySpan<GravitasDebugDrawCommand> DrawCommands => _drawCommands.AsReadOnlySpan(); |
| | | 57 | | |
| | | 58 | | /// <summary> |
| | | 59 | | /// Dispatches captured events in buffer order to a typed diagnostic visitor. |
| | | 60 | | /// </summary> |
| | | 61 | | public void DispatchEventsTo(GravitasDiagnosticEventVisitor visitor) |
| | | 62 | | { |
| | 2 | 63 | | if (visitor == null) |
| | 1 | 64 | | throw new ArgumentNullException(nameof(visitor)); |
| | | 65 | | |
| | 1 | 66 | | ReadOnlySpan<GravitasDiagnosticEvent> events = Events; |
| | 6 | 67 | | for (int i = 0; i < events.Length; i++) |
| | 2 | 68 | | events[i].DispatchTo(visitor); |
| | 1 | 69 | | } |
| | | 70 | | |
| | | 71 | | /// <summary> |
| | | 72 | | /// Dispatches captured draw commands in buffer order to a typed debug draw visitor. |
| | | 73 | | /// </summary> |
| | | 74 | | public void DispatchDrawCommandsTo(GravitasDebugDrawCommandVisitor visitor) |
| | | 75 | | { |
| | 2 | 76 | | if (visitor == null) |
| | 1 | 77 | | throw new ArgumentNullException(nameof(visitor)); |
| | | 78 | | |
| | 1 | 79 | | ReadOnlySpan<GravitasDebugDrawCommand> commands = DrawCommands; |
| | 6 | 80 | | for (int i = 0; i < commands.Length; i++) |
| | 2 | 81 | | commands[i].DispatchTo(visitor); |
| | 1 | 82 | | } |
| | | 83 | | |
| | | 84 | | /// <summary>Gets the number of captured diagnostic events.</summary> |
| | 36 | 85 | | public int EventCount => _events.Count; |
| | | 86 | | |
| | | 87 | | /// <summary>Gets the number of captured debug draw commands.</summary> |
| | 8 | 88 | | public int DrawCommandCount => _drawCommands.Count; |
| | | 89 | | |
| | 3 | 90 | | internal int EventCapacity => _events.Capacity; |
| | | 91 | | |
| | 4 | 92 | | internal int DrawCommandCapacity => _drawCommands.Capacity; |
| | | 93 | | |
| | | 94 | | /// <summary> |
| | | 95 | | /// Enables diagnostics and optionally reserves buffer capacity for hot-path capture. |
| | | 96 | | /// </summary> |
| | | 97 | | public void Enable(int eventCapacity = 128, int drawCommandCapacity = 128) |
| | | 98 | | { |
| | 174 | 99 | | Enabled = true; |
| | 174 | 100 | | if (eventCapacity > 0) |
| | 152 | 101 | | _events.EnsureCapacity(eventCapacity); |
| | 174 | 102 | | if (drawCommandCapacity > 0) |
| | 132 | 103 | | _drawCommands.EnsureCapacity(drawCommandCapacity); |
| | 174 | 104 | | } |
| | | 105 | | |
| | | 106 | | /// <summary> |
| | | 107 | | /// Disables diagnostics and clears captured data. |
| | | 108 | | /// </summary> |
| | | 109 | | public void Disable() |
| | | 110 | | { |
| | 5 | 111 | | Enabled = false; |
| | 5 | 112 | | Clear(); |
| | 5 | 113 | | } |
| | | 114 | | |
| | | 115 | | /// <summary> |
| | | 116 | | /// Clears captured data while retaining allocated buffer capacity. |
| | | 117 | | /// </summary> |
| | | 118 | | public void Clear() |
| | | 119 | | { |
| | 159 | 120 | | _events.FastClear(); |
| | 159 | 121 | | _drawCommands.FastClear(); |
| | 159 | 122 | | _eventSequence = 0; |
| | 159 | 123 | | _drawSequence = 0; |
| | 159 | 124 | | } |
| | | 125 | | |
| | 113 | 126 | | internal void Reset() => Clear(); |
| | | 127 | | |
| | | 128 | | internal void EmitForceDelta(SolidBody body, Vector3d force, Vector3d accelerationDelta) |
| | | 129 | | { |
| | 2157 | 130 | | if (!Enabled) |
| | 2151 | 131 | | return; |
| | | 132 | | |
| | 6 | 133 | | AddEvent( |
| | 6 | 134 | | GravitasDiagnosticEventKind.ForceDelta, |
| | 6 | 135 | | bodyId: body.DynamicId, |
| | 6 | 136 | | colliderAId: body.Collider.Id, |
| | 6 | 137 | | colliderAType: body.Collider.Shape, |
| | 6 | 138 | | vector: force, |
| | 6 | 139 | | pointA: accelerationDelta, |
| | 6 | 140 | | scalarA: force.Magnitude); |
| | 6 | 141 | | } |
| | | 142 | | |
| | | 143 | | internal void EmitTorqueDelta(SolidBody body, Vector3d torque) |
| | | 144 | | { |
| | 114 | 145 | | if (!Enabled) |
| | 108 | 146 | | return; |
| | | 147 | | |
| | 6 | 148 | | AddEvent( |
| | 6 | 149 | | GravitasDiagnosticEventKind.TorqueDelta, |
| | 6 | 150 | | bodyId: body.DynamicId, |
| | 6 | 151 | | colliderAId: body.Collider.Id, |
| | 6 | 152 | | colliderAType: body.Collider.Shape, |
| | 6 | 153 | | vector: torque, |
| | 6 | 154 | | scalarA: torque.Magnitude); |
| | 6 | 155 | | } |
| | | 156 | | |
| | | 157 | | internal void EmitLinearVelocityDelta(SolidBody body, Vector3d before, Vector3d after) |
| | | 158 | | { |
| | 450764 | 159 | | if (!Enabled) |
| | 450593 | 160 | | return; |
| | | 161 | | |
| | 171 | 162 | | AddEvent( |
| | 171 | 163 | | GravitasDiagnosticEventKind.LinearVelocityDelta, |
| | 171 | 164 | | bodyId: body.DynamicId, |
| | 171 | 165 | | colliderAId: body.Collider.Id, |
| | 171 | 166 | | colliderAType: body.Collider.Shape, |
| | 171 | 167 | | start: before, |
| | 171 | 168 | | end: after, |
| | 171 | 169 | | vector: after - before, |
| | 171 | 170 | | scalarA: after.Magnitude); |
| | 171 | 171 | | } |
| | | 172 | | |
| | | 173 | | internal void EmitAngularVelocityDelta(SolidBody body, Vector3d before, Vector3d after) |
| | | 174 | | { |
| | 378732 | 175 | | if (!Enabled) |
| | 378471 | 176 | | return; |
| | | 177 | | |
| | 261 | 178 | | AddEvent( |
| | 261 | 179 | | GravitasDiagnosticEventKind.AngularVelocityDelta, |
| | 261 | 180 | | bodyId: body.DynamicId, |
| | 261 | 181 | | colliderAId: body.Collider.Id, |
| | 261 | 182 | | colliderAType: body.Collider.Shape, |
| | 261 | 183 | | start: before, |
| | 261 | 184 | | end: after, |
| | 261 | 185 | | vector: after - before, |
| | 261 | 186 | | scalarA: after.Magnitude); |
| | 261 | 187 | | } |
| | | 188 | | |
| | | 189 | | internal void EmitGroundProbe( |
| | | 190 | | SolidBody body, |
| | | 191 | | GroundProbeMode mode, |
| | | 192 | | Vector3d origin, |
| | | 193 | | Vector3d end, |
| | | 194 | | Fixed64 radius, |
| | | 195 | | bool hit, |
| | | 196 | | Physics3DHit raycastHit) |
| | | 197 | | { |
| | 13782 | 198 | | if (!Enabled) |
| | 13692 | 199 | | return; |
| | | 200 | | |
| | 90 | 201 | | Vector3d point = default; |
| | 90 | 202 | | bool hasPoint = hit && raycastHit.TryGetPoint(out point); |
| | 90 | 203 | | AddEvent( |
| | 90 | 204 | | GravitasDiagnosticEventKind.GroundProbe, |
| | 90 | 205 | | bodyId: body.DynamicId, |
| | 90 | 206 | | colliderAId: body.Collider.Id, |
| | 90 | 207 | | colliderBId: raycastHit.Collider?.Id ?? -1, |
| | 90 | 208 | | colliderAType: body.Collider.Shape, |
| | 90 | 209 | | colliderBType: raycastHit.Collider?.Shape ?? ColliderType.None, |
| | 90 | 210 | | start: origin, |
| | 90 | 211 | | end: end, |
| | 90 | 212 | | pointA: point, |
| | 90 | 213 | | hasPointA: hasPoint, |
| | 90 | 214 | | vector: raycastHit.Normal, |
| | 90 | 215 | | scalarA: radius, |
| | 90 | 216 | | scalarB: raycastHit.Distance, |
| | 90 | 217 | | dataA: (int)mode, |
| | 90 | 218 | | hit: hit); |
| | 90 | 219 | | } |
| | | 220 | | |
| | | 221 | | internal void EmitGroundProbe( |
| | | 222 | | SolidBody2D body, |
| | | 223 | | GroundProbeMode2D mode, |
| | | 224 | | Vector2d start, |
| | | 225 | | Vector2d end, |
| | | 226 | | Fixed64 radius, |
| | | 227 | | bool hit, |
| | | 228 | | Physics2DHit raycastHit) |
| | | 229 | | { |
| | 7575 | 230 | | if (!Enabled) |
| | 7507 | 231 | | return; |
| | | 232 | | |
| | 68 | 233 | | AddEvent( |
| | 68 | 234 | | GravitasDiagnosticEventKind.GroundProbe, |
| | 68 | 235 | | bodyId: body.DynamicId, |
| | 68 | 236 | | colliderAId: body.Collider.Id, |
| | 68 | 237 | | colliderBId: raycastHit.Collider?.Id ?? -1, |
| | 68 | 238 | | colliderADimension: GravitasColliderDimension.TwoD, |
| | 68 | 239 | | colliderBDimension: raycastHit.Collider == null ? GravitasColliderDimension.None : GravitasColliderDimension |
| | 68 | 240 | | colliderA2DType: body.Collider.Shape, |
| | 68 | 241 | | colliderB2DType: raycastHit.Collider?.Shape ?? ColliderType2D.None, |
| | 68 | 242 | | start: ToDiagnosticVector(start), |
| | 68 | 243 | | end: ToDiagnosticVector(end), |
| | 68 | 244 | | pointA: ToDiagnosticVector(raycastHit.Point), |
| | 68 | 245 | | hasPointA: hit, |
| | 68 | 246 | | vector: ToDiagnosticVector(raycastHit.Normal), |
| | 68 | 247 | | scalarA: radius, |
| | 68 | 248 | | scalarB: raycastHit.Distance, |
| | 68 | 249 | | dataA: (int)mode, |
| | 68 | 250 | | dataB: (int)GravitasColliderDimension.TwoD, |
| | 68 | 251 | | hit: hit); |
| | 68 | 252 | | } |
| | | 253 | | |
| | | 254 | | internal void EmitRayQuery( |
| | | 255 | | Vector3d start, |
| | | 256 | | Vector3d end, |
| | | 257 | | Fixed64 radius, |
| | | 258 | | int layerMaskBits, |
| | | 259 | | bool hit, |
| | | 260 | | int hitCount, |
| | | 261 | | Physics3DHit raycastHit) |
| | | 262 | | { |
| | 26793 | 263 | | if (!Enabled) |
| | 26682 | 264 | | return; |
| | | 265 | | |
| | 111 | 266 | | Vector3d point = default; |
| | 111 | 267 | | bool hasPoint = hit && raycastHit.TryGetPoint(out point); |
| | 111 | 268 | | AddEvent( |
| | 111 | 269 | | GravitasDiagnosticEventKind.RayQuery, |
| | 111 | 270 | | colliderAId: raycastHit.Collider?.Id ?? -1, |
| | 111 | 271 | | colliderAType: raycastHit.Collider?.Shape ?? ColliderType.None, |
| | 111 | 272 | | start: start, |
| | 111 | 273 | | end: end, |
| | 111 | 274 | | pointA: point, |
| | 111 | 275 | | hasPointA: hasPoint, |
| | 111 | 276 | | vector: raycastHit.Normal, |
| | 111 | 277 | | scalarA: radius, |
| | 111 | 278 | | scalarB: raycastHit.Distance, |
| | 111 | 279 | | dataA: layerMaskBits, |
| | 111 | 280 | | dataB: hitCount, |
| | 111 | 281 | | hit: hit); |
| | 111 | 282 | | } |
| | | 283 | | |
| | | 284 | | internal void EmitCircleQuery( |
| | | 285 | | Vector3d center, |
| | | 286 | | Fixed64 radius, |
| | | 287 | | Vector3d direction, |
| | | 288 | | Fixed64 maxDistance, |
| | | 289 | | int layerMaskBits, |
| | | 290 | | bool hit, |
| | | 291 | | int hitCount, |
| | | 292 | | Physics3DHit raycastHit) |
| | | 293 | | { |
| | 41 | 294 | | if (!Enabled) |
| | 37 | 295 | | return; |
| | | 296 | | |
| | 4 | 297 | | Vector3d point = default; |
| | 4 | 298 | | bool hasPoint = hit && raycastHit.TryGetPoint(out point); |
| | 4 | 299 | | AddEvent( |
| | 4 | 300 | | GravitasDiagnosticEventKind.CircleQuery, |
| | 4 | 301 | | colliderAId: raycastHit.Collider?.Id ?? -1, |
| | 4 | 302 | | colliderAType: raycastHit.Collider?.Shape ?? ColliderType.None, |
| | 4 | 303 | | start: center, |
| | 4 | 304 | | end: center + direction * maxDistance, |
| | 4 | 305 | | pointA: point, |
| | 4 | 306 | | hasPointA: hasPoint, |
| | 4 | 307 | | vector: direction, |
| | 4 | 308 | | scalarA: radius, |
| | 4 | 309 | | scalarB: raycastHit.Distance, |
| | 4 | 310 | | dataA: layerMaskBits, |
| | 4 | 311 | | dataB: hitCount, |
| | 4 | 312 | | hit: hit); |
| | 4 | 313 | | } |
| | | 314 | | |
| | | 315 | | internal void EmitQuerySummary( |
| | | 316 | | GravitasColliderDimension sourceDimension, |
| | | 317 | | GravitasColliderDimension targetDimension, |
| | | 318 | | Vector3d start, |
| | | 319 | | Vector3d end, |
| | | 320 | | int exactReducerAttempts, |
| | | 321 | | int acceptedHits, |
| | | 322 | | int fallbackHits, |
| | | 323 | | int rejectedConservativeCandidates) |
| | | 324 | | { |
| | 6 | 325 | | AddEvent( |
| | 6 | 326 | | GravitasDiagnosticEventKind.QuerySummary, |
| | 6 | 327 | | colliderADimension: sourceDimension, |
| | 6 | 328 | | colliderBDimension: targetDimension, |
| | 6 | 329 | | start: start, |
| | 6 | 330 | | end: end, |
| | 6 | 331 | | scalarA: (Fixed64)fallbackHits, |
| | 6 | 332 | | scalarB: (Fixed64)rejectedConservativeCandidates, |
| | 6 | 333 | | dataA: exactReducerAttempts, |
| | 6 | 334 | | dataB: acceptedHits, |
| | 6 | 335 | | hit: fallbackHits > 0 || rejectedConservativeCandidates > 0); |
| | 6 | 336 | | } |
| | | 337 | | |
| | | 338 | | internal void EmitContact(CollisionPair pair, bool hit) |
| | | 339 | | { |
| | 52815 | 340 | | if (!Enabled) |
| | 52798 | 341 | | return; |
| | | 342 | | |
| | 17 | 343 | | bool hasContact = pair.Manifold.HasContact; |
| | 17 | 344 | | ManifoldContact contact = hasContact |
| | 17 | 345 | | ? pair.Manifold.PrimaryContact |
| | 17 | 346 | | : default; |
| | 17 | 347 | | Vector3d pointA = default; |
| | 17 | 348 | | Vector3d pointB = default; |
| | 17 | 349 | | bool hasPointA = hasContact && contact.TryGetPointA(out pointA); |
| | 17 | 350 | | bool hasPointB = hasContact && contact.TryGetPointB(out pointB); |
| | | 351 | | |
| | 17 | 352 | | AddEvent( |
| | 17 | 353 | | GravitasDiagnosticEventKind.Contact, |
| | 17 | 354 | | colliderAId: pair.ColliderA.Id, |
| | 17 | 355 | | colliderBId: pair.ColliderB.Id, |
| | 17 | 356 | | colliderAType: pair.ColliderA.Shape, |
| | 17 | 357 | | colliderBType: pair.ColliderB.Shape, |
| | 17 | 358 | | pointA: pointA, |
| | 17 | 359 | | pointB: pointB, |
| | 17 | 360 | | hasPointA: hasPointA, |
| | 17 | 361 | | hasPointB: hasPointB, |
| | 17 | 362 | | vector: contact.Normal, |
| | 17 | 363 | | scalarA: contact.Depth, |
| | 17 | 364 | | dataA: pair.Manifold.Count, |
| | 17 | 365 | | dataB: contact.DepthIsClamped ? 1 : 0, |
| | 17 | 366 | | hit: hit && hasContact); |
| | 17 | 367 | | } |
| | | 368 | | |
| | | 369 | | internal void EmitResponseImpulse(CollisionPair pair, Vector3d impulse, Fixed64 normalVelocity, |
| | | 370 | | ManifoldContact? sample = null) |
| | | 371 | | { |
| | 57274 | 372 | | if (!Enabled) |
| | 57233 | 373 | | return; |
| | | 374 | | |
| | 41 | 375 | | ManifoldContact contact = sample ?? pair.Manifold.PrimaryContact; |
| | 41 | 376 | | bool hasPointA = contact.TryGetPointA(out Vector3d pointA); |
| | 41 | 377 | | bool hasPointB = contact.TryGetPointB(out Vector3d pointB); |
| | | 378 | | |
| | 41 | 379 | | AddEvent( |
| | 41 | 380 | | GravitasDiagnosticEventKind.ResponseImpulse, |
| | 41 | 381 | | colliderAId: pair.ColliderA.Id, |
| | 41 | 382 | | colliderBId: pair.ColliderB.Id, |
| | 41 | 383 | | colliderAType: pair.ColliderA.Shape, |
| | 41 | 384 | | colliderBType: pair.ColliderB.Shape, |
| | 41 | 385 | | pointA: pointA, |
| | 41 | 386 | | pointB: pointB, |
| | 41 | 387 | | hasPointA: hasPointA, |
| | 41 | 388 | | hasPointB: hasPointB, |
| | 41 | 389 | | vector: impulse, |
| | 41 | 390 | | scalarA: impulse.Magnitude, |
| | 41 | 391 | | scalarB: normalVelocity, |
| | 41 | 392 | | hit: true); |
| | 41 | 393 | | } |
| | | 394 | | |
| | | 395 | | internal void EmitMixedQuery( |
| | | 396 | | Vector3d start, |
| | | 397 | | Vector3d end, |
| | | 398 | | Fixed64 radius, |
| | | 399 | | int layerMaskBits, |
| | | 400 | | bool hit, |
| | | 401 | | int hitCount, |
| | | 402 | | PhysicsMixedHit mixedHit) |
| | | 403 | | { |
| | 2660 | 404 | | if (!Enabled) |
| | 2655 | 405 | | return; |
| | | 406 | | |
| | 5 | 407 | | Vector3d point3D = default; |
| | 5 | 408 | | Vector3d point2D = default; |
| | 5 | 409 | | bool hasPoint3D = hit && mixedHit.TryGetPoint3D(out point3D); |
| | 5 | 410 | | bool hasPoint2D = hit && mixedHit.TryGetPoint2D(out point2D); |
| | 5 | 411 | | AddEvent( |
| | 5 | 412 | | GravitasDiagnosticEventKind.MixedQuery, |
| | 5 | 413 | | colliderAId: mixedHit.Collider3D?.Id ?? -1, |
| | 5 | 414 | | colliderBId: mixedHit.Collider2D?.Id ?? -1, |
| | 5 | 415 | | colliderADimension: mixedHit.Collider3D == null ? GravitasColliderDimension.None : GravitasColliderDimension |
| | 5 | 416 | | colliderBDimension: mixedHit.Collider2D == null ? GravitasColliderDimension.None : GravitasColliderDimension |
| | 5 | 417 | | colliderAType: mixedHit.Collider3D?.Shape ?? ColliderType.None, |
| | 5 | 418 | | colliderB2DType: mixedHit.Collider2D?.Shape ?? ColliderType2D.None, |
| | 5 | 419 | | start: start, |
| | 5 | 420 | | end: end, |
| | 5 | 421 | | pointA: point3D, |
| | 5 | 422 | | pointB: point2D, |
| | 5 | 423 | | hasPointA: hasPoint3D, |
| | 5 | 424 | | hasPointB: hasPoint2D, |
| | 5 | 425 | | vector: mixedHit.Normal3DTo2D, |
| | 5 | 426 | | scalarA: radius, |
| | 5 | 427 | | scalarB: mixedHit.Distance, |
| | 5 | 428 | | dataA: layerMaskBits, |
| | 5 | 429 | | dataB: hitCount, |
| | 5 | 430 | | hit: hit); |
| | 5 | 431 | | } |
| | | 432 | | |
| | | 433 | | internal void EmitMixedContact(CollisionPairMixed pair, MixedContact contact) |
| | | 434 | | { |
| | 16102 | 435 | | if (!Enabled) |
| | 16079 | 436 | | return; |
| | | 437 | | |
| | 23 | 438 | | bool hasPoint3D = contact.TryGetPoint3D(out Vector3d point3D); |
| | 23 | 439 | | bool hasPoint2D = contact.TryGetPoint2D(out Vector3d point2D); |
| | 23 | 440 | | AddEvent( |
| | 23 | 441 | | GravitasDiagnosticEventKind.MixedContact, |
| | 23 | 442 | | colliderAId: pair.Collider3DId, |
| | 23 | 443 | | colliderBId: pair.Collider2DId, |
| | 23 | 444 | | colliderADimension: GravitasColliderDimension.ThreeD, |
| | 23 | 445 | | colliderBDimension: GravitasColliderDimension.TwoD, |
| | 23 | 446 | | colliderAType: pair.Collider3D.Shape, |
| | 23 | 447 | | colliderB2DType: pair.Collider2D.Shape, |
| | 23 | 448 | | pointA: point3D, |
| | 23 | 449 | | pointB: point2D, |
| | 23 | 450 | | hasPointA: hasPoint3D, |
| | 23 | 451 | | hasPointB: hasPoint2D, |
| | 23 | 452 | | vector: contact.Normal3DTo2D, |
| | 23 | 453 | | scalarA: contact.Depth, |
| | 23 | 454 | | dataA: contact.DepthIsClamped ? 1 : 0, |
| | 23 | 455 | | hit: true); |
| | 23 | 456 | | } |
| | | 457 | | |
| | | 458 | | internal void EmitMixedResponseImpulse( |
| | | 459 | | CollisionPairMixed pair, |
| | | 460 | | MixedContact contact, |
| | | 461 | | Vector3d impulse, |
| | | 462 | | Fixed64 normalVelocity, |
| | | 463 | | int iteration, |
| | | 464 | | int iterationLimit) |
| | | 465 | | { |
| | 17853 | 466 | | if (!Enabled) |
| | 17837 | 467 | | return; |
| | | 468 | | |
| | 16 | 469 | | bool hasPoint3D = contact.TryGetPoint3D(out Vector3d point3D); |
| | 16 | 470 | | bool hasPoint2D = contact.TryGetPoint2D(out Vector3d point2D); |
| | 16 | 471 | | AddEvent( |
| | 16 | 472 | | GravitasDiagnosticEventKind.MixedResponseImpulse, |
| | 16 | 473 | | colliderAId: pair.Collider3DId, |
| | 16 | 474 | | colliderBId: pair.Collider2DId, |
| | 16 | 475 | | colliderADimension: GravitasColliderDimension.ThreeD, |
| | 16 | 476 | | colliderBDimension: GravitasColliderDimension.TwoD, |
| | 16 | 477 | | colliderAType: pair.Collider3D.Shape, |
| | 16 | 478 | | colliderB2DType: pair.Collider2D.Shape, |
| | 16 | 479 | | pointA: point3D, |
| | 16 | 480 | | pointB: point2D, |
| | 16 | 481 | | hasPointA: hasPoint3D, |
| | 16 | 482 | | hasPointB: hasPoint2D, |
| | 16 | 483 | | vector: impulse, |
| | 16 | 484 | | scalarA: impulse.Magnitude, |
| | 16 | 485 | | scalarB: normalVelocity, |
| | 16 | 486 | | dataA: iteration, |
| | 16 | 487 | | dataB: iterationLimit, |
| | 16 | 488 | | hit: true); |
| | 16 | 489 | | } |
| | | 490 | | |
| | | 491 | | internal void EmitMixedResponseIsland( |
| | | 492 | | int rootKey, |
| | | 493 | | int constraintCount, |
| | | 494 | | int iterationCount, |
| | | 495 | | bool reachedIterationLimit) |
| | | 496 | | { |
| | 1144 | 497 | | if (!Enabled) |
| | 1137 | 498 | | return; |
| | | 499 | | |
| | 7 | 500 | | AddEvent( |
| | 7 | 501 | | GravitasDiagnosticEventKind.MixedResponseIsland, |
| | 7 | 502 | | bodyId: rootKey, |
| | 7 | 503 | | dataA: constraintCount, |
| | 7 | 504 | | dataB: iterationCount, |
| | 7 | 505 | | hit: reachedIterationLimit); |
| | 7 | 506 | | } |
| | | 507 | | |
| | | 508 | | internal void EmitJointRegistered(Joint3D joint) |
| | | 509 | | { |
| | 372 | 510 | | if (!Enabled) |
| | 367 | 511 | | return; |
| | | 512 | | |
| | 5 | 513 | | AddEvent( |
| | 5 | 514 | | GravitasDiagnosticEventKind.JointRegistered, |
| | 5 | 515 | | jointId: joint.Id, |
| | 5 | 516 | | colliderAId: joint.BodyA.Collider.Id, |
| | 5 | 517 | | colliderBId: joint.BodyB.Collider.Id, |
| | 5 | 518 | | colliderAType: joint.BodyA.Collider.Shape, |
| | 5 | 519 | | colliderBType: joint.BodyB.Collider.Shape, |
| | 5 | 520 | | dataA: (int)joint.Type, |
| | 5 | 521 | | dataB: (int)joint.CollisionPolicy); |
| | 5 | 522 | | } |
| | | 523 | | |
| | | 524 | | internal void EmitJointRegistered(Joint2D joint) |
| | | 525 | | { |
| | 214 | 526 | | if (!Enabled) |
| | 209 | 527 | | return; |
| | | 528 | | |
| | 5 | 529 | | AddEvent( |
| | 5 | 530 | | GravitasDiagnosticEventKind.JointRegistered, |
| | 5 | 531 | | jointId: joint.Id, |
| | 5 | 532 | | colliderAId: joint.BodyA.Collider.Id, |
| | 5 | 533 | | colliderBId: joint.BodyB.Collider.Id, |
| | 5 | 534 | | colliderADimension: GravitasColliderDimension.TwoD, |
| | 5 | 535 | | colliderBDimension: GravitasColliderDimension.TwoD, |
| | 5 | 536 | | colliderA2DType: joint.BodyA.Collider.Shape, |
| | 5 | 537 | | colliderB2DType: joint.BodyB.Collider.Shape, |
| | 5 | 538 | | dataA: (int)joint.Type, |
| | 5 | 539 | | dataB: (int)joint.CollisionPolicy); |
| | 5 | 540 | | } |
| | | 541 | | |
| | | 542 | | internal void EmitJointRemoved(Joint3D joint) |
| | | 543 | | { |
| | 29 | 544 | | if (!Enabled) |
| | 24 | 545 | | return; |
| | | 546 | | |
| | 5 | 547 | | AddEvent( |
| | 5 | 548 | | GravitasDiagnosticEventKind.JointRemoved, |
| | 5 | 549 | | jointId: joint.Id, |
| | 5 | 550 | | colliderAId: joint.BodyA.Collider.Id, |
| | 5 | 551 | | colliderBId: joint.BodyB.Collider.Id, |
| | 5 | 552 | | colliderAType: joint.BodyA.Collider.Shape, |
| | 5 | 553 | | colliderBType: joint.BodyB.Collider.Shape, |
| | 5 | 554 | | dataA: (int)joint.Type, |
| | 5 | 555 | | dataB: (int)joint.CollisionPolicy); |
| | 5 | 556 | | } |
| | | 557 | | |
| | | 558 | | internal void EmitJointRemoved(Joint2D joint) |
| | | 559 | | { |
| | 29 | 560 | | if (!Enabled) |
| | 24 | 561 | | return; |
| | | 562 | | |
| | 5 | 563 | | AddEvent( |
| | 5 | 564 | | GravitasDiagnosticEventKind.JointRemoved, |
| | 5 | 565 | | jointId: joint.Id, |
| | 5 | 566 | | colliderAId: joint.BodyA.Collider.Id, |
| | 5 | 567 | | colliderBId: joint.BodyB.Collider.Id, |
| | 5 | 568 | | colliderADimension: GravitasColliderDimension.TwoD, |
| | 5 | 569 | | colliderBDimension: GravitasColliderDimension.TwoD, |
| | 5 | 570 | | colliderA2DType: joint.BodyA.Collider.Shape, |
| | 5 | 571 | | colliderB2DType: joint.BodyB.Collider.Shape, |
| | 5 | 572 | | dataA: (int)joint.Type, |
| | 5 | 573 | | dataB: (int)joint.CollisionPolicy); |
| | 5 | 574 | | } |
| | | 575 | | |
| | | 576 | | internal void EmitJointImpulse(Joint3D joint, JointSolveMetrics3D metrics) |
| | | 577 | | { |
| | 74265 | 578 | | if (!Enabled) |
| | 74132 | 579 | | return; |
| | | 580 | | |
| | 133 | 581 | | AddEvent( |
| | 133 | 582 | | GravitasDiagnosticEventKind.JointImpulse, |
| | 133 | 583 | | jointId: joint.Id, |
| | 133 | 584 | | colliderAId: joint.BodyA.Collider.Id, |
| | 133 | 585 | | colliderBId: joint.BodyB.Collider.Id, |
| | 133 | 586 | | colliderAType: joint.BodyA.Collider.Shape, |
| | 133 | 587 | | colliderBType: joint.BodyB.Collider.Shape, |
| | 133 | 588 | | vector: new Vector3d( |
| | 133 | 589 | | metrics.MotorImpulseMagnitude, |
| | 133 | 590 | | metrics.MotorErrorMagnitude, |
| | 133 | 591 | | metrics.AngularLimitErrorMagnitude), |
| | 133 | 592 | | scalarA: metrics.AccumulatedImpulseMagnitude, |
| | 133 | 593 | | scalarB: metrics.LinearAnchorErrorMagnitude, |
| | 133 | 594 | | dataA: metrics.PreparedRowCount, |
| | 133 | 595 | | dataB: metrics.ClampedRowCount, |
| | 133 | 596 | | hit: metrics.IncrementalImpulseMagnitude > Fixed64.Zero); |
| | 133 | 597 | | } |
| | | 598 | | |
| | | 599 | | internal void EmitJointImpulse(Joint2D joint, JointSolveMetrics2D metrics) |
| | | 600 | | { |
| | 608 | 601 | | if (!Enabled) |
| | 603 | 602 | | return; |
| | | 603 | | |
| | 5 | 604 | | AddEvent( |
| | 5 | 605 | | GravitasDiagnosticEventKind.JointImpulse, |
| | 5 | 606 | | jointId: joint.Id, |
| | 5 | 607 | | colliderAId: joint.BodyA.Collider.Id, |
| | 5 | 608 | | colliderBId: joint.BodyB.Collider.Id, |
| | 5 | 609 | | colliderADimension: GravitasColliderDimension.TwoD, |
| | 5 | 610 | | colliderBDimension: GravitasColliderDimension.TwoD, |
| | 5 | 611 | | colliderA2DType: joint.BodyA.Collider.Shape, |
| | 5 | 612 | | colliderB2DType: joint.BodyB.Collider.Shape, |
| | 5 | 613 | | vector: new Vector3d( |
| | 5 | 614 | | metrics.MotorImpulseMagnitude, |
| | 5 | 615 | | metrics.MotorErrorMagnitude, |
| | 5 | 616 | | metrics.LimitErrorMagnitude), |
| | 5 | 617 | | scalarA: metrics.AccumulatedImpulseMagnitude, |
| | 5 | 618 | | scalarB: metrics.LinearAnchorErrorMagnitude, |
| | 5 | 619 | | dataA: metrics.PreparedRowCount, |
| | 5 | 620 | | dataB: metrics.ClampedRowCount, |
| | 5 | 621 | | hit: metrics.IncrementalImpulseMagnitude > Fixed64.Zero); |
| | 5 | 622 | | } |
| | | 623 | | |
| | | 624 | | internal void EmitJointLimitReached(Joint3D joint, Fixed64 limitError) |
| | | 625 | | { |
| | 121 | 626 | | if (!Enabled) |
| | 120 | 627 | | return; |
| | | 628 | | |
| | 1 | 629 | | AddEvent( |
| | 1 | 630 | | GravitasDiagnosticEventKind.JointLimitReached, |
| | 1 | 631 | | jointId: joint.Id, |
| | 1 | 632 | | colliderAId: joint.BodyA.Collider.Id, |
| | 1 | 633 | | colliderBId: joint.BodyB.Collider.Id, |
| | 1 | 634 | | colliderAType: joint.BodyA.Collider.Shape, |
| | 1 | 635 | | colliderBType: joint.BodyB.Collider.Shape, |
| | 1 | 636 | | scalarB: limitError, |
| | 1 | 637 | | dataA: (int)joint.Limits.Kind, |
| | 1 | 638 | | hit: limitError != Fixed64.Zero); |
| | 1 | 639 | | } |
| | | 640 | | |
| | | 641 | | internal void EmitJointLimitReached(Joint2D joint, Fixed64 limitError) |
| | | 642 | | { |
| | 161 | 643 | | if (!Enabled) |
| | 160 | 644 | | return; |
| | | 645 | | |
| | 1 | 646 | | AddEvent( |
| | 1 | 647 | | GravitasDiagnosticEventKind.JointLimitReached, |
| | 1 | 648 | | jointId: joint.Id, |
| | 1 | 649 | | colliderAId: joint.BodyA.Collider.Id, |
| | 1 | 650 | | colliderBId: joint.BodyB.Collider.Id, |
| | 1 | 651 | | colliderADimension: GravitasColliderDimension.TwoD, |
| | 1 | 652 | | colliderBDimension: GravitasColliderDimension.TwoD, |
| | 1 | 653 | | colliderA2DType: joint.BodyA.Collider.Shape, |
| | 1 | 654 | | colliderB2DType: joint.BodyB.Collider.Shape, |
| | 1 | 655 | | scalarB: limitError, |
| | 1 | 656 | | dataA: (int)joint.Limits.Kind, |
| | 1 | 657 | | hit: limitError != Fixed64.Zero); |
| | 1 | 658 | | } |
| | | 659 | | |
| | | 660 | | internal void EmitRagdollActivated(int ragdollId, int linkCount, int jointCount, bool isActive) |
| | | 661 | | { |
| | 16 | 662 | | if (!Enabled) |
| | 11 | 663 | | return; |
| | | 664 | | |
| | 5 | 665 | | AddEvent( |
| | 5 | 666 | | GravitasDiagnosticEventKind.RagdollActivated, |
| | 5 | 667 | | bodyId: ragdollId, |
| | 5 | 668 | | dataA: linkCount, |
| | 5 | 669 | | dataB: jointCount, |
| | 5 | 670 | | hit: isActive); |
| | 5 | 671 | | } |
| | | 672 | | |
| | | 673 | | private void AddEvent( |
| | | 674 | | GravitasDiagnosticEventKind kind, |
| | | 675 | | int bodyId = -1, |
| | | 676 | | int jointId = -1, |
| | | 677 | | int colliderAId = -1, |
| | | 678 | | int colliderBId = -1, |
| | | 679 | | GravitasColliderDimension colliderADimension = GravitasColliderDimension.None, |
| | | 680 | | GravitasColliderDimension colliderBDimension = GravitasColliderDimension.None, |
| | | 681 | | ColliderType colliderAType = ColliderType.None, |
| | | 682 | | ColliderType colliderBType = ColliderType.None, |
| | | 683 | | ColliderType2D colliderA2DType = ColliderType2D.None, |
| | | 684 | | ColliderType2D colliderB2DType = ColliderType2D.None, |
| | | 685 | | Vector3d start = default, |
| | | 686 | | Vector3d end = default, |
| | | 687 | | Vector3d pointA = default, |
| | | 688 | | Vector3d pointB = default, |
| | | 689 | | bool hasPointA = false, |
| | | 690 | | bool hasPointB = false, |
| | | 691 | | Vector3d vector = default, |
| | | 692 | | Fixed64 scalarA = default, |
| | | 693 | | Fixed64 scalarB = default, |
| | | 694 | | int dataA = 0, |
| | | 695 | | int dataB = 0, |
| | | 696 | | bool hit = false) |
| | | 697 | | { |
| | 997 | 698 | | _events.Add(new GravitasDiagnosticEvent( |
| | 997 | 699 | | _context.FrameCount, |
| | 997 | 700 | | _eventSequence++, |
| | 997 | 701 | | kind, |
| | 997 | 702 | | bodyId, |
| | 997 | 703 | | jointId, |
| | 997 | 704 | | colliderAId, |
| | 997 | 705 | | colliderBId, |
| | 997 | 706 | | ResolveDimension(colliderADimension, colliderAType), |
| | 997 | 707 | | ResolveDimension(colliderBDimension, colliderBType), |
| | 997 | 708 | | colliderAType, |
| | 997 | 709 | | colliderBType, |
| | 997 | 710 | | colliderA2DType, |
| | 997 | 711 | | colliderB2DType, |
| | 997 | 712 | | start, |
| | 997 | 713 | | end, |
| | 997 | 714 | | pointA, |
| | 997 | 715 | | pointB, |
| | 997 | 716 | | hasPointA, |
| | 997 | 717 | | hasPointB, |
| | 997 | 718 | | vector, |
| | 997 | 719 | | scalarA, |
| | 997 | 720 | | scalarB, |
| | 997 | 721 | | dataA, |
| | 997 | 722 | | dataB, |
| | 997 | 723 | | hit)); |
| | 997 | 724 | | } |
| | | 725 | | |
| | | 726 | | private void AddDrawCommand( |
| | | 727 | | GravitasDebugDrawKind kind, |
| | | 728 | | int colliderId = -1, |
| | | 729 | | ColliderType colliderType = ColliderType.None, |
| | | 730 | | GravitasColliderDimension colliderDimension = GravitasColliderDimension.None, |
| | | 731 | | ColliderType2D collider2DType = ColliderType2D.None, |
| | | 732 | | Vector3d start = default, |
| | | 733 | | Vector3d end = default, |
| | | 734 | | Vector3d center = default, |
| | | 735 | | Vector3d halfExtents = default, |
| | | 736 | | Vector3d pointA = default, |
| | | 737 | | Vector3d pointB = default, |
| | | 738 | | Vector3d pointC = default, |
| | | 739 | | FixedQuaternion rotation = default, |
| | | 740 | | Fixed64 radius = default, |
| | | 741 | | Fixed64 axisLength = default, |
| | | 742 | | Fixed64 height = default, |
| | | 743 | | GravitasDiagnosticColor color = default) |
| | | 744 | | { |
| | 383 | 745 | | _drawCommands.Add(new GravitasDebugDrawCommand( |
| | 383 | 746 | | _context.FrameCount, |
| | 383 | 747 | | _drawSequence++, |
| | 383 | 748 | | kind, |
| | 383 | 749 | | colliderId, |
| | 383 | 750 | | ResolveDimension(colliderDimension, colliderType), |
| | 383 | 751 | | colliderType, |
| | 383 | 752 | | collider2DType, |
| | 383 | 753 | | start, |
| | 383 | 754 | | end, |
| | 383 | 755 | | center, |
| | 383 | 756 | | halfExtents, |
| | 383 | 757 | | pointA, |
| | 383 | 758 | | pointB, |
| | 383 | 759 | | pointC, |
| | 383 | 760 | | rotation, |
| | 383 | 761 | | radius, |
| | 383 | 762 | | axisLength, |
| | 383 | 763 | | height, |
| | 383 | 764 | | color)); |
| | 383 | 765 | | } |
| | | 766 | | |
| | | 767 | | private static GravitasColliderDimension ResolveDimension( |
| | | 768 | | GravitasColliderDimension dimension, |
| | | 769 | | ColliderType colliderType) |
| | | 770 | | { |
| | 2377 | 771 | | if (dimension != GravitasColliderDimension.None) |
| | 291 | 772 | | return dimension; |
| | 2086 | 773 | | if (colliderType != ColliderType.None) |
| | 1357 | 774 | | return GravitasColliderDimension.ThreeD; |
| | 729 | 775 | | return GravitasColliderDimension.None; |
| | | 776 | | } |
| | | 777 | | |
| | 272 | 778 | | private static Vector3d ToDiagnosticVector(Vector2d value) => new(value.X, Fixed64.Zero, value.Y); |
| | | 779 | | } |