diff --git a/.github/workflows/golangci_lint.yml b/.github/workflows/golangci_lint.yml index 363bfff..ae65987 100644 --- a/.github/workflows/golangci_lint.yml +++ b/.github/workflows/golangci_lint.yml @@ -1,6 +1,6 @@ name: Golangci-lint -on: +on: push jobs: @@ -11,8 +11,8 @@ jobs: - uses: actions/checkout@v4 - uses: actions/setup-go@v5 with: - go-version: stable + go-version-file: go.mod - name: golangci-lint - uses: golangci/golangci-lint-action@v6.1.1 + uses: golangci/golangci-lint-action@v8 with: - version: v1.64.5 \ No newline at end of file + version: v2.7.2 diff --git a/ALGORITHMS.md b/ALGORITHMS.md index 00a83b1..3a3dabf 100644 --- a/ALGORITHMS.md +++ b/ALGORITHMS.md @@ -1,911 +1,299 @@ -# Feather Physics Engine - Algorithm Deep Dives - -This document provides detailed explanations of the core algorithms used in Feather: GJK, EPA, manifold generation, and the XPBD constraint solver. - -## Table of Contents - -1. [GJK Algorithm](#gjk-algorithm-gilbert-johnson-keerthi) -2. [EPA Algorithm](#epa-algorithm-expanding-polytope-algorithm) -3. [Manifold Generation](#manifold-generation-sutherland-hodgman-clipping) -4. [Contact Constraint Solver](#contact-constraint-solver-xpbd) -5. [References & Resources](#references--resources) - ---- - -## GJK Algorithm (Gilbert-Johnson-Keerthi) - -The GJK algorithm detects collisions between convex shapes by operating on their **Minkowski difference**. - -### High-Level Explanation - -**Core Idea**: Two convex shapes A and B overlap if and only if their Minkowski difference (A - B) contains the origin. - -**Minkowski Difference**: A - B = {a - b | a ∈ A, b ∈ B} - -**GJK Strategy**: -1. Build a simplex (point, line, triangle, or tetrahedron) within the Minkowski difference -2. Check if the simplex contains the origin -3. If yes → collision detected -4. If no → refine the simplex toward the origin -5. Repeat until either origin is contained or shapes are proven separated - -### Why This Works - -- We never compute the full Minkowski difference (expensive!) -- Only query **support points**: the farthest point in a given direction -- Support points are easy to compute for most shapes -- Simplex refinement converges quickly (typically 3-6 iterations) - -### Algorithm Pseudocode - -``` -function GJK(shapeA, shapeB): - // Initialize with arbitrary direction - direction = (1, 0, 0) - - // Get first support point in Minkowski difference - support = Support(shapeA, direction) - Support(shapeB, -direction) - simplex = [support] - - // Search direction toward origin - direction = -support - - while true: - // Get new support point - support = Support(shapeA, direction) - Support(shapeB, -direction) - - // If we didn't pass the origin, shapes are separated - if dot(support, direction) < 0: - return NO_COLLISION - - // Add point to simplex - simplex.add(support) - - // Check if simplex contains origin - if NearestSimplex(simplex, direction): - return COLLISION - - // Otherwise, continue with refined simplex and new direction -``` - -### Simplex Evolution: Dimension Cases - -GJK builds up a simplex incrementally: - -#### Case 1: Point Simplex (1 vertex) -``` - origin - O - / - / - A -``` -**Action**: Search toward origin from point A -**Next Direction**: -A - -#### Case 2: Line Simplex (2 vertices) -``` - origin - O - /| - / | - B--A -``` -**Action**: Check if origin is in Voronoi region of line AB -- If yes: Keep both points, search perpendicular to line toward origin -- If no: Keep only closest point (A or B), search toward origin - -#### Case 3: Triangle Simplex (3 vertices) -``` - C - /|\ - / | \ - / O \ (origin inside) - / | \ - B----+----A -``` -**Action**: Check which Voronoi region contains origin -- Inside triangle: Check if origin is above/below triangle plane -- Outside: Reduce to closest edge - -#### Case 4: Tetrahedron Simplex (4 vertices) -``` - D - /|\ - / | \ - / (O) \ (origin inside) - / | \ - C----+----B - \ | / - \ | / - \ | / - \|/ - A -``` -**Action**: Check if origin is inside tetrahedron -- Yes: **COLLISION DETECTED** -- No: Reduce to closest face (triangle) - -### Visual Example: 2D Collision - -``` -Step 0: Initial search direction - A ┌────┐ - │ │ - └────┘ - ┌────┐ - │ │ B - └────┘ - direction → - -Step 1: Get first support point - A ┌────┐ - │ │•─→ S1 (rightmost of A - leftmost of B) - └────┘ - ┌────┐ - │ │ B - └────┘ - -Step 2: Direction toward origin - S1 - • - ←─ direction (toward origin at O) - O - -Step 3: Get second support point - S1 - • - / - / S2 - • - O (origin) - -Step 4: Build line simplex - S1 - • - /| ← origin in Voronoi region - / | O - •--+ - S2 - -Step 5: Search perpendicular to line - S1 - ↑ • - │ / - direction / - │ / - │ • S2 - -Step 6: Get third support point - S3 - • - S1 - • - /| - / | O ← origin inside triangle - / | - •---+ - S2 - -Result: COLLISION (origin contained in simplex S1-S2-S3) -``` - -### Implementation Details (gjk/gjk.go) - -**Key Functions**: - -1. **`GJK(bodyA, bodyB *actor.RigidBody) bool`** - - Main entry point - - Returns true if collision detected - - Iterates until collision or separation proven - -2. **`getSupport(bodyA, bodyB, direction)`** - - Computes support point in Minkowski difference - - `supportA - supportB` where points are in opposite directions - -3. **`nearestSimplex(simplex, direction)`** - - Core simplex refinement logic - - Handles 1D (line), 2D (triangle), 3D (tetrahedron) cases - - Updates search direction toward origin - - Returns true if origin contained - -4. **`handleLineCase()`, `handleTriangleCase()`, `handleTetrahedronCase()`** - - Dimension-specific logic - - Voronoi region tests - - Simplex reduction - -### Edge Cases & Optimizations - -**Early Exit**: If `dot(support, direction) < 0`, the support point didn't cross the origin → shapes are separated - -**Degenerate Simplices**: If simplex becomes too small or flat, algorithm may stall. Implementation handles this with epsilon comparisons. - -**Optimization Opportunities** (marked in code): -- Reuse backing arrays for simplex vertices -- Cache previous support points -- Warm-start with previous frame's simplex - -### Limitations - -- **Convex shapes only**: GJK requires convexity -- **No penetration depth**: Only detects IF collision, not HOW MUCH -- **Numerical precision**: Very deep penetrations may fail - ---- - -## EPA Algorithm (Expanding Polytope Algorithm) - -EPA computes **penetration depth** and **contact normal** for overlapping convex shapes. It's always run after GJK detects a collision. - -### High-Level Explanation - -**Input**: Final simplex from GJK (tetrahedron containing origin in Minkowski difference) - -**Output**: -- Penetration depth (how far shapes overlap) -- Contact normal (direction to separate them) - -**Strategy**: -1. Start with GJK's final simplex as initial polytope -2. Find the face of the polytope closest to the origin -3. Expand the polytope in that direction -4. Repeat until convergence or max iterations - -**Result**: The closest face to origin gives us: -- **Normal**: Face normal = separation direction -- **Depth**: Distance from origin to face = penetration depth - -### Why This Works - -The Minkowski difference A - B is a convex polytope. The closest point on this polytope to the origin tells us the **Minimum Translation Vector** (MTV) to separate the shapes. - -EPA builds this polytope incrementally, always expanding toward the origin until it can't get any closer. - -### Algorithm Pseudocode - -``` -function EPA(simplex, shapeA, shapeB): - // Initialize polytope with GJK's final simplex - polytope = Polytope(simplex) - - for iteration = 0 to MAX_ITERATIONS: - // Find face closest to origin - face = polytope.GetClosestFace() - - // Get support point in direction of face normal - support = Support(shapeA, face.normal) - Support(shapeB, -face.normal) - - // Distance from origin to support point - distance = dot(support, face.normal) - - // Convergence check - if abs(distance - face.distance) < TOLERANCE: - // Found closest face! - return { - normal: face.normal, - depth: distance - } - - // Expand polytope by adding support point - // Remove faces that can "see" the new point - // Add new faces connecting to the new point - polytope.Expand(support) - - // Max iterations reached - return ERROR -``` - -### Polytope Expansion: Visual Example - -``` -Step 0: Initial tetrahedron from GJK - D - /|\ - / | \ - / O \ (O = origin) - / | \ - C----+----B - \ | / - \ | / - \ | / - \|/ - A - -Step 1: Find closest face (say, ABC) - D - /|\ - / | \ - / O \ - / [ABC] \ ← closest face - C----+----B - \ | / - \ | / - \|/ - A - -Step 2: Get support point in face normal direction - D - /|\ - / | \ - / O \ - / | \ - C----+----B - \ | / \ - \ | / \ S (new support point) - \|/ \ - A - -Step 3: Expand polytope - D - /|\ - / | \____ - / O \ \ - / | \ S ← new vertex - C----+----B / - \ | / / - \ | / / - \|/ / - A - -Step 4: Remove old faces that can see S -Step 5: Add new faces connecting S to remaining edges -Step 6: Repeat... - -Convergence: When new support point is on (or very close to) closest face -``` - -### Face Management (epa/face.go) - -**Face Structure**: -```go -type Face struct { - vertices [3]mgl64.Vec3 // Triangle vertices - normal mgl64.Vec3 // Outward normal - distance float64 // Distance to origin -} -``` - -**Key Operations**: -1. **ComputeNormal**: Cross product of edges, pointing away from polytope center -2. **ComputeDistance**: `dot(vertex, normal)` gives signed distance -3. **CanSee**: Checks if point is "in front of" face (for removal during expansion) - -### Degenerate Case Handling - -#### Problem 1: Coplanar Faces -When support point lands exactly on closest face → distance doesn't change - -**Solution**: Convergence tolerance check: `abs(newDistance - oldDistance) < EPSILON` - -#### Problem 2: Polytope Collapse -Numerical errors can cause polytope to become degenerate (zero-volume) - -**Solution**: -- Validate all faces have non-zero area -- Check that normals point outward -- Reject support points too close to existing vertices - -#### Problem 3: No Progress -Polytope fails to expand (support points don't add new information) - -**Solution**: Return error after MAX_ITERATIONS (typically 100) - -### Implementation Details (epa/epa.go) - -**Key Constants**: -```go -const ( - EPAMaxIterations = 100 // Prevent infinite loops - EPATolerance = 1e-6 // Convergence threshold - DefaultCompliance = 1e-9 // Soft constraint stiffness -) -``` - -**Key Functions**: - -1. **`EPA(simplex, bodyA, bodyB) (normal, depth, error)`** - - Main EPA algorithm - - Returns contact normal and penetration depth - - Error if convergence fails - -2. **`getClosestFace(faces []Face) Face`** - - Linear search for face nearest to origin - - Could optimize with priority queue - -3. **`expandPolytope(polytope, support)`** - - Core polytope expansion logic - - Removes faces visible from support point - - Adds new faces connecting support to horizon edges - -4. **`buildHorizon(faces, support) []Edge`** - - Finds edges between visible and non-visible faces - - These edges form the "horizon" around the new point - -### Parameter Tuning - -#### MAX_ITERATIONS (currently 100) -- **Lower** (20-50): Faster but may fail on complex shapes -- **Higher** (100-200): More robust but slower -- **Typical convergence**: 5-15 iterations for simple shapes - -#### TOLERANCE (currently 1e-6) -- **Lower** (1e-8): More precise penetration depth -- **Higher** (1e-4): Faster convergence but less accurate -- **Trade-off**: Precision vs speed - -#### DEFAULT_COMPLIANCE (currently 1e-9) -- **Lower** (1e-10): Stiffer contacts, less penetration, more jitter -- **Higher** (1e-6): Softer contacts, more penetration, less jitter -- **See PHYSICS_GUIDE.md** for tuning guidelines - -### Limitations - -- **Convergence not guaranteed**: Degenerate cases may fail (returns error) -- **Computational cost**: O(n) where n = polytope faces (typically 20-50) -- **Numerical precision**: Very shallow or very deep penetrations can be problematic - ---- - -## Manifold Generation (Sutherland-Hodgman Clipping) - -After EPA gives us the collision normal and depth, we need to find **contact points** where the shapes touch. Multiple contact points create a stable "manifold." - -### Why Multi-Point Contacts? - -**Single Point Contact Problems**: -- Unstable (boxes would balance on corners) -- Incorrect torque (rotation around wrong axis) -- Jittery (contact point jumps between features) - -**Multi-Point Manifold Benefits**: -- Stability (objects rest naturally) -- Realistic torque distribution -- Smooth contact transitions - -### High-Level Explanation - -**Goal**: Find 1-4 contact points distributed across the contact area - -**Strategy**: -1. Identify reference face on one shape (most aligned with contact normal) -2. Identify incident face on other shape (most opposing the contact normal) -3. Clip incident face against reference face's side planes (Sutherland-Hodgman) -4. Keep points behind reference face (penetrating) -5. Reduce to best 4 points if more remain - -### Sutherland-Hodgman Algorithm - -Classic polygon clipping algorithm that clips one polygon against a plane. - -**Pseudocode**: -``` -function ClipPolygon(polygon, plane): - output = [] - - for each edge (A, B) in polygon: - if A is behind plane: - output.add(A) - if B is in front of plane: - // Edge crosses plane, add intersection point - intersection = IntersectEdgePlane(A, B, plane) - output.add(intersection) - else: // A is in front of plane - if B is behind plane: - // Edge crosses plane, add intersection point - intersection = IntersectEdgePlane(A, B, plane) - output.add(intersection) - output.add(B) - - return output -``` - -**Applied to Contact Manifold**: -1. Start with incident face vertices (4 points for box) -2. Clip against reference face's 4 side planes -3. Each clip may reduce point count or add intersection points -4. Final points are those inside all planes = contact region - -### Visual Example: Box-Box Contact - -``` -Top view of two boxes colliding: - -Box A (reference face): - ┌─────────┐ - │ A │ - │ │ - └─────────┘ - -Box B (incident face): - ┌─────────┐ - │ B │ - │ │ - └─────────┘ - -Step 1: Identify faces -Reference face: Bottom of A (normal points down) -Incident face: Top of B (normal points up, most opposing) - -Step 2: Clip incident face against reference face side planes - -Clip against left plane: - ┌─────────┐ - │ B │ │ → Keep right portion - │ │ │ - └─────────┘ - ↑ - left plane of A - -Clip against right plane: - ┌──│──┐ - │ B │ → Keep left portion - │ │ - └────│┘ - ↑ - right plane of A - -(Repeat for top/bottom planes) - -Step 3: Result - 4 contact points at corners of overlap region - ┌─────────┐ - │ • ─ ─ • │ - │ │ B │ │ - │ • ─ ─ • │ - └─────────┘ - (4 contacts) -``` - -### Special Cases - -#### Sphere-Sphere Contact -No need for clipping - analytical solution: -``` -contactPoint = centerA + (centerB - centerA).normalize() * radiusA -``` -Single contact point at the midpoint between surface points. - -#### Sphere-Box Contact -Find closest feature on box (face, edge, or corner), project sphere center: -``` -closestPointOnBox = ClampToBox(sphereCenter) -contactPoint = closestPointOnBox -``` - -#### Plane-Box Contact -Project box corners onto plane: -``` -for each corner in box: - if distance(corner, plane) < threshold: - contactPoints.add(corner) -``` -Can generate 1-4 contact points depending on box orientation. - -### Contact Point Reduction - -If clipping generates >4 points (rare but possible): - -**Strategy**: Keep 4 most well-distributed points -1. Find point pair with maximum distance → keep both -2. Find point farthest from line connecting first pair → keep -3. Find point farthest from triangle of first 3 → keep - -**Why 4 points?** Balance between stability and performance: -- <4: May not be stable (torque errors) -- =4: Optimal for most scenarios -- >4: Diminishing returns, more solver cost - -### Implementation Details (epa/manifold.go) - -**Key Functions**: - -1. **`GenerateManifold(bodyA, bodyB, normal, depth) []ContactPoint`** - - Main entry point - - Delegates to shape-specific logic - - Returns 1-4 contact points - -2. **`clipFaceAgainstPlane(face, plane) []Vec3`** - - Sutherland-Hodgman core implementation - - Clips polygon vertices against a plane - - Returns clipped polygon - -3. **`findReferenceAndIncidentFaces(bodyA, bodyB, normal)`** - - Identifies which face to use as reference (most aligned with normal) - - Identifies incident face (most opposing normal) - -4. **`reduceContactPoints(points []Vec3) []ContactPoint`** - - Reduces >4 points to best 4 - - Greedy algorithm for maximum distribution - -### Manifold Quality Metrics - -**Good Manifold**: -- 3-4 contact points for large flat contacts -- 1-2 points for edge/corner contacts -- Well-distributed (not clustered) -- Consistent between frames (no jitter) - -**Poor Manifold**: -- All points clustered at one corner -- Point count varies wildly between frames -- Points far from actual contact region - ---- - -## Contact Constraint Solver (XPBD) - -The constraint solver resolves contacts to prevent penetration and apply restitution (bounciness). Feather uses **XPBD** (Extended Position-Based Dynamics). - -### XPBD Overview - -**Key Idea**: Solve constraints directly in position space, then derive velocities - -**Two-Phase Solving**: -1. **Position Correction**: Move bodies apart to fix penetration -2. **Velocity Correction**: Apply restitution (bounce) and friction (future) - -### Position Constraint - -**Goal**: Eliminate penetration (depth = 0) - -**Constraint**: `C = dot(pB - pA, normal) >= 0` -- Where `pA`, `pB` are contact points on each body -- `normal` points from A to B -- `C < 0` means penetration - -**XPBD Position Correction Formula**: -``` -Δλ = -(C + compliance * λ) / (wA + wB + compliance) -pA += -Δλ * normal * wA -pB += Δλ * normal * wB -λ += Δλ -``` - -Where: -- `C`: Constraint violation (penetration depth, negative) -- `λ`: Lagrange multiplier (accumulated impulse) -- `compliance`: Soft constraint parameter (inverse stiffness) -- `wA, wB`: Inverse masses (0 for static bodies) - -**Compliance Interpretation**: -- `compliance = 0`: Infinitely stiff (hard constraint) -- `compliance > 0`: Soft constraint (allows some penetration) -- Typical value: `1e-9` (very stiff but numerically stable) - -### Velocity Constraint (Restitution) - -**Goal**: Apply bounciness at contact - -**Restitution Coefficient** `e`: -- `e = 0`: Perfectly inelastic (no bounce) -- `e = 1`: Perfectly elastic (full bounce) -- Typical: `0.3-0.8` for most materials - -**Relative Velocity**: -``` -vRel = dot(vB - vA, normal) -``` - -**Restitution Formula**: -``` -if vRel < 0: // Bodies approaching - targetVel = -e * vRel // Reverse with restitution - Δv = (targetVel - vRel) / (wA + wB) - vA += -Δv * normal * wA - vB += Δv * normal * wB -``` - -### XPBD Substep Approach (NOT Iterations) - -**CRITICAL**: XPBD uses **substeps** with **ONE solver pass per substep**, NOT multiple iterations. - -**Why substeps instead of iterations?** -- Each substep uses a smaller timestep (h = dt / substeps) -- Better integration accuracy → fewer convergence issues -- Simpler: no iteration loops needed -- More stable for stiff constraints - -**XPBD Pattern** (what Feather uses): -``` -for substep = 0 to NUM_SUBSTEPS: - h = dt / NUM_SUBSTEPS - - // Apply forces & integrate velocities - - // Solve constraints - SINGLE pass only! - for each contact: - SolvePositionConstraint(contact, h) - - for each contact: - SolveVelocityConstraint(contact, h) - - // Integrate positions -``` - -**Typical Substep Counts**: -- Standard scenes: 1-2 substeps -- Fast objects / tall stacks: 4 substeps -- Extreme precision: 8+ substeps - -**Contrast with traditional solvers**: -- **Traditional PBD/SI**: 1 step, 10-20 iterations -- **XPBD**: 2-4 substeps, 1 iteration each -- Same total solver passes, but better accuracy! - -### Mathematical Derivation (Simplified) - -**Starting Point**: Newton's law `F = ma` - -**Constraint Force**: `F = λ * normal` (along contact normal) - -**Position-Based Dynamics**: Instead of forces, directly compute position changes - -**XPBD Extension**: Add compliance for soft constraints -- Standard PBD: `Δλ = -C / (wA + wB)` -- XPBD: `Δλ = -(C + α * λ) / (wA + wB + α)` -- Where `α = compliance / dt²` - -**Benefit**: Compliance makes constraints "soft" without instability - -### Implementation Details (constraint/contact.go) - -**Contact Constraint Structure**: -```go -type ContactConstraint struct { - bodyA, bodyB *actor.RigidBody - normal mgl64.Vec3 - depth float64 - points []mgl64.Vec3 // Contact manifold - lambda float64 // Accumulated impulse -} -``` - -**Key Functions**: - -1. **`SolvePosition(dt float64, compliance float64)`** - - Applies position correction to separate bodies - - Uses XPBD formula with compliance - - Updates body positions directly - -2. **`SolveVelocity(dt float64, restitution float64)`** - - Applies velocity correction for restitution - - Only affects separating velocity (no stick) - - Updates body velocities directly - -3. **`Solve(dt float64) error`** - - Main entry point (convenience wrapper) - - Calls position then velocity solver - - Uses default compliance and combined restitution - -### Parameter Tuning - -#### Compliance -- **Too low** (<1e-10): Stiff, jittery, potential instability -- **Too high** (>1e-6): Soft, excessive penetration, "mushy" -- **Recommended**: `1e-9` to `1e-8` for rigid bodies - -#### Restitution -- **0.0**: Clay, putty (no bounce) -- **0.3-0.5**: Wood, concrete (typical solids) -- **0.7-0.9**: Rubber ball (bouncy) -- **0.95+**: Super ball (very bouncy) - -**Combined Restitution**: When two materials collide -- Average: `(eA + eB) / 2` -- Maximum: `max(eA, eB)` ← **Feather uses this** -- Multiply: `eA * eB` - -#### Substeps -- **Standard**: 1-2 (sufficient for most scenes) -- **Complex**: 4 (for tall stacks, fast objects) -- **Trade-off**: Accuracy vs performance (each substep = 1 full solver pass) - -### Solver Stability - -**Sources of Instability**: -1. Very small timesteps (dt < 1ms) -2. Very high mass ratios (heavy vs light) -3. Very stiff constraints (low compliance) -4. Too few substeps - -**Stability Techniques in XPBD**: -1. **Compliance**: Softens constraints (numerical damping) -2. **Substeps**: Smaller timesteps improve integration -3. **Warm starting**: Reuse λ from previous frame (future) -4. **Mass clamping**: Limit effective mass ratios - ---- - -## References & Resources - -### Academic Papers - -**GJK Algorithm**: -- Gilbert, Johnson, Keerthi: "A Fast Procedure for Computing the Distance Between Complex Objects in Three-Dimensional Space" (1988) -- Van den Bergen: "Efficient Collision Detection of Complex Deformable Models using AABB Trees" (1997) - -**EPA Algorithm**: -- Van den Bergen: "Proximity Queries and Penetration Depth Computation on 3D Game Objects" (2001) - -**XPBD Solver**: -- Macklin, Müller, Chentanez: "XPBD: Position-Based Simulation of Compliant Constrained Dynamics" (2016) -- Müller, Heidelberger, Hennix, Ratcliff: "Position Based Dynamics" (2007) - -**Contact Manifolds**: -- Catto: "Contact Manifolds" (GDC 2007) - Box2D approach -- Gregorius: "Robust Contact Creation for Physics Simulations" (GDC 2015) - -### Online Resources - -**Tutorials**: -- Casey Muratori's Handmade Hero (GJK explanation): https://www.youtube.com/watch?v=Qupqu1xe7Io -- Winter Dev: GJK & EPA visualization: https://blog.winter.dev/2020/gjk-algorithm/ -- Randy Gaul's Game Physics series: https://www.randygaul.net/ - -**Interactive Demos**: -- GJK Algorithm Visualizer: http://www.cs.ox.ac.uk/people/stephen.cameron/distances/ -- EPA 2D Demo: https://observablehq.com/@esperanc/gjk-and-epa - -**Reference Implementations**: -- **Box2D** (C++): Industry standard 2D physics - excellent collision code -- **Bullet Physics** (C++): 3D physics with GJK/EPA -- **ReactPhysics3D** (C++): Clean, educational implementation - -### Books - -- **"Real-Time Collision Detection"** by Christer Ericson - - Chapter 5: Basic Primitive Tests - - Chapter 9: Convex Objects (GJK) - -- **"Game Physics Engine Development"** by Ian Millington - - Chapters on collision detection and resolution - -- **"Physics for Game Developers"** by David M. Bourg & Bryan Bywalec - - Practical physics implementation - -### Code Study Recommendations - -For deeper understanding, study these files in order: - -1. **gjk/gjk.go**: Start here - GJK is the foundation -2. **epa/epa.go**: EPA builds on GJK's output -3. **epa/face.go**: Understand polytope face management -4. **epa/manifold.go**: See how contact points are generated -5. **constraint/contact.go**: Finally, constraint solving - -Each file has been documented with inline comments referencing this guide. - ---- - -## Summary - -### Algorithm Selection Rationale - -| Algorithm | Why Chosen | Alternatives | -|-----------|-----------|-------------| -| **GJK** | Versatile for all convex shapes, fast convergence | SAT (less general) | -| **EPA** | Precise penetration depth from GJK simplex | MPR (less precise) | -| **Sutherland-Hodgman** | Robust polygon clipping, multi-point contacts | Discrete sampling (less accurate) | -| **XPBD** | Stable stacking, intuitive compliance parameter | Sequential Impulse (less stable), Penalty methods (hard to tune) | - -### Computational Complexity Summary - -| Algorithm | Typical Count | Worst Case | Per Frame Cost | -|-----------|--------------|------------|----------------| -| GJK | 3-6 iterations | 20 iterations | O(1) per pair | -| EPA | 5-15 iterations | 100 iterations | O(1) per collision | -| Manifold | 1 pass | 1 pass | O(1) per collision | -| Position Solver | 1 pass/substep | 1 pass/substep | O(contacts * substeps) | -| Velocity Solver | 1 pass/substep | 1 pass/substep | O(contacts * substeps) | - -### Performance Tips - -1. **Minimize contact count**: Use broad phase effectively -2. **Tune substeps**: Start with 1-2, increase only if needed -3. **Adjust compliance**: Higher = faster but softer -4. **Cache manifolds**: Reuse contact points between frames (future optimization) -5. **Warm start solver**: Reuse λ from previous frame (future optimization) - ---- - -For architectural context and design decisions, see [ARCHITECTURE.md](ARCHITECTURE.md). -For practical parameter tuning and usage examples, see [PHYSICS_GUIDE.md](PHYSICS_GUIDE.md). +# Feather - Algorithms + +1. [GJK Algorithm](#gjk-algorithm) +2. [EPA Algorithm](#epa-algorithm) +3. [Contact points](#contact-points) +4. [Solver](#solver) +5. [Joints](#joints) +6. [Heightfield](#heightfield) +7. [Continuous collision](#continuous-collision) + +## Broad phase + +Two dynamic AABB trees (Catto, "Dynamic Bounding Volume Hierarchies", GDC 2019; the `b2DynamicTree` of Box2D, the +`btDbvt` of Bullet), one per kind of body as in Box2D v3: one for the static bodies, one for the dynamic bodies, awake +or asleep. A dynamic body is stored with its AABB enlarged by `AABBMargin` (0.1 m, as Box2D v2.4): while it moves +inside, the tree is not touched; a sleeping body never touches it. A leaf is inserted next to the sibling which +enlarges the tree the least (the surface area heuristic), found down the tree by the least reachable cost; the +ancestors are then refitted and each tries the rotation of a child with a grandchild which shrinks it the most (the +tree rotations of Catto's talk). The planes and the heightfields are not in the trees: they are tested against every +awake body. + +The pairs of bodies whose stored AABBs overlap are kept from a step to the next (the persistent pairs of Box2D v3): +only a body put in a tree since the last step (a dynamic body out of its enlarged AABB, a static body moved by the +game, a body added) queries the trees for its pairs, and a pair is dropped when its stored AABBs no longer overlap. +A resting scene costs nothing, an awake one only pays for the bodies which left their enlarged AABB (2000 bodies +settling: 1.6 ms for a traversal of the trees against each other, 0.3 ms with the pairs kept). The pairs of the step +are those whose exact AABBs overlap, with an awake dynamic body, sorted by the index of their first body (a counting +sort): the list is the same as a search from scratch, and the same as the former uniform grid gave, so the solver keeps +its order and its results bit for bit. + +## Pair cache and warm start + +The contacts of a pair are kept with the pair of the broad phase. If a body moved less than 1 mm and turned less than +2° relative to the other since their contact points were computed, the points are moved with the bodies and their +separations measured again, instead of running GJK/EPA (the body pair cache of Jolt, with its thresholds). The +impulses of the previous step warm start the new points: each point takes the impulses of the closest previous point +in the local space of body A, within 2 cm (as the contact cache of Jolt; Box2D matches the points by feature id). + +## GJK Algorithm +GJK tests if two convex shapes overlap: they overlap if their Minkowski difference `A - B` contains the origin. +The shapes only need a `Support(direction)` function, the farthest point in a direction. + +```` +direction ← center of B - center of A +simplex ← [Support(A - B, direction)] +direction ← -simplex[0] +loop + point ← Support(A - B, direction) + if point · direction <= 0 then return false // the origin can't be reached + simplex.add(point) + if simplex contains the origin then return true // only a tetrahedron can + simplex ← feature of the simplex closest to the origin, direction ← towards the origin +end +```` + +- Each vertex keeps its support points on A and B, EPA uses them for the witness points. +- With a margin, A is inflated by a sphere: shapes closer than the margin overlap. EPA then gives `depth = margin - distance`. +- When the shapes are only touching (the origin on the simplex), the simplex is completed into a tetrahedron for EPA. + +## EPA Algorithm +EPA starts from the tetrahedron of GJK, and grows it towards the surface of the Minkowski difference: + +```` +polytope ← tetrahedron of GJK +loop + face ← face of the polytope closest to the origin + point ← Support(A - B, face.normal) + if point · face.normal - face.distance < tolerance then + return face.normal, face.distance, witness points + remove the faces visible from point, close the hole with new faces from the horizon to point +end +```` + +- The tolerance is 1e-7 m: it is the error on the penetration depth. +- **Ties**: the faces less deep than the closest one by less than 1 µm are as deep (a cube overlapping 2 others by the + same amount). They all converge, then EPA takes the first in a fixed order in the local space of A, not the one the + rounding found first: a scene moved by 1 µm or 100 km keeps the same normals, and the same motion. The triangles of a + same feature (normals within 1°: a face, a rounded surface) are not tied, the closest is kept. +- The witness points come from the barycentric coordinates of the origin projected on the closest face. +- Tested against exact solutions: SAT for box-box, closest point for sphere-box (see `epa/epa_test.go`). + +## Contact points +From the normal of EPA, each body gives the feature facing the other body (a face for a box, a line or a point for a capsule): + +- **Face contact**: a face is aligned with the normal (0.5°). The other feature is clipped by the side planes of this face (Sutherland-Hodgman). +- **Parallel edges**: a box on an edge, a capsule along an edge. One edge is clipped by the other: 2 points. +- **Otherwise** (crossing edges, a vertex, a sphere): the witness point of EPA. + +The deepest point has the separation of EPA, the other points are higher along the normal. +The points closer than the margin are kept, 4 at most: the deepest, the farthest from it, then the points adding the most +area. A point replaces the best one only if it is clearly better, deeper by 1 µm or a score higher by 1/0.95 (the pecking +order of Box3D): the choice between points as good doesn't depend on the rounding. +A box touches a plane (or the face of a triangle) with its supporting face, the face the most opposed to the normal +(as the incident face of Jolt): the corners behind it are never candidates. The contacts are reduced the same way. + +Spheres and capsules don't use EPA: their contact comes from the closest points of their segments (Ericson 5.1.9). +Parallel capsules get 2 points. +Against the other shapes, a rounded shape is its **core** with a radius (the collision margin of Bullet, the convex +radius of Jolt, the rounded polygons of Box2D v3): a point +for a sphere, a segment for a capsule. GJK gives the distance and the closest points of the cores (exact against a +polytope, in 3 or 4 iterations), the radii and the margin are added along their direction, and the contact points are +clipped as above. EPA runs on the full shapes only if the cores overlap (the center of a sphere inside a box): on the +rounded shape, it would tessellate it (13 iterations and 7 µs for a sphere against a box, against 1.8 µs). + +## Solver +TGS Soft, from Box2D v3 (Erin Catto, [Solver2D](https://box2d.org/posts/2024/02/solver2d/)), in 3D. + +### Prepare (once per step) +For each contact point: the anchors `rA`, `rB` (from the centers of mass), the effective masses along the normal and both tangents, +the impulses of the previous step (warm starting), and the relative normal velocity (for the restitution). + +### Substeps +```` +for numSubsteps do + IntegrateVelocities(); // gravity, forces, gyroscopic torque, damping + WarmStart(); // apply the accumulated impulses + Push(); // soft constraint + IntegratePositions(); + Relax(); // rigid constraint, then friction +end +Restitution(); +```` + +The contact points are not computed again during the substeps: the separation is updated from the motion of both anchors. +```` +separation = baseSeparation + (ΔpB + ΔqB*coreB - ΔpA - ΔqA*coreA) · normal +```` +The anchors are the points on the surface of each body, without the radius of the rounded shapes +(`core = surface ± radius * normal`): the center of a sphere, the axis of a capsule, the corner of a box. +A rolling sphere turns its surface, not its center: with the point of its surface, its contacts would open while it +rolls, and it would sink in the wall in front of it. + +The rotation of a body is limited to `MaxRotation` (π/4) per substep. Box2D limits it per step (with an option for the +wheels), and keeps the anchors fixed during the step. Feather lets the bodies turn faster, so the lever arms of the +contacts turn with the bodies before each `Relax` (`turnAnchors`), when a body turned more than 0.01 rad since the +beginning of the step: a tumbling capsule at 30 rad/s turns by 0.5 rad per step, it would otherwise be pushed at the place +of its contact at the beginning of the step, and the solver would see the contact open while it sinks. +The inertia turns with the body too (`I⁻¹ = ΔR I⁻¹start ΔRᵀ`). +To our knowledge, the cores (the idea of the convex radius of Bullet & Jolt, applied to the separation) and `turnAnchors` +are Feather's own: without them, the bodies tumbling on a slope sink by 14 cm (`TestPileLandsWithoutSinking`). + +### Soft constraint +From Erin Catto, [Soft Constraints](https://box2d.org/files/ErinCatto_SoftConstraints_GDC2011.pdf) (GDC 2011): the +overlap is a spring of frequency `ω = 2π * hertz` and damping ratio `ζ`, whatever the mass (`k = m ω²`, `c = 2 m ζ ω`), +integrated implicitly over the substep `h`: +```` +biasRate = k / (c + h k) = ω / (2ζ + h ω) +gamma = m / (h (c + h k)) = 1 / (h ω (2ζ + h ω)) // the softness γ of the paper, times the mass + +Push: + if separation > 0 then bias = separation / h // speculative: can get closer, not further than the gap + else bias = max(biasRate * separation, -ContactSpeed) + λ = -(m (vn + bias) + gamma * λ_total) / (1 + gamma) + λ_total = max(λ_total + λ, 0) +```` +`Relax` solves the same constraint rigid (`gamma = 0`, bias only for the speculative contacts): the spring adds energy. +The joints use the same soft rows. The contacts with a static body are twice as stiff, with half the damping ratio (as +Box3D). +The friction is solved in `Relax` only, after the normals (as Box3D): solved in `Push` before the normals, it pushed the +light bodies out from under a heavy one. + +### Block solver +The points of a contact are solved together, exactly: their accumulated impulses are the solution of the linear +complementarity problem `w = (K + D) λ + r`, `λ ≥ 0`, `w ≥ 0`, `λ w = 0`, found by enumerating the sets of active +points, all of them first (the block solver of Box2D v2, for 4 points). `K` is the mass matrix of the points, `D` their +softness (`gamma K_ii`, the fixed point of the soft row), `r = vn + bias - K λ_total`. +Solved one after the other (Gauss-Seidel, as PhysX, Jolt & Box3D), the first point takes more than its share and turns +the body: a box landing flat on another starts to spin, and lands back on a corner (Box3D: 30 cm of drift). +4 rigid points on a face give 3 independent rows only: the share of the load between the points is not defined. A +proximal term `ε W (λ - λ₀)` (`W` the diagonal of `K`, `ε = 1e-3`) chooses the share closest to the impulses the rows +start from (the proximal point method: Rockafellar 1976; the proximal formulations of contact: Alart & Curnier 1991, +Acary & Brogliato 2008). Repeated at each pass, its bias vanishes. A term towards 0 (the solution of minimum norm) moved +the load between the points at each substep (30 % of it), and a house of cards fell. + +### Friction +Solved in `Relax`, after the normals, at the friction center of the points of the contact (as Box3D, Jolt, and the +friction patches of PhysX), not at each point: +- along both tangents, with their 2x2 mass matrix: the impulse stays in a disk of radius `µ Σ λ_normal`; +- around the normal (the twist): up to `µ Σ (lever arm × λ_normal)`, the lever arm of a point being its distance to the + center. A single point holds no twist. + +The center is the average of the points, weighted by their separation (as Box3D: 1 up to the speculative distance, 0 at +twice). µ is the static friction when the center slides slower than 1 cm/s, the dynamic friction otherwise. + +### Restitution +Applied once after the substeps, for the contacts hitting faster than 1 m/s. The bounce impulse goes towards the +velocity `-e * vn_before` (Newton), and is at most `e` times the impulse which stopped the point, its normal impulse of +the step (Poisson's hypothesis, W. J. Stronge, Impact Mechanics): +```` +λ = max(0, min(-m (vn + e * vn_before), e * λ_step)) +```` +Both are needed: a pile of balls bouncing with `e = 1` gains energy with Newton alone (411 J) or Poisson alone +(3523 J), not with both (`TestRestitutionNeverAddsEnergy`). The bounce uses the velocity before the step: a body not +round and spinning fast can turn its point away before the end of the step, and bounce higher than it fell over +`e = 0.5` (see ARCHITECTURE.md, as documented by Jolt). Newton's law is the one of the game engines (Box2D, Jolt). + +### Gyroscopic torque +`ω × Iω` is integrated implicitly (1 Newton-Raphson iteration in body space), as described by Erin Catto +([GDC 2015](https://box2d.org/files/ErinCatto_NumericalMethods_GDC2015.pdf)). Dropping it removes the tumbling +of long bodies, integrating it explicitly makes them gain energy. + +### Parallel solver +The solver is a Gauss-Seidel: each constraint uses the velocities left by the previous one. To solve in parallel, +the contacts and the joints are colored (Box2D v3, `constraint_graph.c`): each takes the first color where both of its +dynamic bodies are free (the static bodies don't count). The constraints of a color don't share any body, the workers +solve them at the same time. The constraints without a free color (16 colors) are solved first, on a single goroutine. +A contact with a static body never takes the color 0 (as Box2D v3): it is solved after the contacts between dynamic +bodies, the ground has the last word. Solved first, a light body pressed by a heavy one leaves the step moving into +the ground. + +### Default values +| Constant | Value | +|----------|-------| +| `DefaultContactHertz` | 30 Hz, as Box2D v3.1 (x2 against static bodies, capped to 1/8 of the substeps rate) | +| `ContactDampingRatio` | 10 | +| `ContactSpeed` | 3 m/s | +| `RestitutionThreshold` | 1 m/s | +| `SpeculativeDistance` | 2 cm | +| `LinearSlop` | 5 mm | +| `MaxRotation` | π/4 per substep | + +## Joints +The joints are solved like the contacts (as in Box2D v3): warm starting, soft constraints in `Push` (60 Hz, damping ratio 2 +by default), rigid constraints in `Relax`. They are colored with the contacts and solved with them, in parallel; the +articulations below are solved before the colors, on a single goroutine. + +### Articulations +The point constraints (the anchors kept together) of the joints linking dynamic bodies are solved together, exactly, by +tree of joints: `K Δλ = -(ċ + bias)`, `K = J M⁻¹ Jᵀ` the mass matrix of all their anchors, factored from the leaves to the +root (block `LDLᵀ`, the linear time dynamics of Baraff, "Linear-Time Dynamics using Lagrange Multipliers", SIGGRAPH +1996). A joint is eliminated after the joints below it: a chain gives no fill, a body with `d` children a clique of `d` +blocks. The soft spring acts on the whole system, `Δλ = -(K⁻¹ (ċ + bias) + γ λ) / (1 + γ)`. +Solved one by one, the spring of a joint acts on the mass of its own bodies: a ball 670 times heavier than a link +stretched each joint by 7 cm (a chain of 20 m by 1.4 m). Solved together, 0.1 mm (`TestHeavyChainDoesNotStretch`). +- A chain taut between 2 fixed points has a redundant row: the proximal term of the contacts keeps `K` invertible. +- The joints of a net (a loop between dynamic bodies) are solved one by one, as the other rows of the joints (the axes + of the hinges, the limits, the motors, the springs): the tree exact against the joints closing the loops solved alone + converges slowly (a net of 60 x 60 opened by 357 mm instead of 320). +- A link turning by half a radian in a substep (the tip of a whip, 125 rad/s) opens its joint by `r (ω h)² / 2` for a few + steps: the constraints are linear in the velocities. + +Each joint has a frame on each body. The X axis of the frames is the axis of the hinge, and the twist axis of the ball +joint (as in PhysX). + +- **Point** (ball, hinge, fixed): the anchors stay together, 3 rows solved together: + `K = (mA + mB) I - [rA]x IA [rA]x - [rB]x IB [rB]x` +- **Hinge axis**: the X axis of B stays on the X axis of A: 2 rows along the Y & Z axes of A, the error is `xA × xB`. +- **Angle limits** (hinge, twist): like the contacts, speculative above the limit, soft under it. +- **Cone** (ball): the X axis of B seen in the frame A, `p`, must stay in the elliptic cone of the 2 half angles. + `p` moves as `dp/dt = ω × p`: the rate of the cone function `f(p)` is `ω · (p × ∇f)`, so the constraint turns around + `p × ∇f`. This axis is orthogonal to `p`: it doesn't twist B, and it also follows the cone when B slides along its + border (the limit changes with the direction). +- **Twist** (ball): swing-twist decomposition of the rotation of B relative to A. The rate of the twist isn't exactly + `ω · x`: it is measured numerically, to follow the twist when B also swings. +- **Distance**: 1 row along the axis between the anchors, rigid, or a spring with the limits. +- **Fixed & drive**: 3 angular rows, `K = IA + IB`, the error is the rotation vector between the frames. +- **Configurable**: each axis chooses its row. Linear: 1 row along the axis of A (locked, or 2 limits), all locked = the + point. Angular: the twist row, the cone if both swings are limited, else 1 row per swing + (`atan2(-p.z, p.x)` around Y, `atan2(p.y, p.x)` around Z), all locked = the 3 angular rows. + +## Heightfield +The terrain is a grid of heights, split in 2 triangles per cell (along the diagonal from (x, z) to (x+1, z+1)). +The body is tested against the triangles under its AABB, one by one: the grid is cut in blocks of 16x16 cells with +their lowest and highest heights, to skip the blocks far from the body. + +Each triangle is tested with GJK/EPA: +- **Face**: always, the contact of the body with the plane of the triangle (`CollideWithPlane`), limited to the + points above the triangle. On a flat terrain, a body behaves exactly as on a plane. +- **Inner edges**: a body sliding on the terrain must not hit the edges between the triangles. Each edge is active if it + is on a border or a hole, or if it bends down (convex) by more than 5° (like Jolt, `ActiveEdges.h`). A contact on an inactive + edge (or vertex) takes the normal of its triangle. If the body is beside the triangle, above the edge, it keeps the + witness point of EPA, only if no other triangle has a contact with this normal. +- **Active edges** (a ridge, a border): also the contact of EPA, with its normal. A capsule lying across a ridge touches + the ridge, and its ends can fall on both faces. + +The contacts are then grouped by normal: the contacts of triangles with less than 5° between their normals form a patch, +a manifold of 4 points. A body touches the terrain with 8 patches at most (`MaxManifoldsPerPair`): a box in a valley +gets one patch per slope. The patches of the deepest contacts are kept, the others are dropped (like Jolt). + +## Continuous collision +**Speculative contacts**: against a static body, the contacts are created up to `SpeculativeDistance` + the relative +speed of the bodies * dt (the speculative CCD of PhysX, the "Continuous Speculative" mode of Unity): the solver stops the +bodies before they touch. Between 2 dynamic bodies, only up to `SpeculativeDistance` (as Box2D v3): a fast impact is +absorbed by the spring of the contact over a few substeps. A rigid stop in one substep throws the light body of a +sandwich (a heavy body falling on a light one resting on the ground) and turns both bodies. Their known limits: a contact can be found by a body which will not touch it (a ghost contact), and a body +accelerated by the solver during the step can go further than its margin. + +**Time of impact** (as in Box2D v3): after the solver, a body which moved more than half of its smallest extent is moved +back to its first impact with a static body (a plane, a terrain...) along its motion, its velocity is kept. A bullet +(`IsBullet`) is also stopped by the dynamic bodies. The time of impact is found by conservative advancement +(Mirtich, as in Bullet): the body moves forward by its distance to the other body (GJK) divided by the fastest approach +of its points, until it is `LinearSlop` away. If it already touches at the start, only its core (a sphere of 1/4 of its +smallest extent, as in Box2D) is stopped. + +**GJK distance**: the distance and the closest points of 2 convex shapes. The simplex is reduced to its feature closest +to the origin (Voronoi regions, Ericson 5.1 & 9.5). + +A sleeping body touched by an awake body wakes up with its island during the collision detection, and gets its contacts +in the same step (like Jolt). diff --git a/ARCHITECTURE.md b/ARCHITECTURE.md index 4d7df16..6637859 100644 --- a/ARCHITECTURE.md +++ b/ARCHITECTURE.md @@ -1,502 +1,121 @@ -# Feather Physics Engine - Architecture & Design Decisions - -This document explains the architectural choices, design decisions, and rationale behind the Feather physics engine implementation. - -## Table of Contents - -1. [Architecture Overview](#architecture-overview) -2. [XPBD Solver Choice](#xpbd-solver-choice) -3. [Collision Detection Pipeline](#collision-detection-pipeline) -4. [Shape System Design](#shape-system-design) -5. [Current Limitations](#current-limitations) -6. [Architecture Diagrams](#architecture-diagrams) - ---- - -## Architecture Overview - -Feather is built on a modular architecture with clear separation of concerns: - -``` -┌─────────────────────────────────────────────────────────────┐ -│ World │ -│ - Manages all rigid bodies │ -│ - Orchestrates physics pipeline │ -│ - Applies global forces (gravity) │ -└─────────────────────────────────────────────────────────────┘ - │ - ▼ -┌─────────────────────────────────────────────────────────────┐ -│ Physics Step │ -│ 1. Apply Forces → Integrate to Velocity │ -│ 2. Broad Phase Collision Detection (AABB) │ -│ 3. Narrow Phase Collision Detection (GJK + EPA) │ -│ 4. Generate Contact Manifolds │ -│ 5. Solve Position Constraints │ -│ 6. Solve Velocity Constraints (Restitution) │ -│ 7. Integrate Velocity → Position │ -└─────────────────────────────────────────────────────────────┘ -``` - -### Design Philosophy - -1. **Modularity**: Each component (collision detection, constraint solving, integration) is isolated -2. **Extensibility**: Interface-based design allows easy addition of new shapes and constraints -3. **Performance**: Conscious trade-offs between accuracy and speed for real-time game applications -4. **Simplicity**: Prioritize clear, maintainable code over premature optimization - -### Package Structure +# Feather - Architecture +## Packages ``` feather/ -├── world.go, collision.go # Core physics pipeline -├── actor/ # Physical objects -│ ├── rigidbody.go # Dynamic/static bodies -│ ├── shape.go # Shape interface & implementations -│ ├── transform.go # Spatial transforms -│ └── aabb.go # Bounding volumes -├── constraint/ # Constraint solving -│ ├── constraint.go # Constraint interface -│ └── contact.go # Contact constraint solver -├── gjk/ # GJK collision algorithm -├── epa/ # EPA penetration depth -│ ├── epa.go # Main EPA algorithm -│ ├── manifold.go # Contact point generation -│ └── face.go # Polytope face management -└── example/simpleScene/ # Usage examples +├── world.go # World.Step: collision detection, then solver +├── solver.go # TGS Soft solver +├── graph.go # graph coloring of the contacts and the joints, for the parallel solver +├── pool.go # workers of the step +├── island.go # sleep islands +├── joint.go # joints: distance, ball, hinge, fixed +├── joint_configurable.go # configurable joint: each axis locked, limited or free +├── articulation.go # the anchors of the trees of joints, solved together (Baraff 1996) +├── collision.go # BroadPhase, NarrowPhase, Collide +├── collision_capsule.go# spheres & capsules: closest points of segments +├── collision_heightfield.go # heightfields: triangles, inner edges, patches +├── ccd.go # continuous collision: time of impact of the fast bodies +├── tree.go # broad phase: dynamic AABB trees, the pairs kept from a step to the next +├── event.go # collision, trigger & sleep events +├── actor/ # RigidBody, Material, Transform, shapes (Sphere, Box, Plane, Capsule, Heightfield) +├── constraint/ # Manifold, ContactPoint, friction & restitution mixing +├── gjk/ # GJK (overlap test with margin, distance) +├── epa/ # EPA (penetration depth) & contact points (manifold) +└── bench/ # comparison with v0.2.0 (separate module) ``` ---- - -## XPBD Solver Choice - -### Why XPBD (Extended Position-Based Dynamics)? - -We chose XPBD over alternative physics solvers for several compelling reasons: - -#### 1. **Superior Stability for Stacking** -- XPBD handles resting contacts and stacked objects much better than impulse-based methods -- Position-based constraints prevent the "jitter" and "explosion" issues common in penalty methods -- Critical for games with towers, buildings, or piles of objects - -#### 2. **Compliance-Based Soft Constraints** -- Compliance parameter provides intuitive control over constraint "stiffness" -- More physically meaningful than arbitrary penalty coefficients -- Allows for realistic soft bodies and deformable objects in the future - -#### 3. **Timestep Independence** -- XPBD behavior remains consistent across different timesteps (within reason) -- Unlike basic PBD which is timestep-dependent -- Important for variable framerates in games - -#### 4. **Convergence Guarantees** -- Mathematical guarantees of convergence to correct solution -- More predictable behavior than iterative impulse solvers - -### Alternatives Considered - -#### Sequential Impulse Solver (Box2D/Bullet Style) -- **Pros**: Fast, well-understood, widely used -- **Cons**: - - Less stable for large stacks - - Requires careful parameter tuning - - Timestep-dependent behavior -- **Why not chosen**: Stability was our primary concern - -#### Penalty-Based Methods -- **Pros**: Simple to implement, fast computation -- **Cons**: - - Very difficult to tune (spring constants are unintuitive) - - Prone to explosions with high stiffness - - Requires very small timesteps for stability -- **Why not chosen**: Poor stability and difficult parameterization - -#### Projected Gauss-Seidel (PGS) -- **Pros**: Industry standard, robust -- **Cons**: - - More complex implementation - - Requires warm-starting for good performance - - Similar to what we get with XPBD anyway -- **Why not chosen**: XPBD provides similar benefits with simpler implementation - -### Academic References - -- **Original XPBD Paper**: "XPBD: Position-Based Simulation of Compliant Constrained Dynamics" by Macklin et al. (2016) -- **PBD Foundation**: "Position Based Dynamics" by Müller et al. (2007) -- **Comparison Study**: "Detailed Rigid Body Simulation with Extended Position Based Dynamics" by Macklin et al. (2020) - ---- - -## Collision Detection Pipeline - -Our collision detection uses a classic two-phase approach: broad phase filtering followed by narrow phase precise detection. - -### Broad Phase: Simple AABB Overlap Testing - -**Current Implementation**: O(n²) brute-force AABB overlap checks - -**Why this approach?** -- **Simplicity**: Easy to implement and debug -- **Small-scale focus**: Perfectly acceptable for <100 bodies -- **No false negatives**: Guaranteed to find all potential collisions -- **Minimal memory overhead**: No spatial data structures to maintain - -**Future Plans**: -- Spatial grid / hash for medium-scale scenes (100-1000 bodies) -- Bounding Volume Hierarchy (BVH) for large-scale scenes (1000+ bodies) -- The architecture supports drop-in replacement when needed - -### Narrow Phase: GJK + EPA - -**GJK (Gilbert-Johnson-Keerthi) Algorithm** - -**Why GJK?** -- **Versatility**: Works for ANY convex shape (boxes, spheres, capsules, polyhedra, etc.) -- **Speed**: Converges very quickly (typically 3-6 iterations) -- **Simplicity**: Core algorithm is ~100 lines of code -- **Extensibility**: Adding new shapes only requires implementing a support function - -**Alternative Considered: SAT (Separating Axis Theorem)** -- **Pros**: Slightly faster for boxes, provides penetration depth directly -- **Cons**: - - Shape-specific code for each pair (box-box, sphere-box, etc.) - - Doesn't generalize to smooth shapes (spheres, capsules) - - Much more code to maintain -- **Why not chosen**: GJK's generality is worth the minor performance cost - -**EPA (Expanding Polytope Algorithm)** - -**Why EPA after GJK?** -- GJK only tells us IF shapes overlap, not HOW MUCH -- EPA computes precise penetration depth and contact normal -- Builds on GJK's final simplex (no wasted work) -- Essential for realistic contact resolution - -**Key Implementation Details**: -- Handles degenerate cases (coplanar faces, polytope collapse) -- Maximum iteration limit prevents infinite loops -- Convergence tolerance balances precision vs performance -- Returns error on failure (graceful degradation) - -### Manifold Generation: Sutherland-Hodgman Clipping - -**Why Multi-Point Contacts?** -- **Stability**: Multiple contact points prevent excessive rotation -- **Realism**: Large flat surfaces (box on ground) should have area contact, not point contact -- **Torque**: Proper torque calculation requires knowing contact distribution - -**Algorithm Choice: Sutherland-Hodgman** -- Classic polygon clipping algorithm -- Generates 1-4 contact points (reduced from potential 8+ for performance) -- Well-tested, robust implementation - -**Special Cases**: -- **Sphere-Sphere**: Single contact point at midpoint (analytical solution) -- **Sphere-Box/Plane**: Project sphere center onto closest feature -- **Plane contacts**: Project box corners onto plane - ---- - -## Shape System Design - -### Interface-Based Polymorphism - -The shape system uses Go interfaces for clean polymorphism: - -```go -type ShapeInterface interface { - GetType() ShapeType - ComputeAABB(transform Transform) AABB - ComputeMass(density float64) (mass float64, inertia mgl64.Mat3) - Support(direction mgl64.Vec3) mgl64.Vec3 -} +## World.Step ``` - -**Design Benefits**: -1. **Extensibility**: Adding new shapes requires only implementing the interface -2. **Type Safety**: Compile-time checking of shape requirements -3. **Performance**: Interface calls are fast in Go (static dispatch where possible) -4. **Testability**: Easy to mock shapes for testing - -### Shape Implementations - -#### Box (Oriented Bounding Box) -- **Representation**: Half-extents (width/2, height/2, depth/2) -- **Rotation**: Full quaternion support for arbitrary orientation -- **Mass Properties**: Analytical formulas for mass and inertia tensor -- **Use Cases**: Buildings, crates, walls, platforms - -#### Sphere -- **Representation**: Radius -- **Rotation**: N/A (spherically symmetric) -- **Mass Properties**: I = (2/5) * m * r² -- **Use Cases**: Balls, projectiles, simplified characters - -#### Plane -- **Representation**: Normal vector + distance from origin -- **Rotation**: Normal defines orientation -- **Mass Properties**: Infinite mass (static only) -- **Special Handling**: Infinite shape requires custom collision logic -- **Use Cases**: Ground, walls, infinite surfaces - -### Future Shapes (Planned) - -- **Capsule**: Cylinder with hemispherical caps (great for characters) -- **Cylinder**: For wheels, pillars -- **Convex Hull**: General polyhedra from point clouds -- **Compound Shapes**: Combine multiple shapes into one body - -### Support Function: Core of GJK - -The `Support()` function is critical for GJK algorithm: - -```go -// Returns the farthest point in the given direction -Support(direction Vec3) Vec3 +Step(dt) +├── wake the sleeping bodies touched by a moving body +├── Phase 1: collision detection (once per step) +│ ├── AABBs enlarged by the distance each body can travel during dt +│ ├── broad phase: pairs of overlapping AABBs (AABB trees) +│ ├── narrow phase: manifold of each pair (parallel, Workers goroutines) +│ ├── a sleeping body touched by an awake body wakes up: the detection runs again +│ ├── events: pairs touching or overlapping (triggers are not solved) +│ └── warm start: each point takes the impulses of the closest point of the pair in the previous step +├── Phase 2: solver (substeps: articulations, then contacts and joints by color), then restitution +├── continuous collision: the fast bodies are moved back to their first impact +└── Phase 3: sleep islands & events ``` -**Why this design?** -- GJK only needs to query extreme points, not full geometry -- Allows implicit shapes (no vertex/edge storage needed) -- Analytical computation is often faster than searching vertices -- Natural abstraction for convex shapes - ---- - -## Current Limitations - -Understanding current limitations helps set appropriate expectations and guides future development. - -### 1. Transform: Position-Only (No Rotation) - -**Current State**: `Transform` type only handles translation - -**Why?** -- **Phased Development**: Getting core physics working first -- **Simplification**: Easier debugging without rotational dynamics -- **Incremental Complexity**: Will add rotation once contacts are stable - -**Impact**: -- Boxes cannot rotate (orientation locked) -- No angular velocity or torque -- Simplified inertia tensor calculations - -**Timeline**: Next major feature to implement - -### 2. Friction: Not Implemented - -**Current State**: No friction forces applied at contacts - -**Why?** -- **Core First**: Collision detection and basic response take priority -- **Algorithm Selection**: Still evaluating friction models (Coulomb vs others) -- **Complexity**: Friction requires tangent space calculations and additional solver passes - -**Impact**: -- Objects slide unrealistically on surfaces -- No resistance to lateral motion -- Spinning objects don't slow down - -**Timeline**: Phase 2 development (after rotation) - -### 3. Single-Threaded Execution - -**Current State**: All physics runs on one thread - -**Why?** -- **Simplicity**: Multi-threading adds significant complexity -- **Correctness First**: Get sequential version correct first -- **Profiling Needed**: Need to identify actual bottlenecks before parallelizing - -**Future Plans**: -- Goroutines for independent body integration -- Parallel broad-phase AABB tests -- Island-based parallel constraint solving - -**Acceptable For**: <100 bodies at 60 FPS - -### 4. No Spatial Acceleration - -**Current State**: O(n²) brute-force broad phase - -**Impact**: Performance degrades quadratically with body count - -**Acceptable For**: Small to medium scenes (<100 bodies) - -**Future**: Spatial grid or BVH when needed - -### 5. No Sleep/Deactivation System - -**Current State**: All bodies updated every frame - -**Impact**: Wasted computation on resting objects - -**Future**: Island detection and sleep state for stable bodies - ---- - -## Architecture Diagrams - -### Complete Physics Pipeline Flow - -```mermaid -graph TD - A[World.Step] --> B[Apply Gravity to Dynamic Bodies] - B --> C[Integrate Forces to Velocity] - C --> D[Broad Phase: AABB Overlap Tests] - D --> E[Narrow Phase: GJK Collision Detection] - E --> F{Collision?} - F -->|Yes| G[EPA: Compute Penetration Depth] - G --> H{EPA Success?} - H -->|Yes| I[Generate Contact Manifold] - I --> J[Solve Position Constraints] - J --> K[Solve Velocity Constraints] - K --> L[Integrate Velocity to Position] - L --> M[End Step] -``` - -### Collision Detection Flow - -```mermaid -graph LR - A[Body Pair] --> B[AABB Overlap Test] - B -->|No Overlap| C[Skip] - B -->|Overlap| D[GJK Algorithm] - D -->|Separated| C - D -->|Intersecting| E[EPA Algorithm] - E --> F[Contact Manifold] - F --> G[Contact Constraints] -``` - -### Package Dependencies - -```mermaid -graph TD - World[world.go] --> Actor[actor/] - World --> Collision[collision.go] - World --> Constraint[constraint/] - - Collision --> GJK[gjk/] - Collision --> EPA[epa/] - Collision --> Actor - - EPA --> Actor - GJK --> Actor - - Constraint --> Actor -``` - -### Data Flow in World.Step() - -``` -Input: dt (N substeps) -│ -├─► For each dynamic body: -│ └─► Apply gravity: F = m * g -│ -├─► For each dynamic body: -│ └─► Integrate: v += (F/m) * dt -│ -├─► Broad Phase: -│ ├─► Compute AABB for each body -│ └─► Find overlapping pairs O(n²) -│ -├─► Narrow Phase: -│ ├─► For each pair: -│ │ ├─► GJK: Check collision -│ │ ├─► EPA: Compute depth & normal -│ │ └─► Manifold: Generate contact points -│ └─► Output: List of contacts -│ -├─► Position Solver (XPBD - once per substep): -│ └─► For each contact: -│ └─► Apply position correction (XPBD) -│ -├─► Velocity Solver (XPBD - once per substep): -│ └─► For each contact: -│ └─► Apply velocity correction (restitution) -│ -└─► For each body: - └─► Integrate: x += v * dt - -Output: Updated body positions and velocities -``` - ---- - -## Design Patterns Used - -### 1. **Strategy Pattern**: Shape Interface -- Different collision strategies for different shapes -- Swap implementations without changing client code - -### 2. **Iterator Pattern**: Body Management -- World iterates over bodies for force application, integration -- Decouples physics algorithms from storage - -### 3. **Factory Pattern**: Shape Creation -- Centralized shape construction (future) -- Ensures proper initialization - ---- - -## Performance Characteristics - -### Computational Complexity - -| Component | Complexity | Notes | -|-----------|-----------|-------| -| Broad Phase | O(n²) | Will improve with spatial grid | -| Narrow Phase (GJK) | O(1) per pair | ~3-6 iterations typical | -| EPA | O(1) per collision | ~5-15 iterations typical | -| Manifold | O(1) per collision | Max 4 contact points | -| Position Solver | O(c * s) | c contacts, s substeps (1 pass/substep) | -| Velocity Solver | O(c * s) | c contacts, s substeps (1 pass/substep) | - -### Memory Usage - -- **Per Body**: ~200 bytes (transform, velocity, shape reference) -- **Per Contact**: ~150 bytes (points, normal, penetration depth) -- **Spatial Data**: None currently (will add spatial grid) - -### Typical Performance - -- **Small scenes** (<50 bodies): 1-2ms per frame at 60 FPS -- **Medium scenes** (50-100 bodies): 5-10ms per frame -- **Large scenes** (>100 bodies): Needs spatial acceleration - ---- - -## Future Architecture Enhancements - -### Short-Term -1. ✅ Rotation support (Transform quaternions) -2. ✅ Friction implementation -3. ✅ Sleep/island detection -4. ✅ Spatial grid for broad phase - -### Medium-Term -1. Additional shapes (capsule, cylinder, convex hull) -2. Distance constraints (springs, ropes) -3. Trigger volumes (non-physical overlap detection) -4. Joint constraints (hinge, slider, ball-socket) - -### Long-Term -1. Multi-threading with goroutines -2. Soft body dynamics (cloth, deformables) -3. Fluid simulation integration - ---- - -## Conclusion - -Feather's architecture prioritizes: -- **Clarity** over cleverness -- **Correctness** over optimization (initially) -- **Extensibility** for future features -- **Real-time performance** for game applications - -The XPBD solver, GJK+EPA collision detection, and interface-based shape system provide a solid foundation for a modern physics engine while maintaining code simplicity and maintainability. - -For detailed algorithm explanations, see [ALGORITHMS.md](ALGORITHMS.md). -For practical usage and parameter tuning, see [PHYSICS_GUIDE.md](PHYSICS_GUIDE.md). +## Collision detection +| Pair | Method | +|------|--------| +| any shape - plane | `CollideWithPlane` of the shape | +| any shape - heightfield | each triangle under the body: GJK + EPA, inner edges, patches (up to 8 manifolds) | +| sphere / capsule - sphere / capsule | closest points of the segments (a sphere is a segment of length 0) | +| sphere / capsule - other shape | GJK distance between the core (a point, a segment) and the shape, plus the radius; EPA only if the core is inside | +| other pairs | GJK + EPA, then clipping of the contact points | + +Pair cache (the body pair cache of Jolt): if a body moved less than 1 mm and 2° relative to the other since their contact +points were computed, the previous contact points are moved with the bodies, the collision detection doesn't run again. +The contacts of a pair are kept by its pair in the broad phase: no lookup. + +Contacts are kept up to a margin: `SpeculativeDistance` (2 cm), + the relative speed of the bodies * dt against a +static body. +Each manifold has a normal (from A to B) and up to 4 points. A pair has 1 manifold, up to 8 against a heightfield +(the manifolds of a pair follow each other). Each point has its own separation (< 0 when the bodies overlap). + +## Solver +See [ALGORITHMS.md](ALGORITHMS.md#solver). The solver works on copies of the dynamic bodies (`bodyState`): +the static and sleeping bodies share a state with no mass. + +## Threading & determinism +- From 256 bodies, a step runs on `Workers` goroutines. The workers are created once and sleep between the steps. + `World.Close()` stops them (they are also stopped when the World is garbage collected). +- The broad phase, the narrow phase, the preparation of the contacts and the integration of the bodies: + each body, pair or contact writes its result at its own index, the order of execution doesn't matter. +- The pairs are sorted (index of the first body, then of the second body). +- The solver is a Gauss-Seidel: a constraint uses the result of the previous one. The contacts and the joints are + colored (like Box2D v3): the constraints of a color don't share any dynamic body, so a color is solved in parallel. + The colors are always solved in the same order: the result is the same bit for bit, whatever the number of workers. +- A step doesn't allocate memory after the first steps: the buffers are reused. + +## Tests & benchmarks +**The reference is Box3D** (Erin Catto, 2026): Feather must do at least as well on the same scenes at 60 Hz, Feather +with 8 substeps (its setting for the games), Box3D with its default 4. The scenes come from Solver2D, extruded by 1 m in +3D (the same supports, the same mass ratios). The drivers of Box3D and Jolt on the same scenes are kept outside the +repository; the values of Box3D are written in the tests with their date. The known gaps are +logged, and followed by #821. + +Four levels, from the most precise to the widest: +1. **Minimal scenes** (`scenes_test.go`): one or two bodies isolating a mechanism. The bound of each scene is derived + from a quantity of the engine (`LinearSlop` for a depth), never fixed after a measure. +2. **Invariants** (`invariants_test.go`): `checkInvariants` runs at every step of 60 random scenes (piles on a plane, + on a terrain, bodies & joints in free flight), in parallel, in about 1 s. Each tolerance comes from the method: + the rounding for the momentum, the first order gyroscopic torque and the couple of the joints (gap × impulse) for + the angular momentum, `LinearSlop` for the depth & the energy pushed out of the ground. Reintroducing the bugs fixed on 27/09 (rotation capped per step, + contact points frozen during the step, a box touching a plane with its 8 corners) makes it fail. +3. **Scenes of Solver2D** (`bench/scenes`, `cd bench && go test ./...`): the samples of Erin Catto's Solver2D in 3D + (stacks, high mass ratios, overlap recovery, house of cards, chains, far from the origin...), small in the tests, + full in the bench (`go run . -scenes`, `go run . -compare` side by side with v0.2.0). Each scene holds (a derived + criterion) and, where Box2D has the scene, does at least as well as Box2D v3.1. +4. **Regressions** (`bench/regression.go`): 6 chaotic scenes and the scenes of Solver2D, compared to + `bench/baseline.json`: + - the fingerprint of the final state (identical on the same GOARCH); + - quality metrics, 0.5 mm of tolerance on a depth, 0.1 % on an energy gain; + - the time of a step (+20 %) and of its phases (+30 %, over 5 % of the step), best of 3 runs, only on the machine + of the reference, and for the steps over 0.1 ms (under it, the noise of the timer dominates). + +`World.Profile()` gives the time of each phase of the last step (broad phase, narrow phase, prepare, substeps, +restitution, continuous collision, islands), without allocation. +`World.parallelFrom` (tests only) runs the parallel paths under 256 bodies, for the determinism. + +## Current limitations +- The broad phase is a pair of dynamic AABB trees (static and dynamic bodies), the dynamic AABBs enlarged by a margin: + a sleeping body costs nothing (the planes & the heightfields are not in the trees, they are tested with every awake body). +- A heightfield is a surface: a body entirely under it is not pushed up. +- The contacts are computed once per step: on a rough terrain, a corner of a tumbling body can slide over another + triangle during the step, and sink by a few mm before the next step. +- The friction around the normal comes from the lever arms of the points: a ball spinning on itself on its single point + of contact never stops (no sleep). +- A capsule resting across a bump of a terrain can stay a few mm in the terrain: the contact of a triangle comes from + the feature of the body above the triangle, the middle of the capsule is missed. +- The restitution is applied once per step, with the velocity before the step: a body not round (box, capsule), + bouncy (`e` over 0.5) and spinning fast (20-50 rad/s) can bounce higher than it fell. Measured: up to +60 % of + energy at `e = 1`, never up to `e = 0.5`. Jolt documents the same limit. +- No kinematic bodies (moving platforms): a body is static or dynamic. +- The continuous collision stops the fast bodies against the static bodies (and the bullets against all the bodies), + not the other pairs: 2 fast dynamic bodies rely on their speculative contacts (2 cm) and on the spring of the contact. diff --git a/PHYSICS_GUIDE.md b/PHYSICS_GUIDE.md index 0783428..f6013f8 100644 --- a/PHYSICS_GUIDE.md +++ b/PHYSICS_GUIDE.md @@ -1,830 +1,162 @@ -# Feather Physics Engine - Physics Parameter Guide +# Feather - Physics Guide -This guide helps you configure realistic physics simulations by choosing appropriate material properties, simulation parameters, and troubleshooting common issues. +## Units +Feather uses the SI units: meters, kilograms, seconds, newtons. +- `AddForce` in N, `AddTorque` in N·m, both applied during the next `World.Step`. +- `Velocity` in m/s, `AngularVelocity` in rad/s (world space). +- `Gravity` in m/s². -## Table of Contents - -1. [Material Parameters](#material-parameters) -2. [Simulation Parameters](#simulation-parameters) -3. [Common Scenarios & Troubleshooting](#common-scenarios--troubleshooting) -4. [Code Examples](#code-examples) - ---- - -## Material Parameters +## Materials ### Density (kg/m³) - -Density determines an object's mass and inertia based on its volume. Higher density = heavier object = harder to move. - -#### Realistic Density Values - -| Material | Density (kg/m³) | Use Cases | -|----------|----------------|-----------| -| **Air** | 1.2 | Balloons, airships | -| **Wood (Balsa)** | 160 | Lightweight props | -| **Wood (Pine)** | 500-600 | Furniture, crates | -| **Wood (Oak)** | 700-900 | Heavy furniture, beams | -| **Ice** | 917 | Frozen objects | -| **Water** | 1000 | Reference value, liquids | -| **Concrete** | 2400 | Buildings, structures | -| **Glass** | 2500 | Windows, bottles | -| **Aluminum** | 2700 | Lightweight metal objects | -| **Stone (Granite)** | 2750 | Rocks, monuments | -| **Steel** | 7850 | Heavy machinery, I-beams | -| **Copper** | 8960 | Wires, pipes | -| **Lead** | 11340 | Very heavy objects | -| **Gold** | 19320 | Treasure, jewelry | - -#### How Density Affects Simulation - -```go -// Example: Two boxes with same size, different densities -lightBox := actor.NewBox( - transform, - mgl64.Vec3{1, 1, 1}, // 1m x 1m x 1m - 500, // Wood density -) -// mass = density * volume = 500 * (1*1*1) = 500 kg - -heavyBox := actor.NewBox( - transform, - mgl64.Vec3{1, 1, 1}, // Same size - 7850, // Steel density -) -// mass = 7850 * (1*1*1) = 7850 kg - -// The steel box is 15.7x heavier! -// - Requires 15.7x more force to accelerate -// - Falls at same speed (gravity affects all masses equally) -// - Has 15.7x more momentum when moving -``` - -#### Choosing Density - -**For realistic simulation**: Use real-world values from table above - -**For gameplay**: -- **Heavy objects** (800-10000 kg/m³): Obstacles, boss enemies, wrecking balls -- **Medium objects** (100-800 kg/m³): Player, props, crates -- **Light objects** (10-100 kg/m³): Debris, decorations, pickups - -**Pro tip**: Mass ratio between interacting objects should ideally be <10:1 for stable simulation. If you need a massive object (planet), make it static instead. - ---- - -### Restitution (Coefficient of Restitution) - -Restitution controls "bounciness" - how much kinetic energy is retained after collision. - -#### Restitution Scale (0.0 to 1.0) - -| Value | Behavior | Real Materials | Game Use | -|-------|----------|----------------|----------| -| **0.0** | No bounce (perfectly inelastic) | Clay, putty, wet sand | Sticky surfaces, dampening | -| **0.1-0.2** | Minimal bounce | Lead, wet wood | Realistic ground | -| **0.3-0.4** | Slight bounce | Concrete, hard wood | Standard surfaces | -| **0.5-0.6** | Moderate bounce | Hard plastic, stone | Slightly bouncy | -| **0.7-0.8** | High bounce | Rubber, basketballs | Bouncy surfaces | -| **0.9** | Very high bounce | Super balls | Game power-ups | -| **0.95-1.0** | Nearly perfect bounce | Steel on steel | Pinball, special effects | -| **>1.0** | Gains energy (unphysical!) | N/A | Avoid (causes instability) | - -#### Restitution Formula - -When two objects collide, Feather combines their restitution values: - -```go -// Current implementation: Maximum of the two -combinedRestitution = max(bodyA.Restitution, bodyB.Restitution) - -// Alternative approaches (not currently used): -// Average: (bodyA.Restitution + bodyB.Restitution) / 2 -// Multiply: bodyA.Restitution * bodyB.Restitution -``` - -**Why maximum?** A bouncy ball (e=0.9) should bounce on any surface, even clay (e=0.0). - -#### How Restitution Affects Simulation - -```go -// Example: Dropping a ball from 10m height - -// Clay ball (e = 0.0) -ball1 := actor.NewSphere(transform, 0.5, 1000) -ball1.Restitution = 0.0 -// Hits ground at ~14 m/s, bounces at 0 m/s → stops dead - -// Basketball (e = 0.8) -ball2 := actor.NewSphere(transform, 0.5, 1000) -ball2.Restitution = 0.8 -// Hits ground at ~14 m/s, bounces at 11.2 m/s → bounces to 6.4m - -// Super ball (e = 0.95) -ball3 := actor.NewSphere(transform, 0.5, 1000) -ball3.Restitution = 0.95 -// Hits ground at ~14 m/s, bounces at 13.3 m/s → bounces to 9m -``` - -#### Choosing Restitution - -**For realism**: Use values from table (most materials: 0.2-0.6) - -**For gameplay**: -- **Sticky platforms** (0.0): Player shouldn't bounce when landing -- **Standard ground** (0.1-0.3): Slight bounce, feels natural -- **Bouncy obstacles** (0.7-0.9): Fun gameplay mechanic -- **Hyper-bounce** (0.95+): Special power-up zones - -**Pro tip**: Extremely high restitution (>0.95) can cause jitter. If objects won't settle, reduce restitution or increase compliance. - ---- - -### Compliance (Soft Constraint Parameter) - -Compliance controls constraint "softness" - how much constraints are allowed to violate before being enforced. - -**Physics Meaning**: Compliance = 1 / Stiffness - -#### Compliance Scale - -| Value | Behavior | Visual Effect | Use Cases | -|-------|----------|---------------|-----------| -| **0** | Infinitely stiff (hard constraint) | No penetration, potential jitter | Ideal (but may be unstable) | -| **1e-10** | Extremely stiff | Tiny penetration, may jitter | Very rigid contacts | -| **1e-9** | Very stiff (default) | Barely visible penetration | Standard rigid bodies | -| **1e-8** | Stiff | Slight penetration, smoother | Stable rigid bodies | -| **1e-7** | Moderate | Noticeable soft contact | Slightly squishy objects | -| **1e-6** | Soft | Visible squishing | Soft bodies, cushions | -| **1e-5** | Very soft | Significant deformation | Jello, very soft materials | - -#### How Compliance Affects Simulation - -``` -Low Compliance (1e-10): -┌────┐ -│ │ ← Object barely penetrates surface -└────┘ -═══════ Ground - -High Compliance (1e-6): -┌────┐ -│ │ -└─ ┬ ┴┘ ← Object visibly sinks into surface -══╧═══ Ground -``` - -**Trade-off**: -- **Lower compliance** → Stiffer contacts → Less penetration → More jitter/instability -- **Higher compliance** → Softer contacts → More penetration → More stable/smooth - -#### Compliance Tuning Process - -1. **Start with default**: `1e-9` (very stiff) -2. **If jittery/vibrating**: Increase compliance by 10x (`1e-8`) -3. **If too much penetration**: Decrease compliance by 10x (`1e-10`) -4. **If still unstable**: Increase substeps or decrease timestep -5. **Iterate until satisfied** - -#### Code Example - -```go -// Current: Compliance is hardcoded in epa/epa.go -// Future: Will be a material property - -// Temporary workaround: Modify epa/epa.go constant -const DefaultCompliance = 1e-9 // Adjust this value - -// Ideal future API: -material := actor.Material{ - Density: 500, - Restitution: 0.3, - Compliance: 1e-8, // Per-material compliance -} -box := actor.NewBoxWithMaterial(transform, halfExtents, material) -``` - ---- - -## Simulation Parameters - -These parameters affect the global simulation quality and performance. - -### Timestep (dt) - -Timestep is how much simulated time passes per physics update. - -#### Common Timestep Values - -| Timestep | FPS Equivalent | Use Case | -|----------|----------------|----------| -| **1/30 (0.0333s)** | 30 FPS | Slow-paced games, low-end devices | -| **1/60 (0.0167s)** | 60 FPS | **Standard for most games** | -| **1/120 (0.0083s)** | 120 FPS | High-precision simulation | -| **1/240 (0.0042s)** | 240 FPS | Very fast objects, high accuracy | - -#### Fixed vs Variable Timestep - -**Fixed Timestep (Recommended)**: -```go -const physicsTimestep = 1.0 / 60.0 // 60 FPS - -func GameLoop() { - accumulator := 0.0 - for { - frameTime := GetFrameTime() - accumulator += frameTime - - // Update physics in fixed timesteps - for accumulator >= physicsTimestep { - world.Step(physicsTimestep) - accumulator -= physicsTimestep - } - - Render() - } -} -``` - -**Benefits**: -- Deterministic (same input → same output) -- Stable (physics tuned for one timestep) -- Prevents physics explosions from frame rate drops - -**Variable Timestep (Not Recommended)**: -```go -func GameLoop() { - for { - dt := GetFrameTime() // Variable! - world.Step(dt) // Unstable - Render() - } -} -``` - -**Problems**: -- Non-deterministic -- Unstable (large dt can cause explosions) -- Difficult to tune - -#### Choosing Timestep - -**For most games**: `1/60` (60 FPS physics) - -**Use smaller timestep if**: -- Fast-moving objects tunnel through walls -- High restitution causes instability -- Simulation feels "floaty" or imprecise - -**Trade-off**: Smaller timestep = more accurate but more CPU cost - ---- - -### Substeps - -Substeps divide each physics step into smaller internal steps for better accuracy. - -#### How Substeps Work - -```go -func Step(dt float64, substeps int) { - subDt := dt / float64(substeps) - - for i := 0; i < substeps; i++ { - // Run full physics pipeline - ApplyForces(subDt) - DetectCollisions() - SolveConstraints(subDt) - IntegrateVelocities(subDt) - } -} - -// Example: dt=1/60, substeps=4 -// Each substep processes 1/240 of a second -// Reduces tunneling and improves stability -``` - -#### Substep Guidelines - -| Substeps | Use Case | CPU Cost | -|----------|----------|----------| -| **1** | Standard scenes, medium speeds | 1x (baseline) | -| **2** | Fast-moving objects, bouncy materials | 2x | -| **4** | Very fast projectiles, high stacks | 4x | -| **8+** | Extreme accuracy requirements | 8x+ | - -**When to increase substeps**: -- Fast objects tunnel through thin walls -- Bouncy objects (high restitution) jitter or explode -- Tall stacks collapse unrealistically -- Constraints feel "soft" even with low compliance - -#### Code Example - -```go -// Currently: Substeps are internal to World.Step() -// Check world.go for substep implementation - -world := NewWorld() -world.Substeps = 4 // If this property exists -world.Step(1.0 / 60.0) -``` - ---- - -### Substeps (NOT Solver Iterations) - -**IMPORTANT**: XPBD uses **substeps** with **ONE solver iteration per substep**, not multiple solver iterations per step. - -The key difference from traditional solvers: -- **Traditional solvers**: 1 step, many iterations -- **XPBD**: Many substeps, 1 iteration per substep - -```go -// XPBD approach (what Feather uses) -func Step(dt float64, substeps int) { - h := dt / float64(substeps) - - for i := 0; i < substeps; i++ { - // Apply forces & integrate velocities - // Detect collisions - - // Solve constraints - SINGLE iteration only! - for each contact { - SolvePositionConstraint(h) - } - for each contact { - SolveVelocityConstraint(h) - } - - // Integrate positions - } -} -``` - -#### Substep Guidelines - -| Substeps | Quality | Use Case | CPU Cost | -|----------|---------|----------|----------| -| **1** | Standard | Most scenes | 1x (baseline) | -| **2** | Better | Fast objects, bouncy | 2x | -| **4** | High | Very fast, tall stacks | 4x | -| **8+** | Very high | Extreme accuracy | 8x+ | - -**When to increase substeps** (NOT iterations): -- Fast-moving objects tunnel through walls -- Tall stacks collapse unrealistically -- High restitution causes instability -- Need more precise integration - -**Trade-off**: More substeps = more accurate but slower - -**Why this works**: Each substep operates on a smaller timestep, improving integration accuracy and constraint stability without needing multiple iterations. - -#### Code Example - -```go -// Current implementation (world.go) -world := NewWorld() -world.Substeps = 2 // 2 substeps, 1 iteration each -world.Step(1.0 / 60.0) // Total: 2 solver passes -``` - ---- - -## Common Scenarios & Troubleshooting - -### Scenario 1: Stack of Boxes (Stability Test) - -**Goal**: Build a stable tower of boxes - -#### Recommended Setup - -```go -ground := actor.NewPlane( - actor.Transform{Position: mgl64.Vec3{0, 0, 0}}, - mgl64.Vec3{0, 1, 0}, // Normal pointing up - 0, // Distance from origin -) -ground.Static = true -ground.Restitution = 0.1 // Minimal bounce - -boxes := make([]*actor.RigidBody, 10) -for i := 0; i < 10; i++ { - box := actor.NewBox( - actor.Transform{Position: mgl64.Vec3{0, float64(i)*2 + 1, 0}}, - mgl64.Vec3{0.5, 1, 0.5}, // 1m x 2m x 1m boxes - 700, // Wood density - ) - box.Restitution = 0.1 // Low bounce - boxes[i] = box - world.AddBody(box) -} - -// Simulation settings -dt := 1.0 / 60.0 -substeps := 2 // XPBD uses substeps, not solver iterations -``` - -#### Common Problems & Solutions - -| Problem | Cause | Solution | -|---------|-------|----------| -| Stack collapses immediately | Boxes spawned overlapping | Space boxes further apart initially | -| Stack wobbles and falls | Too few substeps | Increase substeps to 4 | -| Boxes vibrate/jitter | Compliance too low | Increase compliance to 1e-8 | -| Boxes sink into each other | Compliance too high | Decrease compliance to 1e-9 | -| Stack slowly tips over | Numerical drift | Increase substeps to 2-4 | - ---- - -### Scenario 2: Bouncing Ball - -**Goal**: Realistic bouncy ball that loses energy gradually - -#### Recommended Setup - -```go -ground := actor.NewPlane( - actor.Transform{Position: mgl64.Vec3{0, 0, 0}}, - mgl64.Vec3{0, 1, 0}, - 0, -) -ground.Static = true -ground.Restitution = 0.8 // Bouncy surface - -ball := actor.NewSphere( - actor.Transform{Position: mgl64.Vec3{0, 10, 0}}, // 10m high - 0.5, // 0.5m radius - 1100, // Rubber density -) -ball.Restitution = 0.85 // High restitution - -world.AddBody(ground) -world.AddBody(ball) - -// Combined restitution: max(0.8, 0.85) = 0.85 -// Ball will bounce to ~72% of previous height each bounce -``` - -#### Expected Behavior - -``` -Drop height: 10m -Bounce 1: ~7.2m (0.85² ≈ 0.72) -Bounce 2: ~5.2m -Bounce 3: ~3.7m -Bounce 4: ~2.7m -...eventually settles -``` - -#### Common Problems & Solutions - -| Problem | Cause | Solution | -|---------|-------|----------| -| Ball doesn't bounce | Restitution too low | Increase to 0.7-0.9 | -| Ball bounces forever | Restitution too high (>1.0) | Reduce to ≤0.95 | -| Ball bounces higher each time | Restitution >1.0 or solver bug | Check restitution value | -| Ball vibrates on ground | High restitution + low compliance | Increase compliance or reduce restitution | -| Energy loss too fast | Combined restitution low | Increase restitution on both objects | - ---- - -### Scenario 3: Resting Contacts (Jitter Prevention) - -**Goal**: Objects at rest shouldn't vibrate or jitter - -#### Recommended Setup - -```go -ground := actor.NewPlane( - actor.Transform{Position: mgl64.Vec3{0, 0, 0}}, - mgl64.Vec3{0, 1, 0}, - 0, -) -ground.Static = true -ground.Restitution = 0.0 // No bounce for resting - -box := actor.NewBox( - actor.Transform{Position: mgl64.Vec3{0, 1, 0}}, - mgl64.Vec3{1, 1, 1}, - 500, // Wood -) -box.Restitution = 0.0 // No bounce -// Let it fall and settle - -// Tuning parameters -compliance := 1e-8 // Slightly soft for stability -velocityThreshold := 0.01 // Sleep threshold (future feature) -``` - -#### Common Problems & Solutions - -| Problem | Cause | Solution | -|---------|-------|----------| -| Object vibrates up/down | Compliance too low | Increase compliance to 1e-8 or 1e-7 | -| Object slowly sinks | Compliance too high | Decrease compliance to 1e-9 | -| Object drifts sideways | Friction not implemented | Wait for friction feature, or increase compliance | -| Vibration at high FPS | Timestep too small for compliance | Adjust compliance proportionally | - ---- - -### Scenario 4: Fast-Moving Objects (Tunneling Prevention) - -**Goal**: Prevent fast projectiles from passing through thin walls - -#### The Tunneling Problem - -``` -Frame 1: Frame 2: - • - (ball) - | |• - | wall (ball passed through!) - | | -``` - -When object moves >1 thickness per frame, it can "teleport" through walls. - -#### Solutions - -**Solution 1: Increase Substeps** (Recommended) -```go -world.Substeps = 4 // Check collision 4x per frame -// Effective speed limit = wallThickness * substeps / dt -``` - -**Solution 2: Decrease Timestep** -```go -dt = 1.0 / 120.0 // 120 FPS physics (2x more CPU) -``` - -**Solution 3: Thicken Walls** -```go -wall := actor.NewBox( - transform, - mgl64.Vec3{5, 10, 2}, // 4m thick instead of 0.5m - 2400, // Concrete -) -``` - -**Solution 4: Continuous Collision Detection (Future Feature)** -```go -bullet.CCD = true // Will detect collision along swept path -``` - -#### Speed Limit Calculation - -``` -Maximum safe speed = wallThickness / (dt / substeps) - -Example: -- Wall thickness: 0.5m -- Timestep: 1/60 = 0.0167s -- Substeps: 4 -- Safe speed: 0.5 / (0.0167/4) = 120 m/s - -For faster speeds, increase substeps or decrease dt -``` - ---- - -## Code Examples - -### Example 1: Simple Scene Setup - -```go -package main - -import ( - "github.com/akmonengine/feather" - "github.com/akmonengine/feather/actor" - "github.com/go-gl/mathgl/mgl64" -) - -func main() { - // Create world with gravity - world := feather.NewWorld() - world.Gravity = mgl64.Vec3{0, -9.81, 0} // Earth gravity - - // Create ground plane - ground := actor.NewPlane( - actor.Transform{Position: mgl64.Vec3{0, 0, 0}}, - mgl64.Vec3{0, 1, 0}, // Normal (up) - 0, - ) - ground.Static = true - ground.Restitution = 0.3 - world.AddBody(ground) - - // Create falling box - box := actor.NewBox( - actor.Transform{Position: mgl64.Vec3{0, 10, 0}}, - mgl64.Vec3{0.5, 0.5, 0.5}, // 1m cube - 700, // Wood density - ) - box.Restitution = 0.4 - world.AddBody(box) - - // Simulation loop - dt := 1.0 / 60.0 - for i := 0; i < 600; i++ { // 10 seconds - world.Step(dt) - // Render or log positions - } -} -``` - -### Example 2: Tower of Boxes - -```go -func CreateTower(world *feather.World, height int) { - for i := 0; i < height; i++ { - box := actor.NewBox( - actor.Transform{ - Position: mgl64.Vec3{0, float64(i)*2.0 + 1.0, 0}, - }, - mgl64.Vec3{0.5, 1.0, 0.5}, // 1m x 2m x 1m - 700, // Wood - ) - box.Restitution = 0.1 // Low bounce for stability - world.AddBody(box) - } +The mass and inertia come from the density and the volume of the shape. + +| Material | Density (kg/m³) | +|----------|----------------| +| Wood (Balsa) | 160 | +| Wood (Pine) | 500-600 | +| Wood (Oak) | 700-900 | +| Ice | 917 | +| Water | 1000 | +| Concrete | 2400 | +| Glass | 2500 | +| Stone (Granite) | 2750 | +| Steel | 7850 | +| Lead | 11340 | + +### Friction +`StaticFriction` is used while the contact sticks, `DynamicFriction` while it slides (above 1 cm/s). The friction of +a contact acts at the center of its points, along the surface and around the normal (a box turning on the ground). +The friction of a contact is the geometric mean of both bodies: `sqrt(µA * µB)`. +Note: a body with a friction of 0 removes the friction of all its contacts, including with the ground. + +A box stays on a slope when `tan(angle) < µ`. + +| Surfaces | Friction | +|----------|----------| +| Ice | 0.05 | +| Wood on wood | 0.3 - 0.5 | +| Rubber on concrete | 0.8 - 1.0 | + +### Restitution (0.0 to 1.0) +The restitution of a contact is the average of both bodies. A ball dropped from a height h bounces back to `e² * h`. +There is no bounce under 1 m/s of impact (`RestitutionThreshold`), so resting bodies don't jitter. + +| Value | Behavior | Real Materials | +|-------|----------|----------------| +| 0.0 | No bounce | Clay, wet sand | +| 0.1-0.2 | Minimal bounce | Lead, wet wood | +| 0.3-0.4 | Slight bounce | Concrete, hard wood | +| 0.5-0.6 | Moderate bounce | Hard plastic, stone | +| 0.7-0.8 | High bounce | Rubber, basketballs | +| 0.9 | Very high bounce | Super balls | + +### Rolling resistance +`RollingResistance` (usually 0 to 1, 0 by default) slows down the rolling spheres and capsules. Without it, a ball rolls forever +on a flat ground, and a scene with balls never sleeps. The contact uses the largest value of both bodies, times the largest +radius (0 for a box). A ball rolling at v stops after `v² / (2 * 5/7 * resistance * g)`. + +### Damping +`LinearDamping` and `AngularDamping` (1/s) slow the body down: `v = v / (1 + h * damping)` at each substep. + +## Simulation + +### Joints +```go +ball := feather.NewBallJoint(parent, child, anchor, twistAxis) // world space +ball.EnableSwingLimit, ball.SwingLimitY, ball.SwingLimitZ = true, 0.5, 0.3 // rad +ball.EnableTwistLimit, ball.TwistMin, ball.TwistMax = true, -0.2, 0.2 +world.AddJoint(ball) +``` +- `Hertz` & `DampingRatio`: the softness of the joint (60 Hz and 2 by default, capped at 1/4 of the substeps rate). +- `CollideConnected` (false by default): the 2 bodies of the joint don't collide with each other. +- The drive of the ball joint (`DriveTarget`, `DriveHertz`, `DriveDampingRatio`) brings the child to a target rotation, + like a muscle: a damping ratio of 1 reaches it without overshoot. +- The motor of the hinge turns at `MotorSpeed` with at most `MaxMotorTorque`. +- A body removed from the world removes its joints. + +```go +slider := feather.NewConfigurableJoint(frame, carriage, anchor, mgl64.Vec3{1, 0, 0}) // everything locked +slider.LinearMotion[0] = feather.MotionLimited +slider.LinearMin, slider.LinearMax = mgl64.Vec3{-1, 0, 0}, mgl64.Vec3{1, 0, 0} +world.AddJoint(slider) +``` +- The configurable joint sets each axis: `MotionLocked`, `MotionLimited` or `MotionFree`. 3 linear axes, the twist + and 2 swings (a cone if both are limited). +- Its drives bring B to `DriveTargetPosition` and `DriveTargetRotation` (in the frame A). +- The limits are soft: a huge force bends them a little (under 0.5° for 5 g at the end of an arm). + +### Terrain +```go +// the grid of the terrain: heights[x*zSamples+z], shared without copy +field := actor.NewHeightfield(xSamples, zSamples, heights, mgl64.Vec3{0.5, 20, 0.5}) // 0.5 m between samples, heights * 20 +terrain := actor.NewRigidBody(actor.Transform{Rotation: mgl64.QuatIdent()}, field, actor.BodyTypeStatic, 0) +world.AddBody(terrain) + +// after a change of the heights (or of field.Holes) in [minX, maxX] x [minZ, maxZ] +world.UpdateHeightfield(terrain, minX, minZ, maxX, maxZ) +``` +- A heightfield is static. The body is at the center of the grid, the heights along its Y axis. +- The terrain is made of triangles: a finer grid gives finer contacts (a 2048x2048 grid follows the ground better than + 512x512), the bodies slide on the flat parts without hitting the edges between the triangles. +- `Holes[x*(zSamples-1)+z]`: a cell without triangles (a cave, a tunnel entrance). +- `World.UpdateHeightfield` wakes up the bodies above the changed region, and computes their contacts again. + +### Moving a body +A shape has no state: several bodies can share the same shape. Each body keeps its AABB: after moving a body by hand +(its `Transform`), call `UpdateAABB`. + +### Timestep & substeps +```go +world := feather.World{ + Gravity: mgl64.Vec3{0, -9.81, 0}, + Substeps: 8, + SpatialGrid: feather.NewSpatialGrid(2.0, 4096), + Workers: 4, + Events: feather.NewEvents(), } - -func main() { - world := feather.NewWorld() - world.Gravity = mgl64.Vec3{0, -9.81, 0} - - ground := actor.NewPlane( - actor.Transform{Position: mgl64.Vec3{0, 0, 0}}, - mgl64.Vec3{0, 1, 0}, - 0, - ) - ground.Static = true - ground.Restitution = 0.1 - world.AddBody(ground) - - CreateTower(world, 10) // 10-box tower - - // Stable simulation settings - dt := 1.0 / 60.0 - substeps := 2 // XPBD: 2 substeps = 2 solver passes total - - for i := 0; i < 1200; i++ { // 20 seconds - world.Step(dt) - } -} -``` - -### Example 3: Bouncing Balls with Different Materials - -```go -func main() { - world := feather.NewWorld() - world.Gravity = mgl64.Vec3{0, -9.81, 0} - - ground := actor.NewPlane( - actor.Transform{Position: mgl64.Vec3{0, 0, 0}}, - mgl64.Vec3{0, 1, 0}, - 0, - ) - ground.Static = true - ground.Restitution = 0.8 // Bouncy floor - world.AddBody(ground) - - // Different ball materials - materials := []struct { - name string - restitution float64 - density float64 - x float64 - }{ - {"Clay", 0.0, 1500, -3}, - {"Wood", 0.3, 700, -1}, - {"Rubber", 0.85, 1100, 1}, - {"Super Ball", 0.95, 1100, 3}, - } - - for _, mat := range materials { - ball := actor.NewSphere( - actor.Transform{Position: mgl64.Vec3{mat.x, 10, 0}}, - 0.5, - mat.density, - ) - ball.Restitution = mat.restitution - world.AddBody(ball) - } - - // Run simulation and observe different bounce behaviors - dt := 1.0 / 60.0 - for i := 0; i < 600; i++ { - world.Step(dt) - } -} -``` - -### Example 4: Newton's Cradle (Contact Chain) - -```go -func CreateNewtonsCradle(world *feather.World) { - // Five spheres in a row - for i := 0; i < 5; i++ { - sphere := actor.NewSphere( - actor.Transform{ - Position: mgl64.Vec3{float64(i) * 1.1, 5, 0}, - }, - 0.5, // Radius - 7850, // Steel density - ) - sphere.Restitution = 0.95 // Nearly elastic - world.AddBody(sphere) - } - - // Pull first sphere back and release - // (requires distance constraint - not yet implemented) - // For now, just give it initial velocity: - spheres := world.GetBodies() - spheres[0].Velocity = mgl64.Vec3{5, 0, 0} // Push rightward -} - -// Expected behavior: Energy transfers through chain -// First ball stops, last ball swings out -``` - ---- - -## Quick Reference Tables - -### Material Presets - -```go -// Define common material presets -type MaterialPreset struct { - Density float64 - Restitution float64 -} - -var Materials = map[string]MaterialPreset{ - "Wood": {700, 0.3}, - "Stone": {2750, 0.2}, - "Steel": {7850, 0.6}, - "Rubber": {1100, 0.85}, - "Glass": {2500, 0.4}, - "Concrete": {2400, 0.2}, - "Ice": {917, 0.05}, -} - -// Usage: -mat := Materials["Wood"] -box := actor.NewBox(transform, halfExtents, mat.Density) -box.Restitution = mat.Restitution -``` - -### Troubleshooting Checklist - -| Symptom | Check | Typical Fix | -|---------|-------|-------------| -| Objects jitter/vibrate | Compliance | Increase to 1e-8 | -| Objects sink into ground | Compliance | Decrease to 1e-9 | -| Objects bounce forever | Restitution | Reduce to <0.95 | -| No bounce at all | Restitution | Increase to >0.3 | -| Stack collapses | Substeps | Increase to 4 | -| Fast objects tunnel | Substeps | Increase to 4+ | -| Simulation too slow | Substeps | Reduce to 1 | -| Unrealistic movement | Density | Use real-world values | - ---- - -## Performance Optimization - -### Tips for Large Scenes - -1. **Use static bodies for immovable objects** (ground, walls) -2. **Implement sleep system** (deactivate resting bodies) - future feature -3. **Use minimum substeps** (usually 1-2 is sufficient) -4. **Use spatial acceleration** for broad phase - future feature -5. **Optimize contact manifolds** (cache between frames) - future feature - -### Performance Budget Example - -``` -Target: 60 FPS (16.67ms per frame) -Physics budget: 5ms - -Rough estimates: -- 50 bodies: ~2ms (comfortable) -- 100 bodies: ~5ms (at limit) -- 200 bodies: ~12ms (need optimization) - -If over budget: -1. Reduce substeps (2 → 1) -2. Increase timestep (1/60 → 1/30) -3. Implement spatial grid (future) -4. Implement sleep/islands (future) -``` - ---- - -For architectural decisions and design rationale, see [ARCHITECTURE.md](ARCHITECTURE.md). -For detailed algorithm explanations, see [ALGORITHMS.md](ALGORITHMS.md). +world.Step(1.0 / 60.0) // fixed timestep +``` + +- Use a fixed timestep (e.g. 1/60 s), with an accumulator if the frame rate varies. +- The contacts are computed once per step, the solver runs once per substep. +- Usually 4 substeps for simple scenes, 8 to 12 for stacks and heavy bodies. + +### Contact stiffness +`World.ContactHertz` (30 Hz by default, as Box2D v3.1) is the stiffness of the contacts. The contacts with a static body +are twice as stiff. +- Higher values = less overlap under load (stacks), but it is capped at 1/8 of the substeps rate: `substeps / dt / 8`. +- Lower values = softer contacts. + +With 12 substeps at 50 Hz, a stack of 10 boxes of 50 cm sinks by ~23 mm (Box2D v3.1: 30 mm): a contact sinks by +(load / mass) g / (2π hertz)² under its load. A stiffer world sinks less, but a heavy body landing on a light one bounces +more. + +### Fast bodies +The contacts with a static body are created before the body touches it (speculative contacts), from the distance it can +travel during the step. Between 2 dynamic bodies, from 2 cm only: a fast body can enter another one during a step, the +spring of the contact pushes it out. +A fast body is also moved back to its first impact with a static body (continuous collision). Set `IsBullet` on a small +fast body (a projectile) to stop it on the dynamic bodies too. +A ball at 40 m/s does not go through a 4 cm wall at 50 Hz. + +### Sleep +The bodies touching each other form an island. An island resting for 0.5 s (all its bodies under 0.05 m/s and 0.05 rad/s) +falls asleep: it is not simulated anymore. +The whole island wakes up with `AddForce`, `AddTorque` or `WakeUp` on one of its bodies, when a moving body touches it, +or when a body under it is removed. + +### Determinism & threads +The same scene gives the same result, bit for bit, whatever the number of `Workers`. +From 256 bodies, the collision detection and the solver run on `Workers` goroutines: set it to the number of cores. +Call `World.Close()` when the world is not used anymore, to stop its workers. + +## Troubleshooting + +| Problem | Solution | +|---------|----------| +| Bodies slide on slopes | Set `StaticFriction` & `DynamicFriction` on both bodies (the ground too) | +| Stacks sink | Increase `ContactHertz` or `Substeps` | +| Stacks wobble | Increase `Substeps` | +| No bounce | Restitution on both bodies, impact faster than 1 m/s | +| A body does not move | It may be asleep: call `WakeUp` | diff --git a/README.md b/README.md index 7f27f3c..57d4179 100644 --- a/README.md +++ b/README.md @@ -8,99 +8,160 @@ ![GitHub Issues or Pull Requests](https://img.shields.io/github/issues/akmonengine/feather) ![GitHub Issues or Pull Requests](https://img.shields.io/github/issues-pr/akmonengine/feather) -A Go physic library, based on the XPBD solver algorithm. - -## XPBD -The current implementation simplifies the initial algorithm found on the internet: -```` -while simulating do - CollectCollisionPairs(); - h ← Δt/numSubsteps; - for numSubsteps do - for n bodies and particles do - x_prev ← x; - v ← v + h*f_ext/m; - x ← x + h*v; - end - for numPosIters do - SolvePositions(x₁,...,xₙ); - end - for n bodies and particles do - v ← (x - x_prev)/h; - end - SolveVelocities(v₁,...,vₙ); - end -end -```` - -In the papers, the CollectCollisionPairs is applies once, and seems to use some Continuous Collision Detection. - +A Go physic library, based on the TGS Soft solver algorithm. + +## Shapes +All shapes live in the `actor` package and implement `actor.ShapeInterface`. + +| Shape | Definition | Narrow phase | +|---|---|---| +| `Sphere` | `Radius` | analytic against planes, spheres and capsules; its center against the other shapes (GJK distance + radius) | +| `Box` | `HalfExtents` | analytic against planes; GJK/EPA otherwise | +| `Plane` | `Normal`, `Distance` (static only) | analytic | +| `Capsule` | `HalfHeight`, `Radius`, axis along local Y | analytic against planes, spheres and capsules; its segment against the other shapes (GJK distance + radius) | +| `Heightfield` | a grid of heights (static only), 2 triangles per cell | GJK/EPA against each triangle under the body | + +```go +body := actor.NewRigidBody( + actor.Transform{Position: mgl64.Vec3{0, 1, 0}, Rotation: mgl64.QuatIdent()}, + &actor.Capsule{HalfHeight: 0.6, Radius: 0.3}, + actor.BodyTypeDynamic, + 1000, // density +) +body.Material.StaticFriction = 0.6 +body.Material.DynamicFriction = 0.5 +world.AddBody(body) + +body.AddForce(mgl64.Vec3{10, 0, 0}) // in N, during the next step +world.Step(1.0 / 60.0) +``` + +## TGS Soft +TGS Soft (or "Soft Step") is the solver of Box2D v3, described by Erin Catto in Solver2D. +It is made of substeps, soft constraints, warm starting and relaxation: ```` while simulating do + contacts ← CollectContacts(); // once per step, with speculative contacts h ← Δt/numSubsteps; + PrepareContacts(contacts); // anchors, effective masses, previous impulses for numSubsteps do - for n bodies and particles do - x_prev ← x; - v ← v + h*f_ext/m; - x ← x + h*v; + for n bodies do + v ← v + h*(g + f_ext/m); + ω ← ω + h*I⁻¹(τ_ext - ω × Iω); end - - cp ← BroadPhaseCollectCollisionPairs(); - contacts ← CollectCollisionPairs(cp); - - SolvePositions(contacts); - - for n bodies and particles do - v ← (x - x_prev)/h; + WarmStart(contacts); // apply the impulses of the previous substep + Push(contacts); // soft constraint: remove the overlap + for n bodies do + x ← x + h*v; + q ← q + h/2 * ω*q; end - - SolveVelocities(v₁,...,vₙ); + Relax(contacts); // rigid constraint + friction, removes the energy of the soft constraint end + + ApplyRestitution(contacts); + StoreImpulses(contacts); // warm start of the next step end ```` -As explained in https://matthias-research.github.io/pages/publications/PBDBodies.pdf: -"Finally, the concern regarding slow convergence was addressed -most recently in [MSL∗19]. By replacing solver iterations with -substeps, Gauss-Seidel and Jacobi methods become competitors of -global solvers in terms of convergence. Substepping in combination with one NPGS iteration per substep yields a method that looks -computationally almost identical to an explicit integration step, but -with the advantage of being unconditionally stable due to the usage -of compliance. We call it a quasi-explicit method.". - -- It means we can remove the iterations of the solver, using substepping. -- We also apply the velocity/forces first, per body, and then look at if any constraint/collision. - -Note: XPBD computes the position, with an implicit velocity. But this paradigm stops for the friction and the restitution forces. -The last computation in the substeps is SolveVelocities, a dedicated and required step to compute any optional force. -This given velocity is computed by the constraints, and then used in the next world.Step. +- The contacts are computed only once per step: during the substeps, the separation of each contact point is updated from the motion of both bodies. +- The soft constraint is a spring + damper, set with a frequency (`World.ContactHertz`, 30 Hz by default, as Box2D v3.1) and a damping ratio. +- Contacts exist before the bodies touch (speculative contacts), so fast bodies don't go through thin walls. +- Friction follows Coulomb's law: static friction when the contact sticks, dynamic friction when it slides. +- The simulation is deterministic: same result bit for bit, whatever the number of `Workers`. +- The solver is parallel: the contacts are split into colors (graph coloring), the contacts of a color don't share any body. +- A step doesn't allocate memory (after the first steps). +- The bodies touching each other sleep and wake up together (islands). + +### Why not XPBD anymore +Up to v0.2.0, Feather used a simplified XPBD solver. The same scenes (`bench/`, 50 Hz, 12 substeps): + +| Scene | v0.2.0 (XPBD) | TGS Soft | Expected | +|---|---|---|---| +| Pyramid of 55 boxes, 3 s | explodes (top box at 134 m) | stands (4.748 m) | 4.750 m | +| Box on a 20° slope, µ = 0.6 | slides 9.9 m | 0 m | 0 m | +| Box on a 35° slope, µ = 0.3 | slides 16.9 m | 9.654 m | 9.648 m | +| Bounce from 1 m, restitution 0.5 | 0.06 m | 0.24 m | 0.25 m | +| 10 N during 1 s on 32.7 kg | 15279 m/s | 0.306 m/s | 0.306 m/s | +| Same scene, run twice | 39/40 bodies differ | identical | identical | +| EPA sphere-box normal (p99) | 2.7° | 0.03° | 0° | +| Step, 10 / 100 / 500 bodies resting on the ground (one layer of boxes & spheres), 1 worker | 0.41 / 1.94 / 8.8 ms | 0.03 / 0.27 / 1.28 ms | | + +``` +cd bench +go run . # current version +go run -tags v020 -modfile=go.v020.mod . # v0.2.0 +``` + +Both versions run 12 substeps at 50 Hz. A heavier scene, 500 boxes & spheres falling on each other (`BenchmarkWorldStep`), +takes ~5.5 ms per step on 1 worker, ~1.9 ms on 8 workers. ### Constraints -- ContactConstraint: temporary constraint, generated when a collision is detected between two rigid bodies. - -A not exhaustive list of possible constraints (not implemented yet): -- Friction: Opposes tangential motion at contact points. Usage: Realistic sliding, grip, objects staying on slopes -- Manifold (multi point contact): multiple contact points. Usage: Stacking stable, boxes -- Distance: Maintains constant distance between two points. Usage: Ropes, chains, rigid connections, ragdoll bones -- Distance Range: Keeps distance within [min, max] range. Usage: Elastic ropes, springs with limits, telescopic joints -- Hinge: Allows rotation around one axis only (like a door). Usage: Doors, wheels, joints, rotating platforms -- Angular Range: limits rotation within [min/max]. Usage: articulation +- Contact: generated when a collision is detected between two rigid bodies, up to 4 points (manifold), with friction, + rolling resistance and restitution. +- Distance: fixed length, a range [min, max] (a rope), or a spring. Usage: ropes, chains, springs +- Ball (ball and socket): the anchors stay together, with an optional elliptic cone for the swing and a range for the twist, + and an optional drive towards a target rotation. Usage: ragdolls, physical bones, tails +- Hinge: rotation around one axis only, with an optional angle range, motor and spring. Usage: doors, wheels, knees +- Fixed: the position and the rotation of the 2 bodies are frozen together +- Configurable: each of the 6 axes is locked, limited or free, with optional drives. Usage: sliders, shoulders, vehicles, + anything the other joints don't cover + +```go +hinge := feather.NewHingeJoint(frame, door, mgl64.Vec3{0, 1, 0}, mgl64.Vec3{0, 1, 0}) // anchor, axis (world space) +hinge.EnableLimit = true +hinge.LowerAngle, hinge.UpperAngle = -math.Pi/2, math.Pi/2 +world.AddJoint(hinge) +``` ## GJK +Detects if two convex shapes overlap. With a margin, it also detects the shapes closer than the margin (speculative contacts). ## EPA +Computes the penetration depth, the normal and the witness points. The contact points are then clipped between the faces +of both shapes (Sutherland-Hodgman), each point with its own separation. + +See [ALGORITHMS.md](ALGORITHMS.md), [ARCHITECTURE.md](ARCHITECTURE.md) and the [physics guide](PHYSICS_GUIDE.md). + +## Tests & benchmarks +- **Minimal scenes** (`scenes_test.go`): one mechanism each (a box landing on a corner, a capsule spinning like a top, + a sphere in a V...), with a bound derived from the engine (`LinearSlop`), never from a measure. +- **Invariants** (`invariants_test.go`): 60 random scenes checked at every step: finite values, unit quaternions, + 1 and 8 workers giving the same bits, no body in a plane, no energy gained, momentum & angular momentum kept in + free flight. +- **Scenes of Solver2D** (`bench/scenes`): the samples of Erin Catto's Solver2D in 3D, each checked, and compared to + Box2D v3.1 on the same scenes (the reference: Feather must do at least as well; the known gaps are followed by #821). +- **Regressions** (`bench/`): 6 scenes (piles, pyramid, joint chain, rain on a terrain) and the scenes of Solver2D against a committed reference: + fingerprint, quality and speed per phase (`World.Profile`). +```` +go test ./... +cd bench && go run . -check # exit 1 on a regression +cd bench && go run . -update # after a wanted change +cd bench && go test ./... # the scenes of Solver2D +cd bench && go run . -scenes # the scenes at full size (-compare: with v0.2.0) +```` ## Sources -- https://matthias-research.github.io/pages/publications/PBDBodies.pdf -- https://matthias-research.github.io/pages/publications/smallsteps.pdf -- https://matthias-research.github.io/pages/tenMinutePhysics/09-xpbd.pdf -- https://matthias-research.github.io/pages/tenMinutePhysics/22-rigidBodies.pdf -- https://blog.mmacklin.com/2016/10/12/xpbd-slides-and-stiffness/ -- https://johanhelsing.studio/posts/bevy-xpbd/ +- https://box2d.org/posts/2024/02/solver2d/ +- https://github.com/erincatto/box2d (v3) +- https://box2d.org/files/ErinCatto_SoftConstraints_GDC2011.pdf +- https://box2d.org/files/ErinCatto_NumericalMethods_GDC2015.pdf (gyroscopic torque) +- https://github.com/bepu/bepuphysics2 - https://cse442-17f.github.io/Gilbert-Johnson-Keerthi-Distance-Algorithm/ -- https://medium.com/@mbayburt/walkthrough-of-the-gjk-collision-detection-algorithm-80823ef5c774 (this method seems valid only for 2D) - https://winter.dev/articles/epa-algorithm +- Christer Ericson, Real-Time Collision Detection (2004) +- https://github.com/jrouwe/JoltPhysics (active edges, contact patches, body pair cache) +- W. J. Stronge, Impact Mechanics (2000): Poisson's hypothesis for the restitution +- Brian Mirtich, Impulse-based Dynamic Simulation of Rigid Body Systems (1996): conservative advancement +- Solver2D, the samples of the reference scenes: https://github.com/erincatto/solver2d (MIT) +- PhysX speculative CCD & Unity "Continuous Speculative": https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/docs/AdvancedCollisionDetection.html + +## Acknowledgements +Feather is written from the publications and the documentation of these projects: +- [Box2D](https://github.com/erincatto/box2d), by Erin Catto: the TGS Soft solver (Solver2D, Soft Constraints), + the graph coloring, the continuous collision +- [Jolt Physics](https://github.com/jrouwe/JoltPhysics), by Jorrit Rouwe: the active edges of the terrains, + the contact patches, the body pair cache ## Contributing Guidelines diff --git a/actor/aabb.go b/actor/aabb.go index 7d2f236..f78a78b 100644 --- a/actor/aabb.go +++ b/actor/aabb.go @@ -18,7 +18,7 @@ func (a AABB) ContainsPoint(point mgl64.Vec3) bool { // Overlaps checks if two AABBs overlap func (a AABB) Overlaps(other AABB) bool { // AABBs overlap if they overlap on all three axes - return a.Max.X() >= other.Min.X() && a.Min.X() <= other.Max.X() && - a.Max.Y() >= other.Min.Y() && a.Min.Y() <= other.Max.Y() && - a.Max.Z() >= other.Min.Z() && a.Min.Z() <= other.Max.Z() + return a.Max[0] >= other.Min[0] && a.Min[0] <= other.Max[0] && + a.Max[1] >= other.Min[1] && a.Min[1] <= other.Max[1] && + a.Max[2] >= other.Min[2] && a.Min[2] <= other.Max[2] } diff --git a/actor/capsule.go b/actor/capsule.go new file mode 100644 index 0000000..cbc3d54 --- /dev/null +++ b/actor/capsule.go @@ -0,0 +1,132 @@ +package actor + +import ( + "math" + + "github.com/go-gl/mathgl/mgl64" +) + +const ( + // capsuleSideFeatureTolerance: if the direction is perpendicular to the axis (~1.1°), + // the contact feature is the whole side line, otherwise a single point on a cap + capsuleSideFeatureTolerance = 0.02 + + capsuleDirectionEpsilon = 1e-12 +) + +// Capsule is a cylinder with 2 hemispheres. +// The segment is along the local Y axis, from -HalfHeight to +HalfHeight: the total height is 2*(HalfHeight+Radius) +type Capsule struct { + HalfHeight float64 // Half length of the inner segment (cylinder part) + Radius float64 // Radius of the cylinder and of both caps +} + +// Segment returns both ends of the segment in world space (bottom, then top) +func (c *Capsule) Segment(transform Transform) (mgl64.Vec3, mgl64.Vec3) { + axis := transform.Rotation.Rotate(mgl64.Vec3{0, c.HalfHeight, 0}) + return transform.Position.Sub(axis), transform.Position.Add(axis) +} + +func (c *Capsule) ComputeAABB(transform Transform) AABB { + axis := transform.Rotation.Rotate(mgl64.Vec3{0, c.HalfHeight, 0}) + extent := mgl64.Vec3{ + math.Abs(axis.X()) + c.Radius, + math.Abs(axis.Y()) + c.Radius, + math.Abs(axis.Z()) + c.Radius, + } + + return AABB{ + Min: transform.Position.Sub(extent), + Max: transform.Position.Add(extent), + } +} + +// ComputeMass: cylinder + 1 full sphere +func (c *Capsule) ComputeMass(density float64) float64 { + return density * c.volume() +} + +func (c *Capsule) volume() float64 { + r := c.Radius + cylinder := math.Pi * r * r * 2 * c.HalfHeight + sphere := 4.0 / 3.0 * math.Pi * r * r * r + return cylinder + sphere +} + +// ComputeInertia: the mass is split between the cylinder (h = 2*HalfHeight) and both hemispheres, by volume +// +// I_yy = mCylinder r²/2 + mCaps 2r²/5 +// I_xx = I_zz = mCylinder (r²/4 + h²/12) + mCaps (2r²/5 + h²/4 + 3hr/8) +// +// Parallel axis theorem for the caps: the centroid of a hemisphere is at 3r/8 from its flat face +func (c *Capsule) ComputeInertia(mass float64) mgl64.Mat3 { + r := c.Radius + h := 2 * c.HalfHeight + + volume := c.volume() + if volume <= 0 { + return mgl64.Mat3{} + } + cylinderMass := mass * (math.Pi * r * r * h) / volume + capsMass := mass - cylinderMass + + axial := cylinderMass*r*r/2 + capsMass*2*r*r/5 + transverse := cylinderMass*(r*r/4+h*h/12) + capsMass*(2*r*r/5+h*h/4+3*h*r/8) + + return mgl64.Mat3{ + transverse, 0, 0, + 0, axial, 0, + 0, 0, transverse, + } +} + +func (c *Capsule) Support(direction mgl64.Vec3) mgl64.Vec3 { + end := mgl64.Vec3{0, c.HalfHeight, 0} + if direction.Y() < 0 { + end[1] = -c.HalfHeight + } + + length := direction.Len() + if length < capsuleDirectionEpsilon { + return end.Add(mgl64.Vec3{0, c.Radius, 0}) + } + + return end.Add(direction.Mul(c.Radius / length)) +} + +// GetContactFeature returns the side line (2 points) if the direction is perpendicular to the axis, +// otherwise the support point on a cap +func (c *Capsule) GetContactFeature(direction mgl64.Vec3, output *[8]mgl64.Vec3, count *int) { + length := direction.Len() + if c.HalfHeight > 0 && length >= capsuleDirectionEpsilon && math.Abs(direction.Y())/length < capsuleSideFeatureTolerance { + radial := mgl64.Vec3{direction.X(), 0, direction.Z()} + radial = radial.Mul(c.Radius / radial.Len()) + + output[0] = radial.Add(mgl64.Vec3{0, c.HalfHeight, 0}) + output[1] = radial.Sub(mgl64.Vec3{0, c.HalfHeight, 0}) + *count = 2 + return + } + + output[0] = c.Support(direction) + *count = 1 +} + +// CollideWithPlane tests both caps: a lying capsule gets 2 contacts, so it does not roll +func (c *Capsule) CollideWithPlane(planeNormal mgl64.Vec3, planeDistance float64, myTransform Transform, margin float64, contacts PlaneContact) PlaneContact { + bottom, top := c.Segment(myTransform) + + for _, end := range [2]mgl64.Vec3{bottom, top} { + separation := end.Dot(planeNormal) + planeDistance - c.Radius + if separation > margin { + continue + } + + contacts = append(contacts, ContactPoint{ + Position: end.Sub(planeNormal.Mul(c.Radius + separation/2)), + Separation: separation, + }) + } + + return contacts +} diff --git a/actor/capsule_test.go b/actor/capsule_test.go new file mode 100644 index 0000000..ff222e2 --- /dev/null +++ b/actor/capsule_test.go @@ -0,0 +1,346 @@ +package actor + +import ( + "math" + "testing" + + "github.com/go-gl/mathgl/mgl64" +) + +// capsuleTransform builds a transform with a consistent inverse rotation. +func capsuleTransform(position mgl64.Vec3, rotation mgl64.Quat) Transform { + return Transform{Position: position, Rotation: rotation} +} + +// lyingAlongX rotates the local Y axis onto the world X axis. +var lyingAlongX = mgl64.QuatRotate(-math.Pi/2, mgl64.Vec3{0, 0, 1}) + +func TestCapsuleComputeMass(t *testing.T) { + c := &Capsule{HalfHeight: 1.5, Radius: 0.4} + density := 2.5 + + cylinder := math.Pi * 0.4 * 0.4 * 3.0 + sphere := 4.0 / 3.0 * math.Pi * 0.4 * 0.4 * 0.4 + want := density * (cylinder + sphere) + + if got := c.ComputeMass(density); math.Abs(got-want) > 1e-12 { + t.Errorf("ComputeMass = %.15f, want %.15f", got, want) + } +} + +// expectedCapsuleInertia derives the inertia with the parallel-axis theorem, independently +// of the implementation: a solid cylinder plus two hemispheres whose centroids sit +// 3r/8 away from their flat faces. +func expectedCapsuleInertia(mass, halfHeight, radius float64) (axial, transverse float64) { + height := 2 * halfHeight + cylinderVolume := math.Pi * radius * radius * height + hemisphereVolume := 2.0 / 3.0 * math.Pi * radius * radius * radius + totalVolume := cylinderVolume + 2*hemisphereVolume + + cylinderMass := mass * cylinderVolume / totalVolume + hemisphereMass := mass * hemisphereVolume / totalVolume + + centroidOffset := 3.0 * radius / 8.0 + hemisphereAboutFlatFace := 2.0 / 5.0 * hemisphereMass * radius * radius + hemisphereAboutCentroid := hemisphereAboutFlatFace - hemisphereMass*centroidOffset*centroidOffset + hemisphereDistance := halfHeight + centroidOffset + + axial = cylinderMass*radius*radius/2 + 2*hemisphereAboutFlatFace + transverse = cylinderMass*(radius*radius/4+height*height/12) + + 2*(hemisphereAboutCentroid+hemisphereMass*hemisphereDistance*hemisphereDistance) + return axial, transverse +} + +// integrateCapsuleInertia integrates the inertia slice by slice along the axis with a +// composite Simpson rule. Each slice is a disk of radius rho(y); the integrands are +// piecewise polynomials, so the quadrature converges far below 1e-9. +func integrateCapsuleInertia(mass, halfHeight, radius float64) (axial, transverse float64) { + rhoSquared := func(y float64) float64 { + overshoot := math.Abs(y) - halfHeight + if overshoot <= 0 { + return radius * radius + } + return math.Max(0, radius*radius-overshoot*overshoot) + } + simpson := func(f func(float64) float64, from, to float64) float64 { + const intervals = 2000 + step := (to - from) / intervals + sum := f(from) + f(to) + for i := 1; i < intervals; i++ { + weight := 2.0 + if i%2 == 1 { + weight = 4.0 + } + sum += weight * f(from+float64(i)*step) + } + return sum * step / 3 + } + integrate := func(f func(float64) float64) float64 { + return simpson(f, -halfHeight-radius, -halfHeight) + + simpson(f, -halfHeight, halfHeight) + + simpson(f, halfHeight, halfHeight+radius) + } + + volume := integrate(func(y float64) float64 { return math.Pi * rhoSquared(y) }) + density := mass / volume + axial = density * integrate(func(y float64) float64 { + r2 := rhoSquared(y) + return math.Pi * r2 * r2 / 2 + }) + transverse = density * integrate(func(y float64) float64 { + r2 := rhoSquared(y) + return math.Pi*r2*r2/4 + math.Pi*r2*y*y + }) + return axial, transverse +} + +func TestCapsuleComputeInertia(t *testing.T) { + const tolerance = 1e-9 + + cases := []struct { + name string + mass, half, radius float64 + }{ + {"unit", 1, 1, 0.5}, + {"character", 80, 0.6, 0.3}, + {"thin bone", 0.25, 2, 0.05}, + {"stubby", 12, 0.1, 1.2}, + {"sphere limit", 3, 0, 0.7}, + } + + for _, tc := range cases { + t.Run(tc.name, func(t *testing.T) { + c := &Capsule{HalfHeight: tc.half, Radius: tc.radius} + got := c.ComputeInertia(tc.mass) + + axial, transverse := expectedCapsuleInertia(tc.mass, tc.half, tc.radius) + want := mgl64.Mat3{transverse, 0, 0, 0, axial, 0, 0, 0, transverse} + if !mat3Equal(got, want, tolerance) { + t.Errorf("ComputeInertia = %v, closed form = %v", got, want) + } + + axialNum, transverseNum := integrateCapsuleInertia(tc.mass, tc.half, tc.radius) + if math.Abs(got.At(1, 1)-axialNum) > tolerance || math.Abs(got.At(0, 0)-transverseNum) > tolerance || + math.Abs(got.At(2, 2)-transverseNum) > tolerance { + t.Errorf("ComputeInertia = %v, numerical integration axial=%.12f transverse=%.12f", + got, axialNum, transverseNum) + } + }) + } +} + +func TestCapsuleInertiaSphereLimit(t *testing.T) { + c := &Capsule{HalfHeight: 0, Radius: 0.8} + s := &Sphere{Radius: 0.8} + if !mat3Equal(c.ComputeInertia(5), s.ComputeInertia(5), 1e-12) { + t.Errorf("zero-height capsule inertia %v differs from sphere %v", c.ComputeInertia(5), s.ComputeInertia(5)) + } +} + +func TestCapsuleComputeAABB(t *testing.T) { + c := &Capsule{HalfHeight: 1, Radius: 0.5} + + got := c.ComputeAABB(capsuleTransform(mgl64.Vec3{1, 2, 3}, mgl64.QuatIdent())) + want := AABB{Min: mgl64.Vec3{0.5, 0.5, 2.5}, Max: mgl64.Vec3{1.5, 3.5, 3.5}} + if !vec3Equal(got.Min, want.Min, 1e-12) || !vec3Equal(got.Max, want.Max, 1e-12) { + t.Errorf("upright AABB = %v, want %v", got, want) + } + + got = c.ComputeAABB(capsuleTransform(mgl64.Vec3{0, 0, 0}, lyingAlongX)) + want = AABB{Min: mgl64.Vec3{-1.5, -0.5, -0.5}, Max: mgl64.Vec3{1.5, 0.5, 0.5}} + if !vec3Equal(got.Min, want.Min, 1e-12) || !vec3Equal(got.Max, want.Max, 1e-12) { + t.Errorf("lying AABB = %v, want %v", got, want) + } + + // 45° around Z: the segment end sits at (±sqrt(2)/2, ±sqrt(2)/2, 0). + got = c.ComputeAABB(capsuleTransform(mgl64.Vec3{0, 0, 0}, mgl64.QuatRotate(math.Pi/4, mgl64.Vec3{0, 0, 1}))) + e := math.Sqrt2/2 + 0.5 + want = AABB{Min: mgl64.Vec3{-e, -e, -0.5}, Max: mgl64.Vec3{e, e, 0.5}} + if !vec3Equal(got.Min, want.Min, 1e-12) || !vec3Equal(got.Max, want.Max, 1e-12) { + t.Errorf("tilted AABB = %v, want %v", got, want) + } +} + +func TestCapsuleSupport(t *testing.T) { + c := &Capsule{HalfHeight: 1, Radius: 0.5} + + cases := []struct { + direction, want mgl64.Vec3 + }{ + {mgl64.Vec3{0, 1, 0}, mgl64.Vec3{0, 1.5, 0}}, + {mgl64.Vec3{0, -3, 0}, mgl64.Vec3{0, -1.5, 0}}, + {mgl64.Vec3{1, 1, 0}, mgl64.Vec3{0.5 * math.Sqrt2 / 2, 1 + 0.5*math.Sqrt2/2, 0}}, + {mgl64.Vec3{0, -1, -1}, mgl64.Vec3{0, -1 - 0.5*math.Sqrt2/2, -0.5 * math.Sqrt2 / 2}}, + } + for _, tc := range cases { + if got := c.Support(tc.direction); !vec3Equal(got, tc.want, 1e-12) { + t.Errorf("Support(%v) = %v, want %v", tc.direction, got, tc.want) + } + } + + // Perpendicular direction: any point of the side line is a valid support, but it + // must lie on that line. + got := c.Support(mgl64.Vec3{2, 0, 0}) + if math.Abs(got.X()-0.5) > 1e-12 || math.Abs(got.Z()) > 1e-12 || math.Abs(got.Y()) > 1+1e-12 { + t.Errorf("Support(+X) = %v, want a point of the line x=0.5, |y|<=1", got) + } + + // A zero direction must not produce NaN. + for _, v := range c.Support(mgl64.Vec3{}) { + if math.IsNaN(v) { + t.Fatalf("Support(0) = %v, must not be NaN", c.Support(mgl64.Vec3{})) + } + } +} + +// TestCapsuleSupportIsExtreme checks the support property against sampled surface points. +func TestCapsuleSupportIsExtreme(t *testing.T) { + c := &Capsule{HalfHeight: 0.7, Radius: 0.3} + directions := []mgl64.Vec3{{1, 0.2, 0}, {-0.3, 1, 0.5}, {0.1, -1, -0.2}, {0, 0.05, -1}} + + for _, d := range directions { + support := c.Support(d).Dot(d) + for i := 0; i <= 40; i++ { + y := -0.7 + 1.4*float64(i)/40 + for j := 0; j < 36; j++ { + angle := float64(j) * math.Pi / 18 + for _, sphereY := range []float64{-1, 0, 1} { + // Points of the cylinder side and of the caps' equators. + p := mgl64.Vec3{0.3 * math.Cos(angle), y, 0.3 * math.Sin(angle)} + if sphereY != 0 { + p = mgl64.Vec3{0, sphereY * 0.7, 0}.Add(mgl64.Vec3{math.Cos(angle), sphereY, math.Sin(angle)}.Normalize().Mul(0.3)) + } + if p.Dot(d) > support+1e-12 { + t.Fatalf("Support(%v)·d = %f but surface point %v gives %f", d, support, p, p.Dot(d)) + } + } + } + } + } +} + +func TestCapsuleGetContactFeature(t *testing.T) { + c := &Capsule{HalfHeight: 1, Radius: 0.5} + var output [8]mgl64.Vec3 + var count int + + // Side contact: the feature is the side line of the cylinder. + c.GetContactFeature(mgl64.Vec3{0, 0, -2}, &output, &count) + if count != 2 { + t.Fatalf("side feature count = %d, want 2", count) + } + top, bottom := output[0], output[1] + if top.Y() < bottom.Y() { + top, bottom = bottom, top + } + if !vec3Equal(top, mgl64.Vec3{0, 1, -0.5}, 1e-12) || !vec3Equal(bottom, mgl64.Vec3{0, -1, -0.5}, 1e-12) { + t.Errorf("side feature = %v %v, want (0,±1,-0.5)", output[0], output[1]) + } + + // Cap contact: a single point. + c.GetContactFeature(mgl64.Vec3{0, -1, 0}, &output, &count) + if count != 1 || !vec3Equal(output[0], mgl64.Vec3{0, -1.5, 0}, 1e-12) { + t.Errorf("cap feature = %v (count %d), want (0,-1.5,0)", output[0], count) + } + + // Oblique contact: a single support point. + d := mgl64.Vec3{1, 1, 0} + c.GetContactFeature(d, &output, &count) + if count != 1 || !vec3Equal(output[0], c.Support(d), 1e-12) { + t.Errorf("oblique feature = %v (count %d), want %v", output[0], count, c.Support(d)) + } +} + +func TestCapsuleSegment(t *testing.T) { + c := &Capsule{HalfHeight: 2, Radius: 0.1} + a, b := c.Segment(capsuleTransform(mgl64.Vec3{1, 1, 1}, lyingAlongX)) + if !vec3Equal(a, mgl64.Vec3{-1, 1, 1}, 1e-12) || !vec3Equal(b, mgl64.Vec3{3, 1, 1}, 1e-12) { + t.Errorf("Segment = %v %v, want (-1,1,1) (3,1,1)", a, b) + } +} + +func TestCapsuleCollideWithPlane(t *testing.T) { + c := &Capsule{HalfHeight: 1, Radius: 0.5} + up := mgl64.Vec3{0, 1, 0} + + // Contacts lie halfway between the capsule surface and the plane; the separation is + // negative when they overlap. + t.Run("upright on its cap", func(t *testing.T) { + contacts := c.CollideWithPlane(up, 0, capsuleTransform(mgl64.Vec3{0, 1.4, 0}, mgl64.QuatIdent()), 0, nil) + ok := len(contacts) > 0 + if !ok || len(contacts) != 1 { + t.Fatalf("collision = %v, contacts = %v, want 1 contact", ok, contacts) + } + if !vec3Equal(contacts[0].Position, mgl64.Vec3{0, -0.05, 0}, 1e-12) || !floatEqual(contacts[0].Separation, -0.1, 1e-12) { + t.Errorf("contact = %+v, want (0,-0.05,0) separation -0.1", contacts[0]) + } + }) + + t.Run("lying on its side", func(t *testing.T) { + contacts := c.CollideWithPlane(up, 0, capsuleTransform(mgl64.Vec3{2, 0.45, 0}, lyingAlongX), 0, nil) + ok := len(contacts) > 0 + if !ok || len(contacts) != 2 { + t.Fatalf("collision = %v, contacts = %v, want 2 contacts", ok, contacts) + } + xs := []float64{contacts[0].Position.X(), contacts[1].Position.X()} + if xs[0] > xs[1] { + xs[0], xs[1] = xs[1], xs[0] + } + if !floatEqual(xs[0], 1, 1e-12) || !floatEqual(xs[1], 3, 1e-12) { + t.Errorf("contact x = %v, want [1 3]", xs) + } + for _, p := range contacts { + if !floatEqual(p.Position.Y(), -0.025, 1e-12) || !floatEqual(p.Separation, -0.05, 1e-12) { + t.Errorf("contact = %+v, want y=-0.025 separation -0.05", p) + } + } + }) + + t.Run("tilted: only the lower cap touches", func(t *testing.T) { + rotation := mgl64.QuatRotate(math.Pi/4, mgl64.Vec3{0, 0, 1}) + // Lower segment end at (sqrt2/2, -sqrt2/2 + y) ; put it 0.4 above the plane. + y := math.Sqrt2/2 + 0.4 + contacts := c.CollideWithPlane(up, 0, capsuleTransform(mgl64.Vec3{0, y, 0}, rotation), 0, nil) + ok := len(contacts) > 0 + if !ok || len(contacts) != 1 { + t.Fatalf("collision = %v, contacts = %v, want 1 contact", ok, contacts) + } + if !vec3Equal(contacts[0].Position, mgl64.Vec3{math.Sqrt2 / 2, -0.05, 0}, 1e-12) || !floatEqual(contacts[0].Separation, -0.1, 1e-12) { + t.Errorf("contact = %+v, want (0.707,-0.05,0) separation -0.1", contacts[0]) + } + }) + + t.Run("offset oblique plane", func(t *testing.T) { + // Plane (x + y)/sqrt2 = -1, i.e. Normal·p + Distance = 0 with Distance = 1. + n := mgl64.Vec3{1, 1, 0}.Normalize() + contacts := c.CollideWithPlane(n, 1, capsuleTransform(mgl64.Vec3{0, 0, 0}, mgl64.QuatIdent()), 0, nil) + ok := len(contacts) > 0 + if !ok || len(contacts) != 1 { + t.Fatalf("collision = %v, contacts = %v, want 1 contact", ok, contacts) + } + // Only the lower end (0,-1,0) is closer than the radius: signed distance 1 - sqrt2/2. + distance := 1 - math.Sqrt2/2 + separation := distance - 0.5 + want := mgl64.Vec3{0, -1, 0}.Sub(n.Mul(0.5 + separation/2)) + if !vec3Equal(contacts[0].Position, want, 1e-12) || !floatEqual(contacts[0].Separation, separation, 1e-12) { + t.Errorf("contact = %+v, want %v separation %f", contacts[0], want, separation) + } + }) + + t.Run("above the plane", func(t *testing.T) { + if contacts := c.CollideWithPlane(up, 0, capsuleTransform(mgl64.Vec3{0, 1.6, 0}, mgl64.QuatIdent()), 0, nil); len(contacts) != 0 { + t.Errorf("capsule above the plane reported a contact %v", contacts) + } + }) + + t.Run("speculative: within the margin", func(t *testing.T) { + contacts := c.CollideWithPlane(up, 0, capsuleTransform(mgl64.Vec3{0, 1.51, 0}, mgl64.QuatIdent()), 0.02, nil) + ok := len(contacts) > 0 + if !ok || len(contacts) != 1 || !floatEqual(contacts[0].Separation, 0.01, 1e-12) { + t.Fatalf("collision = %v, contacts = %v, want 1 speculative contact at separation 0.01", ok, contacts) + } + if contacts := c.CollideWithPlane(up, 0, capsuleTransform(mgl64.Vec3{0, 1.53, 0}, mgl64.QuatIdent()), 0.02, nil); len(contacts) > 0 { + t.Error("capsule beyond the margin reported a contact") + } + }) +} diff --git a/actor/heightfield.go b/actor/heightfield.go new file mode 100644 index 0000000..bef2b56 --- /dev/null +++ b/actor/heightfield.go @@ -0,0 +1,290 @@ +package actor + +import ( + "math" + + "github.com/go-gl/mathgl/mgl64" +) + +const ( + // HeightfieldBlockSize: the cells are grouped by blocks of 16x16, with their lowest & highest heights, + // to skip the blocks far from a body + HeightfieldBlockSize = 16 + + // activeEdgeCos: an edge between 2 triangles bent less than 5°, or bent inwards, is inactive: cos(5°) + activeEdgeCos = 0.99619469809174553229501040247389 +) + +// heightfieldNeighbors of the edges of the 2 triangles of the cell (x, z): the cell of the triangle on the other side +// of the edge, and the vertex of this triangle opposite to the edge (offsets from the sample (x, z)) +var heightfieldNeighbors = [2][3]struct{ cellX, cellZ, triangle, vertexX, vertexZ int }{ + {{-1, 0, 1, -1, 0}, {0, 1, 1, 1, 2}, {0, 0, 1, 1, 0}}, + {{0, 0, 0, 0, 1}, {1, 0, 0, 2, 1}, {0, -1, 0, 0, -1}}, +} + +// heightfieldTriangles: the vertices of the 2 triangles of a cell, as offsets from the sample (x, z). +// Both are split along the diagonal from (x, z) to (x+1, z+1), their normals point up (+Y) +var heightfieldTriangles = [2][3][2]int{ + {{0, 0}, {0, 1}, {1, 1}}, + {{0, 0}, {1, 1}, {1, 0}}, +} + +// Heightfield is a static terrain: a grid of heights, 2 triangles per cell. +// Heights[x*ZSamples+z] is the height of the sample (x, z): the grid of the terrains of AkmonEngine, shared without copy. +// In the local space, the sample (x, z) is at +// +// ((x - (XSamples-1)/2) * Scale.X, height * Scale.Y, (z - (ZSamples-1)/2) * Scale.Z) +// +// the body is at the center of the terrain. The terrain is a surface: the bodies collide with its top side. +// After a change of Heights or Holes, call Update on the changed samples. +type Heightfield struct { + XSamples int + ZSamples int + Heights []float32 + // Scale: X & Z are the distances between 2 samples (m), Y multiplies the heights + Scale mgl64.Vec3 + // Holes of the cells (optional), Holes[x*(ZSamples-1)+z]: a hole has no triangle + Holes []bool + + blocksZ int + blocks []heightBlock + // edges of the cells, 6 bits per triangle: active, then convex + edges []uint16 + minHeight float64 + maxHeight float64 +} + +// heightBlock: lowest & highest heights of the samples of a block (local) +type heightBlock struct { + min float64 + max float64 +} + +// NewHeightfield: xSamples*zSamples heights, at least 2x2 samples +func NewHeightfield(xSamples, zSamples int, heights []float32, scale mgl64.Vec3) *Heightfield { + if xSamples < 2 || zSamples < 2 || len(heights) != xSamples*zSamples { + panic("feather: a heightfield needs xSamples*zSamples heights, at least 2x2") + } + h := &Heightfield{XSamples: xSamples, ZSamples: zSamples, Heights: heights, Scale: scale} + h.Update(0, 0, xSamples-1, zSamples-1) + return h +} + +// Update the blocks & the active edges around the samples [minX, maxX] x [minZ, maxZ], after a change of the heights or +// of the holes. The bodies resting on the terrain must be woken up (World.UpdateHeightfield does both) +func (h *Heightfield) Update(minX, minZ, maxX, maxZ int) { + cellsX, cellsZ := h.XSamples-1, h.ZSamples-1 + blocksX := (cellsX + HeightfieldBlockSize - 1) / HeightfieldBlockSize + h.blocksZ = (cellsZ + HeightfieldBlockSize - 1) / HeightfieldBlockSize + if len(h.blocks) != blocksX*h.blocksZ { + h.blocks = make([]heightBlock, blocksX*h.blocksZ) + h.edges = make([]uint16, cellsX*cellsZ) + minX, minZ, maxX, maxZ = 0, 0, h.XSamples-1, h.ZSamples-1 + } + minX, minZ = max(0, minX), max(0, minZ) + maxX, maxZ = min(h.XSamples-1, maxX), min(h.ZSamples-1, maxZ) + + // ========== BLOCKS ========== + // a sample is shared by the blocks around it + for bx := max(0, (minX-1)/HeightfieldBlockSize); bx <= min(blocksX-1, maxX/HeightfieldBlockSize); bx++ { + for bz := max(0, (minZ-1)/HeightfieldBlockSize); bz <= min(h.blocksZ-1, maxZ/HeightfieldBlockSize); bz++ { + block := heightBlock{min: math.Inf(1), max: math.Inf(-1)} + for x := bx * HeightfieldBlockSize; x <= min(h.XSamples-1, (bx+1)*HeightfieldBlockSize); x++ { + for z := bz * HeightfieldBlockSize; z <= min(h.ZSamples-1, (bz+1)*HeightfieldBlockSize); z++ { + height := h.height(x, z) + block.min = math.Min(block.min, height) + block.max = math.Max(block.max, height) + } + } + h.blocks[bx*h.blocksZ+bz] = block + } + } + h.minHeight, h.maxHeight = math.Inf(1), math.Inf(-1) + for _, block := range h.blocks { + h.minHeight = math.Min(h.minHeight, block.min) + h.maxHeight = math.Max(h.maxHeight, block.max) + } + + // ========== ACTIVE EDGES ========== + // the edges of the cells around the changed samples, and of their neighbors + for x := max(0, minX-2); x <= min(cellsX-1, maxX+1); x++ { + for z := max(0, minZ-2); z <= min(cellsZ-1, maxZ+1); z++ { + h.edges[x*cellsZ+z] = h.cellEdges(x, z) + } + } +} + +// cellEdges: an edge is convex if the neighbor triangle bends down, or if there is no neighbor on this side +// (border, hole). A convex edge is active if it bends by more than 5°: only the active edges can push a body sideways +func (h *Heightfield) cellEdges(x, z int) uint16 { + var edges uint16 + for t := 0; t < 2; t++ { + triangle := h.localTriangle(x, z, t) + normal := triangleNormal(triangle) + for e, neighbor := range heightfieldNeighbors[t] { + active, convex := uint16(1)<<(t*6+e), uint16(1)<<(t*6+3+e) + cellX, cellZ := x+neighbor.cellX, z+neighbor.cellZ + if !h.hasCell(cellX, cellZ) { + edges |= active | convex + continue + } + opposite := h.localVertex(x+neighbor.vertexX, z+neighbor.vertexZ) + if opposite.Sub(triangle[e]).Dot(normal) >= 0 { + continue + } + edges |= convex + if normal.Dot(triangleNormal(h.localTriangle(cellX, cellZ, neighbor.triangle))) < activeEdgeCos { + edges |= active + } + } + } + return edges +} + +func (h *Heightfield) height(x, z int) float64 { + return float64(h.Heights[x*h.ZSamples+z]) * h.Scale.Y() +} + +// hasCell: the cell exists and is not a hole +func (h *Heightfield) hasCell(x, z int) bool { + if x < 0 || z < 0 || x >= h.XSamples-1 || z >= h.ZSamples-1 { + return false + } + return h.Holes == nil || !h.Holes[x*(h.ZSamples-1)+z] +} + +func (h *Heightfield) localVertex(x, z int) mgl64.Vec3 { + return mgl64.Vec3{ + (float64(x) - float64(h.XSamples-1)/2) * h.Scale.X(), + h.height(x, z), + (float64(z) - float64(h.ZSamples-1)/2) * h.Scale.Z(), + } +} + +func (h *Heightfield) localTriangle(x, z, t int) [3]mgl64.Vec3 { + var triangle [3]mgl64.Vec3 + for i, offset := range heightfieldTriangles[t] { + triangle[i] = h.localVertex(x+offset[0], z+offset[1]) + } + return triangle +} + +func triangleNormal(triangle [3]mgl64.Vec3) mgl64.Vec3 { + return triangle[1].Sub(triangle[0]).Cross(triangle[2].Sub(triangle[0])).Normalize() +} + +// Triangle t (0 or 1) of the cell (x, z), in the local space, with its edges: bit e if the edge from the vertex e +// to the vertex e+1 is active, bit 3+e if it is convex +func (h *Heightfield) Triangle(x, z, t int) ([3]mgl64.Vec3, uint8) { + return h.localTriangle(x, z, t), uint8(h.edges[x*(h.ZSamples-1)+z]>>(t*6)) & 0b111111 +} + +// OverlapCells appends to cells the index x*(ZSamples-1)+z of the cells which may touch the local bounds: +// the cells under the bounds, without the holes, whose block and heights overlap the bounds +func (h *Heightfield) OverlapCells(bounds AABB, cells []int32) []int32 { + halfX, halfZ := float64(h.XSamples-1)/2, float64(h.ZSamples-1)/2 + minX := max(0, int(math.Floor(bounds.Min.X()/h.Scale.X()+halfX))) + maxX := min(h.XSamples-2, int(math.Floor(bounds.Max.X()/h.Scale.X()+halfX))) + minZ := max(0, int(math.Floor(bounds.Min.Z()/h.Scale.Z()+halfZ))) + maxZ := min(h.ZSamples-2, int(math.Floor(bounds.Max.Z()/h.Scale.Z()+halfZ))) + if minX > maxX || minZ > maxZ || bounds.Min.Y() > h.maxHeight || bounds.Max.Y() < h.minHeight { + return cells + } + + cellsZ := h.ZSamples - 1 + for bx := minX / HeightfieldBlockSize; bx <= maxX/HeightfieldBlockSize; bx++ { + for bz := minZ / HeightfieldBlockSize; bz <= maxZ/HeightfieldBlockSize; bz++ { + block := h.blocks[bx*h.blocksZ+bz] + if bounds.Min.Y() > block.max || bounds.Max.Y() < block.min { + continue + } + for x := max(minX, bx*HeightfieldBlockSize); x <= min(maxX, (bx+1)*HeightfieldBlockSize-1); x++ { + for z := max(minZ, bz*HeightfieldBlockSize); z <= min(maxZ, (bz+1)*HeightfieldBlockSize-1); z++ { + if !h.hasCell(x, z) { + continue + } + h00, h01, h10, h11 := h.height(x, z), h.height(x, z+1), h.height(x+1, z), h.height(x+1, z+1) + if bounds.Min.Y() > max(h00, h01, h10, h11) || bounds.Max.Y() < min(h00, h01, h10, h11) { + continue + } + cells = append(cells, int32(x*cellsZ+z)) + } + } + } + } + return cells +} + +// HeightAt returns the height of the triangles at the local position (x, z), false outside the terrain or in a hole +func (h *Heightfield) HeightAt(x, z float64) (float64, bool) { + fx := x/h.Scale.X() + float64(h.XSamples-1)/2 + fz := z/h.Scale.Z() + float64(h.ZSamples-1)/2 + cellX, cellZ := int(math.Floor(fx)), int(math.Floor(fz)) + // the last samples belong to the last cells + if fx == float64(h.XSamples-1) { + cellX-- + } + if fz == float64(h.ZSamples-1) { + cellZ-- + } + if !h.hasCell(cellX, cellZ) { + return 0, false + } + u, v := fx-float64(cellX), fz-float64(cellZ) + h00, h01, h10, h11 := h.height(cellX, cellZ), h.height(cellX, cellZ+1), h.height(cellX+1, cellZ), h.height(cellX+1, cellZ+1) + if v >= u { + // triangle 0: (0,0), (0,1), (1,1) + return h00 + (h01-h00)*(v-u) + (h11-h00)*u, true + } + // triangle 1: (0,0), (1,1), (1,0) + return h00 + (h10-h00)*(u-v) + (h11-h00)*v, true +} + +func (h *Heightfield) ComputeAABB(transform Transform) AABB { + halfX := float64(h.XSamples-1) / 2 * h.Scale.X() + halfZ := float64(h.ZSamples-1) / 2 * h.Scale.Z() + min := mgl64.Vec3{math.Inf(1), math.Inf(1), math.Inf(1)} + max := mgl64.Vec3{math.Inf(-1), math.Inf(-1), math.Inf(-1)} + for i := 0; i < 8; i++ { + corner := mgl64.Vec3{-halfX, h.minHeight, -halfZ} + if i&1 != 0 { + corner[0] = halfX + } + if i&2 != 0 { + corner[1] = h.maxHeight + } + if i&4 != 0 { + corner[2] = halfZ + } + world := transform.ToWorld(corner) + for k := 0; k < 3; k++ { + min[k] = math.Min(min[k], world[k]) + max[k] = math.Max(max[k], world[k]) + } + } + return AABB{Min: min, Max: max} +} + +// ComputeMass: a heightfield is static +func (h *Heightfield) ComputeMass(density float64) float64 { + return math.Inf(1) +} + +func (h *Heightfield) ComputeInertia(mass float64) mgl64.Mat3 { + return mgl64.Mat3{} +} + +// Support: a heightfield is not convex, its triangles are tested one by one +func (h *Heightfield) Support(direction mgl64.Vec3) mgl64.Vec3 { + return mgl64.Vec3{} +} + +func (h *Heightfield) GetContactFeature(direction mgl64.Vec3, output *[8]mgl64.Vec3, count *int) { + output[0] = mgl64.Vec3{} + *count = 1 +} + +// CollideWithPlane - Heightfield/Plane collision (not supported) +func (h *Heightfield) CollideWithPlane(planeNormal mgl64.Vec3, planeDistance float64, myTransform Transform, margin float64, contacts PlaneContact) PlaneContact { + return contacts +} diff --git a/actor/heightfield_test.go b/actor/heightfield_test.go new file mode 100644 index 0000000..d01d2b2 --- /dev/null +++ b/actor/heightfield_test.go @@ -0,0 +1,206 @@ +package actor + +import ( + "math" + "math/rand" + "testing" + + "github.com/go-gl/mathgl/mgl64" +) + +func randomHeightfield(seed int64, xSamples, zSamples int) *Heightfield { + r := rand.New(rand.NewSource(seed)) + heights := make([]float32, xSamples*zSamples) + for i := range heights { + heights[i] = float32(r.Float64()) + } + return NewHeightfield(xSamples, zSamples, heights, mgl64.Vec3{0.5, 2, 0.25}) +} + +// The height of the triangles: the samples at the samples, a plane inside each triangle +func TestHeightfieldHeightAt(t *testing.T) { + h := randomHeightfield(1, 7, 5) + for x := 0; x < h.XSamples; x++ { + for z := 0; z < h.ZSamples; z++ { + vertex := h.localVertex(x, z) + height, ok := h.HeightAt(vertex.X(), vertex.Z()) + if !ok || math.Abs(height-vertex.Y()) > 1e-12 { + t.Fatalf("sample (%d, %d): %v %v, want %v", x, z, height, ok, vertex.Y()) + } + } + } + + r := rand.New(rand.NewSource(2)) + for i := 0; i < 1000; i++ { + x, z := r.Intn(h.XSamples-1), r.Intn(h.ZSamples-1) + triangle := h.localTriangle(x, z, r.Intn(2)) + // a random point of the triangle + a, b := r.Float64(), r.Float64() + if a+b > 1 { + a, b = 1-a, 1-b + } + p := triangle[0].Add(triangle[1].Sub(triangle[0]).Mul(a)).Add(triangle[2].Sub(triangle[0]).Mul(b)) + if height, ok := h.HeightAt(p.X(), p.Z()); !ok || math.Abs(height-p.Y()) > 1e-9 { + t.Fatalf("point %v of the triangle: height %v", p, height) + } + } + + if _, ok := h.HeightAt(10, 0); ok { + t.Error("a height outside the terrain") + } + h.Holes = make([]bool, (h.XSamples-1)*(h.ZSamples-1)) + h.Holes[2*(h.ZSamples-1)+1] = true + center := h.localVertex(2, 1).Add(h.localVertex(3, 2)).Mul(0.5) + if _, ok := h.HeightAt(center.X(), center.Z()); ok { + t.Error("a height in a hole") + } +} + +// edgesOf the triangle t of the cell (x, z): active, convex +func edgesOf(h *Heightfield, x, z, t, e int) (bool, bool) { + _, edges := h.Triangle(x, z, t) + return edges&(1<= low.X() && bounds.Min.X() < high.X() && bounds.Min.Z() < high.Z() { + want[int32(x*(h.ZSamples-1)+z)] = true + } + } + } + if len(cells) != len(want) { + t.Fatalf("bounds %v: %d cells, want %d", bounds, len(cells), len(want)) + } + for _, cell := range cells { + if !want[cell] { + t.Fatalf("bounds %v: cell %d is not under the bounds", bounds, cell) + } + } + } +} + +// Update of a region gives the same terrain as a new terrain +func TestHeightfieldUpdate(t *testing.T) { + h := randomHeightfield(5, 50, 45) + r := rand.New(rand.NewSource(6)) + for i := 0; i < 20; i++ { + minX, minZ := r.Intn(50), r.Intn(45) + maxX, maxZ := min(49, minX+r.Intn(6)), min(44, minZ+r.Intn(6)) + for x := minX; x <= maxX; x++ { + for z := minZ; z <= maxZ; z++ { + h.Heights[x*45+z] = float32(r.Float64()*3 - 1) + } + } + h.Update(minX, minZ, maxX, maxZ) + + fresh := NewHeightfield(50, 45, h.Heights, h.Scale) + for k := range fresh.blocks { + if fresh.blocks[k] != h.blocks[k] { + t.Fatalf("update %d: block %d is %v, want %v", i, k, h.blocks[k], fresh.blocks[k]) + } + } + for k := range fresh.edges { + if fresh.edges[k] != h.edges[k] { + t.Fatalf("update %d: edges of the cell %d are %b, want %b", i, k, h.edges[k], fresh.edges[k]) + } + } + if fresh.minHeight != h.minHeight || fresh.maxHeight != h.maxHeight { + t.Fatalf("update %d: heights [%v, %v], want [%v, %v]", i, h.minHeight, h.maxHeight, fresh.minHeight, fresh.maxHeight) + } + } +} + +func TestHeightfieldAABB(t *testing.T) { + h := randomHeightfield(7, 9, 5) + transform := Transform{Position: mgl64.Vec3{1, 2, 3}, Rotation: mgl64.QuatRotate(0.7, mgl64.Vec3{0, 1, 0})} + aabb := h.ComputeAABB(transform) + for x := 0; x < h.XSamples; x++ { + for z := 0; z < h.ZSamples; z++ { + if !aabb.ContainsPoint(transform.ToWorld(h.localVertex(x, z))) { + t.Fatalf("the sample (%d, %d) is outside the AABB", x, z) + } + } + } +} + +func TestHeightfieldNeedsSamples(t *testing.T) { + defer func() { + if recover() == nil { + t.Error("no panic") + } + }() + NewHeightfield(3, 3, make([]float32, 8), mgl64.Vec3{1, 1, 1}) +} diff --git a/actor/linalg.go b/actor/linalg.go new file mode 100644 index 0000000..294e12b --- /dev/null +++ b/actor/linalg.go @@ -0,0 +1,97 @@ +package actor + +import "github.com/go-gl/mathgl/mgl64" + +// ========== PRODUCTS ========== +// The products of mgl64 on pointers: Mat3.Mul3x1 and Mat3.Mul3 copy their 72 bytes at each call, and the methods +// are not always inlined. The arithmetic is the same, in the same order: the results are bit-exact + +// MulMat3 is m × v +func MulMat3(m *mgl64.Mat3, v mgl64.Vec3) mgl64.Vec3 { + return mgl64.Vec3{ + m[0]*v[0] + m[3]*v[1] + m[6]*v[2], + m[1]*v[0] + m[4]*v[1] + m[7]*v[2], + m[2]*v[0] + m[5]*v[1] + m[8]*v[2], + } +} + +// Mul3 is a × b +func Mul3(a, b *mgl64.Mat3) mgl64.Mat3 { + return mgl64.Mat3{ + a[0]*b[0] + a[3]*b[1] + a[6]*b[2], + a[1]*b[0] + a[4]*b[1] + a[7]*b[2], + a[2]*b[0] + a[5]*b[1] + a[8]*b[2], + a[0]*b[3] + a[3]*b[4] + a[6]*b[5], + a[1]*b[3] + a[4]*b[4] + a[7]*b[5], + a[2]*b[3] + a[5]*b[4] + a[8]*b[5], + a[0]*b[6] + a[3]*b[7] + a[6]*b[8], + a[1]*b[6] + a[4]*b[7] + a[7]*b[8], + a[2]*b[6] + a[5]*b[7] + a[8]*b[8], + } +} + +// Rotate is q v q⁻¹, the arithmetic of Quat.Rotate: v + 2 w (q × v) + 2 q × (q × v) +func Rotate(q *mgl64.Quat, v mgl64.Vec3) mgl64.Vec3 { + c := mgl64.Vec3{q.V[1]*v[2] - q.V[2]*v[1], q.V[2]*v[0] - q.V[0]*v[2], q.V[0]*v[1] - q.V[1]*v[0]} + w2 := 2 * q.W + q2 := mgl64.Vec3{q.V[0] * 2, q.V[1] * 2, q.V[2] * 2} + return mgl64.Vec3{ + v[0] + c[0]*w2 + (q2[1]*c[2] - q2[2]*c[1]), + v[1] + c[1]*w2 + (q2[2]*c[0] - q2[0]*c[2]), + v[2] + c[2]*w2 + (q2[0]*c[1] - q2[1]*c[0]), + } +} + +// RotateInverse is q⁻¹ v q: the rotation by the conjugate +func RotateInverse(q *mgl64.Quat, v mgl64.Vec3) mgl64.Vec3 { + conjugate := mgl64.Quat{W: q.W, V: mgl64.Vec3{q.V[0] * -1, q.V[1] * -1, q.V[2] * -1}} + return Rotate(&conjugate, v) +} + +// MulQuat is a × b, the arithmetic of Quat.Mul +func MulQuat(a, b *mgl64.Quat) mgl64.Quat { + c := mgl64.Vec3{a.V[1]*b.V[2] - a.V[2]*b.V[1], a.V[2]*b.V[0] - a.V[0]*b.V[2], a.V[0]*b.V[1] - a.V[1]*b.V[0]} + return mgl64.Quat{ + W: a.W*b.W - (a.V[0]*b.V[0] + a.V[1]*b.V[1] + a.V[2]*b.V[2]), + V: mgl64.Vec3{c[0] + b.V[0]*a.W + a.V[0]*b.W, c[1] + b.V[1]*a.W + a.V[1]*b.W, c[2] + b.V[2]*a.W + a.V[2]*b.W}, + } +} + +// Add3 is a + b, Sub3 is a - b +func Add3(a, b *mgl64.Mat3) mgl64.Mat3 { + return mgl64.Mat3{a[0] + b[0], a[1] + b[1], a[2] + b[2], a[3] + b[3], a[4] + b[4], a[5] + b[5], a[6] + b[6], a[7] + b[7], a[8] + b[8]} +} + +func Sub3(a, b *mgl64.Mat3) mgl64.Mat3 { + return mgl64.Mat3{a[0] - b[0], a[1] - b[1], a[2] - b[2], a[3] - b[3], a[4] - b[4], a[5] - b[5], a[6] - b[6], a[7] - b[7], a[8] - b[8]} +} + +// Det3 is the determinant, the arithmetic of Mat3.Det +func Det3(m *mgl64.Mat3) float64 { + return m[0]*m[4]*m[8] + m[3]*m[7]*m[2] + m[6]*m[1]*m[5] - m[6]*m[4]*m[2] - m[3]*m[1]*m[8] - m[0]*m[7]*m[5] +} + +// Inv3 is m⁻¹, the arithmetic of Mat3.Inv: the adjugate over the determinant, the zero matrix if the determinant is 0 +func Inv3(m *mgl64.Mat3) mgl64.Mat3 { + det := Det3(m) + if mgl64.FloatEqual(det, 0) { + return mgl64.Mat3{} + } + c := 1 / det + return mgl64.Mat3{ + (m[4]*m[8] - m[5]*m[7]) * c, + (m[2]*m[7] - m[1]*m[8]) * c, + (m[1]*m[5] - m[2]*m[4]) * c, + (m[5]*m[6] - m[3]*m[8]) * c, + (m[0]*m[8] - m[2]*m[6]) * c, + (m[2]*m[3] - m[0]*m[5]) * c, + (m[3]*m[7] - m[4]*m[6]) * c, + (m[1]*m[6] - m[0]*m[7]) * c, + (m[0]*m[4] - m[1]*m[3]) * c, + } +} + +// Transpose3 is mᵀ +func Transpose3(m *mgl64.Mat3) mgl64.Mat3 { + return mgl64.Mat3{m[0], m[3], m[6], m[1], m[4], m[7], m[2], m[5], m[8]} +} diff --git a/actor/rigidbody.go b/actor/rigidbody.go index 165aebc..1eb8385 100644 --- a/actor/rigidbody.go +++ b/actor/rigidbody.go @@ -2,7 +2,7 @@ package actor import ( "math" - "sync" + "sync/atomic" "github.com/go-gl/mathgl/mgl64" ) @@ -20,15 +20,26 @@ const ( BodyTypeStatic ) +const ( + // DefaultSleepSpeed: under this linear (m/s) and angular (rad/s) speed, a body is resting + DefaultSleepSpeed = 0.05 + + // DefaultTimeToSleep: a body resting for this duration (s) falls asleep + DefaultTimeToSleep = 0.5 +) + type Material struct { Density float64 mass float64 Restitution float64 // 0= no rebound, 1= perfect restitution + // StaticFriction when the surfaces stick, DynamicFriction when they slide StaticFriction float64 DynamicFriction float64 - LinearDamping float64 // 0.0 - 1.0, typique : 0.01 - AngularDamping float64 // 0.0 - 1.0, typique : 0.05 + // RollingResistance slows down the rolling spheres and capsules, usually in the range [0,1] + RollingResistance float64 + LinearDamping float64 // 0.0 - 1.0, typical: 0.01 + AngularDamping float64 // 0.0 - 1.0, typical: 0.05 } func (material Material) GetMass() float64 { @@ -41,24 +52,25 @@ type RigidBody struct { Id any // Spatial properties - PreviousTransform Transform - Transform Transform + Transform Transform // Linear motion - PresolveVelocity mgl64.Vec3 - Velocity mgl64.Vec3 // Linear velocity (m/s) - - // Angular motion (NOUVEAU) - PresolveAngularVelocity mgl64.Vec3 - AngularVelocity mgl64.Vec3 // Vitesse de rotation (rad/s) - // Inertia (NOUVEAU) - InertiaLocal mgl64.Mat3 // Tenseur d'inertie en espace local + Velocity mgl64.Vec3 // Linear velocity (m/s) + + // Angular motion + AngularVelocity mgl64.Vec3 // Angular velocity (rad/s) + // Inertia + InertiaLocal mgl64.Mat3 // Inertia tensor, in the local space InverseInertiaLocal mgl64.Mat3 + // Force (N) & torque (N·m) applied during the next step accumulatedForce mgl64.Vec3 accumulatedTorque mgl64.Vec3 - IsTrigger bool + IsTrigger bool + // IsBullet: a fast body is stopped at its first impact with the dynamic bodies too, not only with the static ones + // (continuous collision). For small fast bodies: projectiles + IsBullet bool IsSleeping bool SleepTimer float64 @@ -68,19 +80,24 @@ type RigidBody struct { // Collision shape Shape ShapeInterface // The collision shape - - Mutex sync.Mutex + aabb AABB + // serial: a unique number, given by NewRigidBody + serial uint64 } +// serials of the bodies created by NewRigidBody +var serials atomic.Uint64 + // NewRigidBody creates a new rigid body with the given properties // density is used to calculate mass for dynamic bodies (ignored for static) func NewRigidBody(transform Transform, shape ShapeInterface, bodyType BodyType, density float64) *RigidBody { + transform.Rotation = transform.Rotation.Normalize() rb := &RigidBody{ - PreviousTransform: transform, - Transform: transform, - Shape: shape, - BodyType: bodyType, - Velocity: mgl64.Vec3{0, 0, 0}, + serial: serials.Add(1), + Transform: transform, + Shape: shape, + BodyType: bodyType, + Velocity: mgl64.Vec3{0, 0, 0}, } // Calculate mass data based on body type @@ -105,37 +122,43 @@ func NewRigidBody(transform Transform, shape ShapeInterface, bodyType BodyType, } } - rb.InertiaLocal = shape.ComputeInertia(rb.Material.mass) - rb.InverseInertiaLocal = rb.InertiaLocal.Inv() - rb.Shape.ComputeAABB(rb.Transform) + // a static body has no inertia (its inverse inertia is 0, it never turns) + if bodyType != BodyTypeStatic { + rb.InertiaLocal = shape.ComputeInertia(rb.Material.mass) + rb.InverseInertiaLocal = rb.InertiaLocal.Inv() + } + rb.UpdateAABB() return rb } -// TrySleep check if a body can be set to sleep. -// returns 0 if no changes, 1 if set to sleep, 2 if waken -func (rb *RigidBody) TrySleep(dt float64, timethreshold float64, velocityThreshold float64) uint8 { - if rb.Velocity.Len() < velocityThreshold && rb.AngularVelocity.Len() < velocityThreshold { - rb.SleepTimer += dt // Incrémente le timer - if !rb.IsSleeping && rb.SleepTimer >= timethreshold { - rb.Sleep() +// Serial is a unique number of the body, given by NewRigidBody +func (rb *RigidBody) Serial() uint64 { + return rb.serial +} - return 1 - } - } else { - rb.WakeUp() +// AABB of the body, at its transform +func (rb *RigidBody) AABB() AABB { + return rb.aabb +} - return 2 - } +// UpdateAABB after a change of the transform (the World updates it after each step) +func (rb *RigidBody) UpdateAABB() { + rb.aabb = rb.Shape.ComputeAABB(rb.Transform) +} - return 0 +func (rb *RigidBody) InverseMass() float64 { + if rb.BodyType == BodyTypeStatic { + return 0 + } + return 1 / rb.Material.mass } func (rb *RigidBody) Sleep() { rb.IsSleeping = true rb.SleepTimer = 0.0 - rb.Shape.ComputeAABB(rb.Transform) + rb.UpdateAABB() rb.ClearForces() rb.Velocity = mgl64.Vec3{} rb.AngularVelocity = mgl64.Vec3{} @@ -146,112 +169,85 @@ func (rb *RigidBody) WakeUp() { rb.SleepTimer = 0.0 } -func (rb *RigidBody) Integrate(dt float64, gravity mgl64.Vec3) { - if rb.BodyType == BodyTypeStatic || rb.IsSleeping { - return +// AddForce in N, during the next step +func (rb *RigidBody) AddForce(force mgl64.Vec3) { + if rb.BodyType != BodyTypeStatic { + rb.WakeUp() + rb.accumulatedForce = rb.accumulatedForce.Add(force) } - - // Stockage état précédent - rb.PreviousTransform.Position = rb.Transform.Position - rb.PreviousTransform.Rotation = rb.Transform.Rotation - - // ========== INTÉGRATION LINÉAIRE ========== - forces := gravity.Mul(rb.Material.mass).Mul(dt * (1.0 / rb.Material.GetMass())) - forces = forces.Add(rb.accumulatedForce.Mul(1.0 / rb.Material.GetMass())) - rb.Velocity = rb.Velocity.Add(forces) - - // ========== LINEAR DAMPING ========== - rb.Velocity = rb.Velocity.Mul(math.Exp(-rb.Material.LinearDamping * dt)) - rb.Transform.Position = rb.Transform.Position.Add(rb.Velocity.Mul(dt)) - - // ========== INTÉGRATION ANGULAIRE ========== - I_inv := rb.GetInverseInertiaWorld() - torques := rb.accumulatedTorque.Mul(1.0 / dt) - angularAccel := I_inv.Mul3x1(torques) - rb.AngularVelocity = rb.AngularVelocity.Add(angularAccel.Mul(dt)) - - // ========== ANGULAR DAMPING ========== - rb.AngularVelocity = rb.AngularVelocity.Mul(math.Exp(-rb.Material.AngularDamping * dt)) - - // ========== UPDATE QUATERNION ========== - omegaQuat := mgl64.Quat{V: rb.AngularVelocity, W: 0} - q_dot := omegaQuat.Mul(rb.Transform.Rotation).Scale(0.5) - rb.Transform.Rotation = rb.Transform.Rotation.Add(q_dot.Scale(dt)).Normalize() - rb.Transform.InverseRotation = rb.Transform.Rotation.Inverse() - - rb.PresolveVelocity = rb.Velocity - rb.PresolveAngularVelocity = rb.AngularVelocity - - rb.Shape.ComputeAABB(rb.Transform) - rb.ClearForces() } -func (rb *RigidBody) Update(dt float64) { - if rb.BodyType == BodyTypeStatic || rb.IsSleeping { - return +// AddTorque in N·m (world space), during the next step +func (rb *RigidBody) AddTorque(torque mgl64.Vec3) { + if rb.BodyType != BodyTypeStatic { + rb.WakeUp() + rb.accumulatedTorque = rb.accumulatedTorque.Add(torque) } +} - // Commit predicted position to actual position - rb.Velocity = rb.Transform.Position.Sub(rb.PreviousTransform.Position).Mul(1.0 / dt) - qDelta := rb.Transform.Rotation.Mul(rb.PreviousTransform.Rotation.Conjugate()) - qDelta = qDelta.Normalize() - if qDelta.W >= 0.0 { - rb.AngularVelocity = qDelta.V.Mul(2.0 / dt) - } else { - rb.AngularVelocity = qDelta.V.Mul(-2.0 / dt) - } +// AddForceAtPoint in N, applied at a point in world space: it also adds the torque (point - center) × force +func (rb *RigidBody) AddForceAtPoint(force mgl64.Vec3, point mgl64.Vec3) { + rb.AddForce(force) + rb.AddTorque(point.Sub(rb.Transform.Position).Cross(force)) } -// AddForce in 1000N (1000 * kg⋅m/s²) -func (rb *RigidBody) AddForce(force mgl64.Vec3) { +// AddImpulse in N·s: the velocity changes immediately (a hit, a jump) +func (rb *RigidBody) AddImpulse(impulse mgl64.Vec3) { if rb.BodyType != BodyTypeStatic { rb.WakeUp() - - rb.accumulatedForce = rb.accumulatedForce.Add(force.Mul(1000)) + rb.Velocity = rb.Velocity.Add(impulse.Mul(rb.InverseMass())) } } -// AddTorque in 1000N⋅m -func (rb *RigidBody) AddTorque(torque mgl64.Vec3) { +// AddImpulseAtPoint in N·s, applied at a point in world space: the body also starts to spin +func (rb *RigidBody) AddImpulseAtPoint(impulse mgl64.Vec3, point mgl64.Vec3) { + rb.AddImpulse(impulse) + rb.AddAngularImpulse(point.Sub(rb.Transform.Position).Cross(impulse)) +} + +// AddAngularImpulse in N·m·s (world space): the angular velocity changes immediately +func (rb *RigidBody) AddAngularImpulse(impulse mgl64.Vec3) { if rb.BodyType != BodyTypeStatic { rb.WakeUp() - - rb.accumulatedTorque = rb.accumulatedTorque.Add(torque.Mul(1000)) + rb.AngularVelocity = rb.AngularVelocity.Add(rb.GetInverseInertiaWorld().Mul3x1(impulse)) } } -// Méthodes optionnelles pour reset +func (rb *RigidBody) Force() mgl64.Vec3 { return rb.accumulatedForce } + +func (rb *RigidBody) Torque() mgl64.Vec3 { return rb.accumulatedTorque } + func (rb *RigidBody) ClearForces() { rb.accumulatedForce = mgl64.Vec3{0, 0, 0} rb.accumulatedTorque = mgl64.Vec3{0, 0, 0} } func (rb *RigidBody) SupportWorld(direction mgl64.Vec3) mgl64.Vec3 { - // 1. Transformer la direction en espace local (rotation inverse) - localDirection := rb.Transform.InverseRotation.Rotate(direction) - - // 2. Trouver le support en espace local + localDirection := rb.Transform.Rotation.Conjugate().Rotate(direction) localSupport := rb.Shape.Support(localDirection) - - // 3. Transformer le point support en espace monde (rotation + translation) - worldSupport := rb.Transform.Rotation.Rotate(localSupport) - return rb.Transform.Position.Add(worldSupport) + return rb.Transform.Position.Add(rb.Transform.Rotation.Rotate(localSupport)) } -// Inertie en espace monde +// Inertia in world space: R * inertiaLocal * R^T func (rb *RigidBody) GetInertiaWorld() mgl64.Mat3 { - // I_world = R * I_local * R^T R := rb.Transform.Rotation.Mat4().Mat3() return R.Mul3(rb.InertiaLocal).Mul3(R.Transpose()) } -// Inverse de l'inertie en espace monde +// Inverse inertia in world space: R * inertiaLocal^-1 * R^T func (rb *RigidBody) GetInverseInertiaWorld() mgl64.Mat3 { if rb.BodyType == BodyTypeStatic { - return mgl64.Mat3{0, 0, 0, 0, 0, 0, 0, 0, 0} + return mgl64.Mat3{} } - - // I_world^(-1) = R * I_local^(-1) * R^T - R := rb.Transform.Rotation.Mat4().Mat3() - return R.Mul3(rb.InverseInertiaLocal).Mul3(R.Transpose()) + // the rotation matrix of mgl64 (Quat.Mat4), and R I⁻¹ Rᵀ without copying the matrices: the same arithmetic + q := rb.Transform.Rotation + w, x, y, z := q.W, q.V[0], q.V[1], q.V[2] + r := mgl64.Mat3{ + 1 - 2*y*y - 2*z*z, 2*x*y + 2*w*z, 2*x*z - 2*w*y, + 2*x*y - 2*w*z, 1 - 2*x*x - 2*z*z, 2*y*z + 2*w*x, + 2*x*z + 2*w*y, 2*y*z - 2*w*x, 1 - 2*x*x - 2*y*y, + } + ri := Mul3(&r, &rb.InverseInertiaLocal) + rt := Transpose3(&r) + return Mul3(&ri, &rt) } diff --git a/actor/rigidbody_test.go b/actor/rigidbody_test.go index a485c7c..688872c 100644 --- a/actor/rigidbody_test.go +++ b/actor/rigidbody_test.go @@ -98,9 +98,6 @@ func TestNewRigidBody_Dynamic(t *testing.T) { if !vec3AlmostEqual(rb.Transform.Position, transform.Position, 1e-10) { t.Errorf("Transform.Position = %v, want %v", rb.Transform.Position, transform.Position) } - if !vec3AlmostEqual(rb.PreviousTransform.Position, transform.Position, 1e-10) { - t.Errorf("PreviousTransform.Position = %v, want %v", rb.PreviousTransform.Position, transform.Position) - } // Verify velocity is zero initialized expectedVelocity := mgl64.Vec3{0, 0, 0} @@ -210,292 +207,6 @@ func TestNewRigidBody_DifferentShapes(t *testing.T) { // Integrate Tests // ============================================================================= -func TestIntegrate_Dynamic_NoGravity(t *testing.T) { - transform := NewTransform() - sphere := &Sphere{Radius: 1.0} - rb := NewRigidBody(transform, sphere, BodyTypeDynamic, 1.0) - - // Set initial velocity - rb.Velocity = mgl64.Vec3{1, 2, 3} - - dt := 0.1 - gravity := mgl64.Vec3{0, 0, 0} // No gravity - - rb.Integrate(dt, gravity) - - // With no gravity, velocity should remain constant - expectedVelocity := mgl64.Vec3{1, 2, 3} - if !vec3AlmostEqual(rb.Velocity, expectedVelocity, 1e-10) { - t.Errorf("Velocity = %v, want %v", rb.Velocity, expectedVelocity) - } - - // Position should update based on velocity - expectedPosition := mgl64.Vec3{0.1, 0.2, 0.3} // dt * velocity - if !vec3AlmostEqual(rb.Transform.Position, expectedPosition, 1e-10) { - t.Errorf("Position = %v, want %v", rb.Transform.Position, expectedPosition) - } - - // Previous velocity should be saved - expectedPreviousVelocity := mgl64.Vec3{1, 2, 3} - if !vec3AlmostEqual(rb.PresolveVelocity, expectedPreviousVelocity, 1e-10) { - t.Errorf("PresolveVelocity = %v, want %v", rb.PresolveVelocity, expectedPreviousVelocity) - } - - // Previous position should be saved - expectedPreviousPosition := mgl64.Vec3{0, 0, 0} - if !vec3AlmostEqual(rb.PreviousTransform.Position, expectedPreviousPosition, 1e-10) { - t.Errorf("PreviousTransform.Position = %v, want %v", rb.PreviousTransform.Position, expectedPreviousPosition) - } -} - -func TestIntegrate_Dynamic_WithGravity(t *testing.T) { - transform := NewTransform() - sphere := &Sphere{Radius: 1.0} - density := 1.0 - rb := NewRigidBody(transform, sphere, BodyTypeDynamic, density) - - // Initial velocity is zero - rb.Velocity = mgl64.Vec3{0, 0, 0} - - dt := 0.1 - gravity := mgl64.Vec3{0, -10, 0} // Standard gravity - - rb.Integrate(dt, gravity) - - // Velocity should increase due to gravity: v = v0 + g*dt - // Since mass cancels out in the force calculation: a = F/m = (g*m)/m = g - expectedVelocity := mgl64.Vec3{0, -1, 0} // g * dt = -10 * 0.1 - if !vec3AlmostEqual(rb.Velocity, expectedVelocity, 1e-10) { - t.Errorf("Velocity = %v, want %v", rb.Velocity, expectedVelocity) - } - - // Position should update: p = p0 + v*dt - // Since initial velocity was 0, and we use the NEW velocity: - expectedPosition := mgl64.Vec3{0, -0.1, 0} // v * dt = -1 * 0.1 - if !vec3AlmostEqual(rb.Transform.Position, expectedPosition, 1e-10) { - t.Errorf("Position = %v, want %v", rb.Transform.Position, expectedPosition) - } -} - -func TestIntegrate_Dynamic_MultipleSteps(t *testing.T) { - transform := NewTransform() - sphere := &Sphere{Radius: 1.0} - rb := NewRigidBody(transform, sphere, BodyTypeDynamic, 1.0) - - dt := 0.1 - gravity := mgl64.Vec3{0, -10, 0} - - // Integrate multiple times - for i := 0; i < 3; i++ { - rb.Integrate(dt, gravity) - } - - // After 3 steps: - // Step 1: v = 0 + (-10)*0.1 = -1, p = 0 + (-1)*0.1 = -0.1 - // Step 2: v = -1 + (-10)*0.1 = -2, p = -0.1 + (-2)*0.1 = -0.3 - // Step 3: v = -2 + (-10)*0.1 = -3, p = -0.3 + (-3)*0.1 = -0.6 - - expectedVelocity := mgl64.Vec3{0, -3, 0} - if !vec3AlmostEqual(rb.Velocity, expectedVelocity, 1e-9) { - t.Errorf("Velocity after 3 steps = %v, want %v", rb.Velocity, expectedVelocity) - } - - expectedPosition := mgl64.Vec3{0, -0.6, 0} - if !vec3AlmostEqual(rb.Transform.Position, expectedPosition, 1e-9) { - t.Errorf("Position after 3 steps = %v, want %v", rb.Transform.Position, expectedPosition) - } -} - -func TestIntegrate_Static_NoMovement(t *testing.T) { - transform := Transform{ - Position: mgl64.Vec3{5, 10, 15}, - } - box := &Box{HalfExtents: mgl64.Vec3{1, 1, 1}} - rb := NewRigidBody(transform, box, BodyTypeStatic, 1.0) - - // Try to set velocity (shouldn't matter for static) - rb.Velocity = mgl64.Vec3{100, 200, 300} - - dt := 0.1 - gravity := mgl64.Vec3{0, -10, 0} - - initialPosition := rb.Transform.Position - - rb.Integrate(dt, gravity) - - // Static bodies should not move - if !vec3AlmostEqual(rb.Transform.Position, initialPosition, 1e-10) { - t.Errorf("Static body moved: Position = %v, want %v", rb.Transform.Position, initialPosition) - } - - // Velocity should remain unchanged - expectedVelocity := mgl64.Vec3{100, 200, 300} - if !vec3AlmostEqual(rb.Velocity, expectedVelocity, 1e-10) { - t.Errorf("Static body velocity changed: Velocity = %v, want %v", rb.Velocity, expectedVelocity) - } -} - -func TestIntegrate_Dynamic_WithInitialVelocity(t *testing.T) { - transform := NewTransform() - sphere := &Sphere{Radius: 1.0} - rb := NewRigidBody(transform, sphere, BodyTypeDynamic, 1.0) - - // Set initial velocity - rb.Velocity = mgl64.Vec3{5, 10, 0} - - dt := 0.1 - gravity := mgl64.Vec3{0, -10, 0} - - rb.Integrate(dt, gravity) - - // v = v0 + g*dt = (5, 10, 0) + (0, -10, 0)*0.1 = (5, 9, 0) - expectedVelocity := mgl64.Vec3{5, 9, 0} - if !vec3AlmostEqual(rb.Velocity, expectedVelocity, 1e-10) { - t.Errorf("Velocity = %v, want %v", rb.Velocity, expectedVelocity) - } - - // p = p0 + v*dt = (0, 0, 0) + (5, 9, 0)*0.1 = (0.5, 0.9, 0) - expectedPosition := mgl64.Vec3{0.5, 0.9, 0} - if !vec3AlmostEqual(rb.Transform.Position, expectedPosition, 1e-10) { - t.Errorf("Position = %v, want %v", rb.Transform.Position, expectedPosition) - } -} - -func TestIntegrate_Dynamic_DifferentMasses(t *testing.T) { - tests := []struct { - name string - density float64 - radius float64 - }{ - { - name: "light sphere", - density: 0.5, - radius: 1.0, - }, - { - name: "heavy sphere", - density: 10.0, - radius: 1.0, - }, - { - name: "large light sphere", - density: 0.1, - radius: 5.0, - }, - } - - dt := 0.1 - gravity := mgl64.Vec3{0, -10, 0} - - for _, tt := range tests { - t.Run(tt.name, func(t *testing.T) { - transform := NewTransform() - sphere := &Sphere{Radius: tt.radius} - rb := NewRigidBody(transform, sphere, BodyTypeDynamic, tt.density) - - rb.Integrate(dt, gravity) - - // All bodies should fall at the same rate (mass cancels out) - // v = g*dt, regardless of mass - expectedVelocity := mgl64.Vec3{0, -1, 0} - if !vec3AlmostEqual(rb.Velocity, expectedVelocity, 1e-9) { - t.Errorf("%s: Velocity = %v, want %v", tt.name, rb.Velocity, expectedVelocity) - } - }) - } -} - -func TestIntegrate_Dynamic_ZeroTimeStep(t *testing.T) { - transform := NewTransform() - sphere := &Sphere{Radius: 1.0} - rb := NewRigidBody(transform, sphere, BodyTypeDynamic, 1.0) - - rb.Velocity = mgl64.Vec3{5, 10, 15} - - dt := 0.0 - gravity := mgl64.Vec3{0, -10, 0} - - initialPosition := rb.Transform.Position - initialVelocity := rb.Velocity - - rb.Integrate(dt, gravity) - - // With dt=0, nothing should change - if !vec3AlmostEqual(rb.Transform.Position, initialPosition, 1e-10) { - t.Errorf("Position changed with dt=0: Position = %v, want %v", rb.Transform.Position, initialPosition) - } - if !vec3AlmostEqual(rb.Velocity, initialVelocity, 1e-10) { - t.Errorf("Velocity changed with dt=0: Velocity = %v, want %v", rb.Velocity, initialVelocity) - } -} - -func TestIntegrate_Dynamic_3DGravity(t *testing.T) { - transform := NewTransform() - sphere := &Sphere{Radius: 1.0} - rb := NewRigidBody(transform, sphere, BodyTypeDynamic, 1.0) - - dt := 0.1 - // Diagonal gravity vector - gravity := mgl64.Vec3{1, -10, 2} - - rb.Integrate(dt, gravity) - - // v = g*dt - expectedVelocity := mgl64.Vec3{0.1, -1, 0.2} - if !vec3AlmostEqual(rb.Velocity, expectedVelocity, 1e-10) { - t.Errorf("Velocity = %v, want %v", rb.Velocity, expectedVelocity) - } - - // p = v*dt - expectedPosition := mgl64.Vec3{0.01, -0.1, 0.02} - if !vec3AlmostEqual(rb.Transform.Position, expectedPosition, 1e-10) { - t.Errorf("Position = %v, want %v", rb.Transform.Position, expectedPosition) - } -} - -func TestIntegrate_Dynamic_PreviousStateTracking(t *testing.T) { - transform := Transform{ - Position: mgl64.Vec3{1, 2, 3}, - } - sphere := &Sphere{Radius: 1.0} - rb := NewRigidBody(transform, sphere, BodyTypeDynamic, 1.0) - - rb.Velocity = mgl64.Vec3{10, 20, 30} - - dt := 0.1 - gravity := mgl64.Vec3{0, -10, 0} - - // First integration - rb.Integrate(dt, gravity) - - // Check that previous states were saved - expectedPreviousPosition := mgl64.Vec3{1, 2, 3} - if !vec3AlmostEqual(rb.PreviousTransform.Position, expectedPreviousPosition, 1e-10) { - t.Errorf("PreviousTransform.Position = %v, want %v", rb.PreviousTransform.Position, expectedPreviousPosition) - } - - // PresolveVelocity is set to velocity AFTER integration (not before) - // After first integration: velocity becomes [10, 20, 30] + gravity*dt = [10, 19, 30] (assuming gravity = [0, -10, 0]) - expectedPresolveVelocity := rb.Velocity // PresolveVelocity should equal the current velocity after integration - if !vec3AlmostEqual(rb.PresolveVelocity, expectedPresolveVelocity, 1e-10) { - t.Errorf("PresolveVelocity = %v, want %v (current velocity after integration)", rb.PresolveVelocity, expectedPresolveVelocity) - } - - // Second integration - currentPosition := rb.Transform.Position - - rb.Integrate(dt, gravity) - - // Previous state should now be the state before this integration - if !vec3AlmostEqual(rb.PreviousTransform.Position, currentPosition, 1e-10) { - t.Errorf("PreviousTransform.Position after 2nd integration = %v, want %v", rb.PreviousTransform.Position, currentPosition) - } - // PresolveVelocity should equal the velocity after the 2nd integration (not the velocity before it) - if !vec3AlmostEqual(rb.PresolveVelocity, rb.Velocity, 1e-10) { - t.Errorf("PresolveVelocity after 2nd integration = %v, want %v (velocity after integration)", rb.PresolveVelocity, rb.Velocity) - } -} - // ============================================================================= // Edge Cases and Stress Tests // ============================================================================= @@ -511,490 +222,14 @@ func TestNewRigidBody_ZeroDensity(t *testing.T) { } } -func TestIntegrate_Dynamic_LargeTimeStep(t *testing.T) { - transform := NewTransform() - sphere := &Sphere{Radius: 1.0} - rb := NewRigidBody(transform, sphere, BodyTypeDynamic, 1.0) - - dt := 10.0 // Very large time step - gravity := mgl64.Vec3{0, -10, 0} - - rb.Integrate(dt, gravity) - - // Should still work, just with large changes - expectedVelocity := mgl64.Vec3{0, -100, 0} - if !vec3AlmostEqual(rb.Velocity, expectedVelocity, 1e-8) { - t.Errorf("Velocity with large dt = %v, want %v", rb.Velocity, expectedVelocity) - } - - expectedPosition := mgl64.Vec3{0, -1000, 0} - if !vec3AlmostEqual(rb.Transform.Position, expectedPosition, 1e-6) { - t.Errorf("Position with large dt = %v, want %v", rb.Transform.Position, expectedPosition) - } -} - -func TestIntegrate_Dynamic_SmallTimeStep(t *testing.T) { - transform := NewTransform() - sphere := &Sphere{Radius: 1.0} - rb := NewRigidBody(transform, sphere, BodyTypeDynamic, 1.0) - - dt := 0.001 // Very small time step - gravity := mgl64.Vec3{0, -10, 0} - - rb.Integrate(dt, gravity) - - expectedVelocity := mgl64.Vec3{0, -0.01, 0} - if !vec3AlmostEqual(rb.Velocity, expectedVelocity, 1e-10) { - t.Errorf("Velocity with small dt = %v, want %v", rb.Velocity, expectedVelocity) - } -} - -func TestIntegrate_Dynamic_NegativeGravity(t *testing.T) { - transform := NewTransform() - sphere := &Sphere{Radius: 1.0} - rb := NewRigidBody(transform, sphere, BodyTypeDynamic, 1.0) - - dt := 0.1 - gravity := mgl64.Vec3{0, 10, 0} // Upward gravity - - rb.Integrate(dt, gravity) - - // Body should accelerate upward - expectedVelocity := mgl64.Vec3{0, 1, 0} - if !vec3AlmostEqual(rb.Velocity, expectedVelocity, 1e-10) { - t.Errorf("Velocity with upward gravity = %v, want %v", rb.Velocity, expectedVelocity) - } - - expectedPosition := mgl64.Vec3{0, 0.1, 0} - if !vec3AlmostEqual(rb.Transform.Position, expectedPosition, 1e-10) { - t.Errorf("Position with upward gravity = %v, want %v", rb.Transform.Position, expectedPosition) - } -} - // ============================================================================= // PHASE 1: Angular Motion Tests (CRITICAL - Previously Untested) // ============================================================================= -// TestIntegrate_AngularVelocity_Basic verifies that a body with no initial -// angular velocity and no external torques maintains zero angular velocity -func TestIntegrate_AngularVelocity_Basic(t *testing.T) { - transform := NewTransform() - sphere := &Sphere{Radius: 1.0} - rb := NewRigidBody(transform, sphere, BodyTypeDynamic, 1.0) - - // No initial angular velocity - rb.AngularVelocity = mgl64.Vec3{0, 0, 0} - - dt := 0.1 - gravity := mgl64.Vec3{0, -10, 0} - - rb.Integrate(dt, gravity) - - // With no external torque and no initial rotation, AngularVelocity should remain zero - expectedAngularVelocity := mgl64.Vec3{0, 0, 0} - if !vec3AlmostEqual(rb.AngularVelocity, expectedAngularVelocity, 1e-10) { - t.Errorf("AngularVelocity = %v, want %v", rb.AngularVelocity, expectedAngularVelocity) - } - - // PresolveAngularVelocity should also be zero - if !vec3AlmostEqual(rb.PresolveAngularVelocity, expectedAngularVelocity, 1e-10) { - t.Errorf("PresolveAngularVelocity = %v, want %v", rb.PresolveAngularVelocity, expectedAngularVelocity) - } - - // Rotation quaternion should remain identity (no rotation) - identityQuat := mgl64.QuatIdent() - if !quatAlmostEqual(rb.Transform.Rotation, identityQuat, 1e-10) { - t.Errorf("Transform.Rotation = %v, want identity quaternion %v", rb.Transform.Rotation, identityQuat) - } -} - -// TestIntegrate_AngularVelocity_WithInitialRotation verifies that a body with -// initial angular velocity correctly updates its quaternion rotation -func TestIntegrate_AngularVelocity_WithInitialRotation(t *testing.T) { - transform := NewTransform() - sphere := &Sphere{Radius: 1.0} - rb := NewRigidBody(transform, sphere, BodyTypeDynamic, 1.0) - - // Set initial angular velocity (rotation around Z axis) - rb.AngularVelocity = mgl64.Vec3{0, 0, 1} // 1 rad/s around Z - - dt := 0.1 - gravity := mgl64.Vec3{0, 0, 0} // No gravity for clearer test - - initialRotation := rb.Transform.Rotation - - rb.Integrate(dt, gravity) - - // Angular velocity should remain constant (no external torque, no damping) - expectedAngularVelocity := mgl64.Vec3{0, 0, 1} - if !vec3AlmostEqual(rb.AngularVelocity, expectedAngularVelocity, 1e-10) { - t.Errorf("AngularVelocity = %v, want %v", rb.AngularVelocity, expectedAngularVelocity) - } - - // Rotation quaternion should have changed - if quatAlmostEqual(rb.Transform.Rotation, initialRotation, 1e-10) { - t.Error("Transform.Rotation did not change despite angular velocity") - } - - // Quaternion should still be normalized - quatMagnitude := math.Sqrt(rb.Transform.Rotation.W*rb.Transform.Rotation.W + - rb.Transform.Rotation.V.X()*rb.Transform.Rotation.V.X() + - rb.Transform.Rotation.V.Y()*rb.Transform.Rotation.V.Y() + - rb.Transform.Rotation.V.Z()*rb.Transform.Rotation.V.Z()) - if !almostEqual(quatMagnitude, 1.0, 1e-10) { - t.Errorf("Quaternion magnitude = %v, want 1.0 (normalized)", quatMagnitude) - } - - // PresolveAngularVelocity should be saved - if !vec3AlmostEqual(rb.PresolveAngularVelocity, rb.AngularVelocity, 1e-10) { - t.Errorf("PresolveAngularVelocity = %v, want %v", rb.PresolveAngularVelocity, rb.AngularVelocity) - } -} - -// TestIntegrate_QuaternionNormalization verifies that quaternion remains normalized -// after many integration steps (prevents numerical drift) -func TestIntegrate_QuaternionNormalization(t *testing.T) { - transform := NewTransform() - sphere := &Sphere{Radius: 1.0} - rb := NewRigidBody(transform, sphere, BodyTypeDynamic, 1.0) - - // High angular velocity to accumulate potential errors faster - rb.AngularVelocity = mgl64.Vec3{10, 5, 3} - - dt := 0.01 - gravity := mgl64.Vec3{0, 0, 0} - - // Integrate many times - numSteps := 1000 - for i := 0; i < numSteps; i++ { - rb.Integrate(dt, gravity) - } - - // Quaternion must remain normalized - quatMagnitude := math.Sqrt(rb.Transform.Rotation.W*rb.Transform.Rotation.W + - rb.Transform.Rotation.V.X()*rb.Transform.Rotation.V.X() + - rb.Transform.Rotation.V.Y()*rb.Transform.Rotation.V.Y() + - rb.Transform.Rotation.V.Z()*rb.Transform.Rotation.V.Z()) - - if !almostEqual(quatMagnitude, 1.0, 1e-6) { - t.Errorf("After %d steps, quaternion magnitude = %v, want 1.0 (BUG: quaternion drift)", numSteps, quatMagnitude) - } - - // Check for NaN values - if math.IsNaN(rb.Transform.Rotation.W) || math.IsNaN(rb.Transform.Rotation.V.X()) || - math.IsNaN(rb.Transform.Rotation.V.Y()) || math.IsNaN(rb.Transform.Rotation.V.Z()) { - t.Error("Quaternion contains NaN values (BUG: numerical instability)") - } -} - -// TestIntegrate_GyroscopicTerm verifies the gyroscopic term ω × (I·ω) is correctly computed -// For a spinning body, this term should maintain angular momentum conservation -func TestIntegrate_GyroscopicTerm(t *testing.T) { - transform := NewTransform() - // Use asymmetric box to make gyroscopic effects visible - box := &Box{HalfExtents: mgl64.Vec3{1, 2, 0.5}} - rb := NewRigidBody(transform, box, BodyTypeDynamic, 1.0) - - // Set angular velocity - rb.AngularVelocity = mgl64.Vec3{5, 0, 0} - - dt := 0.01 - gravity := mgl64.Vec3{0, 0, 0} - - // Compute initial angular momentum L = I·ω - I := rb.GetInertiaWorld() - L_initial := I.Mul3x1(rb.AngularVelocity) - - // Integrate multiple steps - for i := 0; i < 100; i++ { - rb.Integrate(dt, gravity) - } - - // Angular momentum should be conserved (no external torque) - I_final := rb.GetInertiaWorld() - L_final := I_final.Mul3x1(rb.AngularVelocity) - - // Note: L conservation is approximate due to numerical integration - if !vec3AlmostEqual(L_final, L_initial, 1e-3) { - t.Logf("POTENTIAL BUG: Angular momentum not conserved") - t.Logf("L_initial = %v", L_initial) - t.Logf("L_final = %v", L_final) - t.Logf("Difference = %v", L_final.Sub(L_initial)) - // Don't fail the test - this is expected to have some drift, just log it - } -} - -// TestIntegrate_AngularDamping verifies angular damping reduces rotation over time -func TestIntegrate_AngularDamping(t *testing.T) { - transform := NewTransform() - sphere := &Sphere{Radius: 1.0} - rb := NewRigidBody(transform, sphere, BodyTypeDynamic, 1.0) - - // Set angular damping - rb.Material.AngularDamping = 0.1 - - // Initial angular velocity - rb.AngularVelocity = mgl64.Vec3{10, 0, 0} - initialAngularSpeed := rb.AngularVelocity.Len() - - dt := 0.1 - gravity := mgl64.Vec3{0, 0, 0} - - rb.Integrate(dt, gravity) - - // Expected: v_new = v_old * exp(-AngularDamping * dt) - expectedFactor := math.Exp(-rb.Material.AngularDamping * dt) - expectedAngularVelocity := mgl64.Vec3{10 * expectedFactor, 0, 0} - - if !vec3AlmostEqual(rb.AngularVelocity, expectedAngularVelocity, 1e-9) { - t.Errorf("AngularVelocity after damping = %v, want %v", rb.AngularVelocity, expectedAngularVelocity) - t.Logf("POTENTIAL BUG: Angular damping formula incorrect") - t.Logf("Expected formula: ω_new = ω_old * (1 - drag*dt)") - t.Logf("Initial speed: %v, Final speed: %v", initialAngularSpeed, rb.AngularVelocity.Len()) - } - - // After many steps, angular velocity should approach zero - for i := 0; i < 100; i++ { - rb.Integrate(dt, gravity) - } - - finalAngularSpeed := rb.AngularVelocity.Len() - if finalAngularSpeed >= initialAngularSpeed*0.5 { - t.Errorf("After damping, angular speed = %v, expected significant reduction from initial %v", - finalAngularSpeed, initialAngularSpeed) - } -} - -// TestIntegrate_PreviousRotationTracking verifies PreviousTransform.Rotation is saved -func TestIntegrate_PreviousRotationTracking(t *testing.T) { - transform := NewTransform() - sphere := &Sphere{Radius: 1.0} - rb := NewRigidBody(transform, sphere, BodyTypeDynamic, 1.0) - - rb.AngularVelocity = mgl64.Vec3{1, 2, 3} - - dt := 0.1 - gravity := mgl64.Vec3{0, 0, 0} - - initialRotation := rb.Transform.Rotation - - // First integration - rb.Integrate(dt, gravity) - - // Previous rotation should be the initial rotation - if !quatAlmostEqual(rb.PreviousTransform.Rotation, initialRotation, 1e-10) { - t.Errorf("PreviousTransform.Rotation = %v, want %v", rb.PreviousTransform.Rotation, initialRotation) - } - - // Second integration - currentRotation := rb.Transform.Rotation - rb.Integrate(dt, gravity) - - // Previous rotation should now be the rotation before this integration - if !quatAlmostEqual(rb.PreviousTransform.Rotation, currentRotation, 1e-10) { - t.Errorf("PreviousTransform.Rotation after 2nd integration = %v, want %v", - rb.PreviousTransform.Rotation, currentRotation) - } -} - // ============================================================================= // PHASE 4: Damping Tests (HIGH PRIORITY - Production Code Never Tested) // ============================================================================= -// TestIntegrate_LinearDamping_Zero verifies that zero linear damping means no velocity reduction -func TestIntegrate_LinearDamping_Zero(t *testing.T) { - transform := NewTransform() - sphere := &Sphere{Radius: 1.0} - rb := NewRigidBody(transform, sphere, BodyTypeDynamic, 1.0) - - // Zero damping (default) - rb.Material.LinearDamping = 0.0 - - // Set initial velocity - rb.Velocity = mgl64.Vec3{10, 20, 30} - - dt := 0.1 - gravity := mgl64.Vec3{0, 0, 0} // No gravity to isolate damping - - rb.Integrate(dt, gravity) - - // With zero damping, velocity should remain constant - expectedVelocity := mgl64.Vec3{10, 20, 30} - if !vec3AlmostEqual(rb.Velocity, expectedVelocity, 1e-10) { - t.Errorf("Velocity with zero damping = %v, want %v", rb.Velocity, expectedVelocity) - t.Logf("POTENTIAL BUG: Zero damping should not change velocity") - } -} - -// TestIntegrate_LinearDamping_Positive verifies linear damping reduces velocity correctly -func TestIntegrate_LinearDamping_Positive(t *testing.T) { - transform := NewTransform() - sphere := &Sphere{Radius: 1.0} - rb := NewRigidBody(transform, sphere, BodyTypeDynamic, 1.0) - - // Set linear damping - rb.Material.LinearDamping = 0.1 - // Initial velocity - rb.Velocity = mgl64.Vec3{10, 0, 0} - dt := 0.1 - gravity := mgl64.Vec3{0, 0, 0} - - // Integrate one step - rb.Integrate(dt, gravity) - - // Expected: v_new = v_old * exp(-LinearDamping * dt) - // exp(-0.1 * 0.1) = exp(-0.01) ≈ 0.99004983 - expectedFactor := math.Exp(-rb.Material.LinearDamping * dt) - expectedVelocity := mgl64.Vec3{10 * expectedFactor, 0, 0} - - if !vec3AlmostEqual(rb.Velocity, expectedVelocity, 1e-8) { - t.Errorf("Velocity after one damping step = %v, want %v", rb.Velocity, expectedVelocity) - t.Logf("Expected: v *= exp(-LinearDamping * dt)") - t.Logf("LinearDamping = %v, dt = %v, exp(-k*dt) = %v", rb.Material.LinearDamping, dt, expectedFactor) - t.Logf("Actual velocity: %v", rb.Velocity) - } - - // After many steps, velocity should approach zero exponentially - for i := 0; i < 500; i++ { - rb.Integrate(dt, gravity) - } - - finalSpeed := rb.Velocity.Len() - // After 500 steps: exp(-0.1 * 0.1 * 500) = exp(-5) ≈ 0.0067 - // So final speed should be ~10 * 0.0067 ≈ 0.067 — well below 0.1 - if finalSpeed > 0.1 { - t.Errorf("After 500 damping steps, speed = %v, expected near zero (exp(-5) ≈ 0.0067)", finalSpeed) - t.Logf("Theoretical decay: 10 * exp(-0.1 * 0.1 * 500) = %v", 10*math.Exp(-5)) - } -} - -// TestIntegrate_LinearDamping_ExtremeValues tests edge case where drag*dt > 1 -// This is a CRITICAL test - if drag*dt > 1, the formula v *= (1 - drag*dt) gives NEGATIVE velocity! -func TestIntegrate_LinearDamping_ExtremeValues(t *testing.T) { - transform := NewTransform() - sphere := &Sphere{Radius: 1.0} - rb := NewRigidBody(transform, sphere, BodyTypeDynamic, 1.0) - - // EXTREME damping - rb.Material.LinearDamping = 0.99 - - // Initial velocity - rb.Velocity = mgl64.Vec3{10, 0, 0} - - dt := 1.5 // Large dt - gravity := mgl64.Vec3{0, 0, 0} - - rb.Integrate(dt, gravity) - - // CRITICAL BUG CHECK: drag*dt = 0.99*1.5 = 1.485 > 1 - // Formula v *= (1 - 1.485) = v *= (-0.485) would give NEGATIVE velocity! - // Expected behavior: velocity should be clamped to zero or positive - - if rb.Velocity.X() < 0 { - t.Errorf("CRITICAL BUG: Linear damping caused negative velocity! v = %v", rb.Velocity) - t.Logf("LinearDamping*dt = %v * %v = %v > 1", rb.Material.LinearDamping, dt, rb.Material.LinearDamping*dt) - t.Logf("Formula (1 - drag*dt) = %v (negative!)", 1-rb.Material.LinearDamping*dt) - } - - // Velocity should be zero or very small, not negative - if rb.Velocity.Len() > 1.0 { - t.Logf("WARNING: With extreme damping (drag*dt > 1), velocity = %v", rb.Velocity) - t.Logf("Expected velocity to be clamped near zero") - } -} - -// TestIntegrate_AngularDamping_Positive verifies angular damping reduces rotation correctly -func TestIntegrate_AngularDamping_Positive(t *testing.T) { - transform := NewTransform() - sphere := &Sphere{Radius: 1.0} - rb := NewRigidBody(transform, sphere, BodyTypeDynamic, 1.0) - - // Set angular damping - rb.Material.AngularDamping = 0.05 - - // Initial angular velocity - rb.AngularVelocity = mgl64.Vec3{20, 0, 0} - - dt := 0.1 - gravity := mgl64.Vec3{0, 0, 0} - - rb.Integrate(dt, gravity) - - // Expected: v_new = v_old * exp(-AngularDamping * dt) - expectedFactor := math.Exp(-rb.Material.AngularDamping * dt) - expectedAngularVelocity := mgl64.Vec3{20 * expectedFactor, 0, 0} - - if !vec3AlmostEqual(rb.AngularVelocity, expectedAngularVelocity, 1e-9) { - t.Errorf("AngularVelocity after damping = %v, want %v", rb.AngularVelocity, expectedAngularVelocity) - t.Logf("POTENTIAL BUG: Angular damping formula incorrect") - t.Logf("Expected formula: ω_new = ω_old * (1 - AngularDamping*dt)") - } - - // After many steps, angular velocity should approach zero - for i := 0; i < 2000; i++ { - rb.Integrate(dt, gravity) - } - - finalAngularSpeed := rb.AngularVelocity.Len() - if finalAngularSpeed > 0.1 { - t.Errorf("After 500 damping steps, angular speed = %v, expected near zero", finalAngularSpeed) - } -} - -// TestIntegrate_BothDampings verifies linear and angular damping work independently -func TestIntegrate_BothDampings(t *testing.T) { - transform := NewTransform() - sphere := &Sphere{Radius: 1.0} - rb := NewRigidBody(transform, sphere, BodyTypeDynamic, 1.0) - - // Set both dampings - rb.Material.LinearDamping = 0.1 - rb.Material.AngularDamping = 0.05 - - // Initial velocities - rb.Velocity = mgl64.Vec3{10, 0, 0} - rb.AngularVelocity = mgl64.Vec3{0, 20, 0} - - dt := 0.1 - gravity := mgl64.Vec3{0, 0, 0} - - rb.Integrate(dt, gravity) - - // Expected: v_new = v_old * exp(-LinearDamping * dt) - expectedFactor := math.Exp(-rb.Material.LinearDamping * dt) - expectedVelocity := mgl64.Vec3{10 * expectedFactor, 0, 0} - - if !vec3AlmostEqual(rb.Velocity, expectedVelocity, 1e-9) { - t.Errorf("Linear velocity = %v, want %v", rb.Velocity, expectedVelocity) - } - - // Expected: v_new = v_old * exp(-LinearDamping * dt) - expectedFactor = math.Exp(-rb.Material.AngularDamping * dt) - expectedAngularVelocity := mgl64.Vec3{0, 20 * expectedFactor, 0} - if !vec3AlmostEqual(rb.AngularVelocity, expectedAngularVelocity, 1e-9) { - t.Errorf("Angular velocity = %v, want %v", rb.AngularVelocity, expectedAngularVelocity) - } - - // Verify independence: apply more steps - for i := 0; i < 100; i++ { - rb.Integrate(dt, gravity) - } - - // Both should decay, but independently - finalLinearSpeed := rb.Velocity.Len() - finalAngularSpeed := rb.AngularVelocity.Len() - - if finalLinearSpeed == 0 || finalAngularSpeed == 0 { - t.Error("Damping caused complete stop too quickly") - } - - // Linear should decay faster (higher drag coefficient) - // After t=10s (100 steps * 0.1), decay factors: - // Linear: (0.99)^100 ≈ 0.366 - // Angular: (0.995)^100 ≈ 0.606 - // So angular should be larger relative to initial -} - // ============================================================================= // PHASE 2: Inertia Tensor Tests (Previously Untested) // ============================================================================= @@ -1005,20 +240,20 @@ func TestGetInertiaWorld_NoRotation(t *testing.T) { box := &Box{HalfExtents: mgl64.Vec3{1, 2, 3}} rb := NewRigidBody(transform, box, BodyTypeDynamic, 1.0) - I_world := rb.GetInertiaWorld() - I_local := rb.InertiaLocal + inertiaWorld := rb.GetInertiaWorld() + inertiaLocal := rb.InertiaLocal - // With identity rotation, I_world should equal I_local + // With identity rotation, inertiaWorld should equal inertiaLocal for i := 0; i < 3; i++ { for j := 0; j < 3; j++ { - if !almostEqual(I_world[i*3+j], I_local[i*3+j], 1e-10) { - t.Errorf("I_world[%d,%d] = %v, want %v (I_local)", i, j, I_world[i*3+j], I_local[i*3+j]) + if !almostEqual(inertiaWorld[i*3+j], inertiaLocal[i*3+j], 1e-10) { + t.Errorf("inertiaWorld[%d,%d] = %v, want %v (inertiaLocal)", i, j, inertiaWorld[i*3+j], inertiaLocal[i*3+j]) } } } } -// TestGetInertiaWorld_WithRotation verifies correct transformation I_world = R * I_local * R^T +// TestGetInertiaWorld_WithRotation verifies correct transformation inertiaWorld = R * inertiaLocal * R^T func TestGetInertiaWorld_WithRotation(t *testing.T) { transform := NewTransform() // Asymmetric box to make rotation effects visible @@ -1028,31 +263,31 @@ func TestGetInertiaWorld_WithRotation(t *testing.T) { // Rotate 90° around Z axis rb.Transform.Rotation = mgl64.QuatRotate(math.Pi/2, mgl64.Vec3{0, 0, 1}) - I_world := rb.GetInertiaWorld() - I_local := rb.InertiaLocal + inertiaWorld := rb.GetInertiaWorld() + inertiaLocal := rb.InertiaLocal - // After rotation, I_world should differ from I_local + // After rotation, inertiaWorld should differ from inertiaLocal different := false for i := 0; i < 3; i++ { for j := 0; j < 3; j++ { - if !almostEqual(I_world[i*3+j], I_local[i*3+j], 1e-6) { + if !almostEqual(inertiaWorld[i*3+j], inertiaLocal[i*3+j], 1e-6) { different = true } } } if !different { - t.Error("I_world should differ from I_local after rotation") + t.Error("inertiaWorld should differ from inertiaLocal after rotation") } - // Verify manual calculation: I_world = R * I_local * R^T + // Verify manual calculation: inertiaWorld = R * inertiaLocal * R^T R := rb.Transform.Rotation.Mat4().Mat3() - expected_I_world := R.Mul3(I_local).Mul3(R.Transpose()) + expectedInertiaWorld := R.Mul3(inertiaLocal).Mul3(R.Transpose()) for i := 0; i < 3; i++ { for j := 0; j < 3; j++ { - if !almostEqual(I_world[i*3+j], expected_I_world[i*3+j], 1e-9) { - t.Errorf("I_world[%d,%d] = %v, want %v (manual calc)", i, j, I_world[i*3+j], expected_I_world[i*3+j]) + if !almostEqual(inertiaWorld[i*3+j], expectedInertiaWorld[i*3+j], 1e-9) { + t.Errorf("inertiaWorld[%d,%d] = %v, want %v (manual calc)", i, j, inertiaWorld[i*3+j], expectedInertiaWorld[i*3+j]) } } } @@ -1073,22 +308,22 @@ func TestGetInertiaWorld_DifferentShapes(t *testing.T) { transform := NewTransform() rb := NewRigidBody(transform, tt.shape, BodyTypeDynamic, 1.0) - I_world := rb.GetInertiaWorld() + inertiaWorld := rb.GetInertiaWorld() // Inertia tensor should be symmetric for i := 0; i < 3; i++ { for j := 0; j < 3; j++ { - if !almostEqual(I_world[i*3+j], I_world[j*3+i], 1e-10) { - t.Errorf("%s: I_world not symmetric: I[%d,%d]=%v != I[%d,%d]=%v", - tt.name, i, j, I_world[i*3+j], j, i, I_world[j*3+i]) + if !almostEqual(inertiaWorld[i*3+j], inertiaWorld[j*3+i], 1e-10) { + t.Errorf("%s: inertiaWorld not symmetric: I[%d,%d]=%v != I[%d,%d]=%v", + tt.name, i, j, inertiaWorld[i*3+j], j, i, inertiaWorld[j*3+i]) } } } // Diagonal elements should be positive for i := 0; i < 3; i++ { - if I_world[i*3+i] <= 0 { - t.Errorf("%s: I_world[%d,%d] = %v, should be > 0", tt.name, i, i, I_world[i*3+i]) + if inertiaWorld[i*3+i] <= 0 { + t.Errorf("%s: inertiaWorld[%d,%d] = %v, should be > 0", tt.name, i, i, inertiaWorld[i*3+i]) } } }) @@ -1101,19 +336,19 @@ func TestGetInverseInertiaWorld_StaticBody(t *testing.T) { box := &Box{HalfExtents: mgl64.Vec3{1, 1, 1}} rb := NewRigidBody(transform, box, BodyTypeStatic, 1.0) - I_inv := rb.GetInverseInertiaWorld() + inverseInertia := rb.GetInverseInertiaWorld() // Static bodies should have zero inverse inertia (infinite inertia) for i := 0; i < 3; i++ { for j := 0; j < 3; j++ { - if I_inv[i*3+j] != 0 { - t.Errorf("Static body I_inv[%d,%d] = %v, want 0", i, j, I_inv[i*3+j]) + if inverseInertia[i*3+j] != 0 { + t.Errorf("Static body inverseInertia[%d,%d] = %v, want 0", i, j, inverseInertia[i*3+j]) } } } } -// TestGetInverseInertiaWorld_DynamicBody verifies I_inv * I = I * I_inv = Identity +// TestGetInverseInertiaWorld_DynamicBody verifies inverseInertia * I = I * inverseInertia = Identity func TestGetInverseInertiaWorld_DynamicBody(t *testing.T) { transform := NewTransform() box := &Box{HalfExtents: mgl64.Vec3{1, 2, 3}} @@ -1123,10 +358,10 @@ func TestGetInverseInertiaWorld_DynamicBody(t *testing.T) { rb.Transform.Rotation = mgl64.QuatRotate(math.Pi/4, mgl64.Vec3{1, 1, 0}.Normalize()) I := rb.GetInertiaWorld() - I_inv := rb.GetInverseInertiaWorld() + inverseInertia := rb.GetInverseInertiaWorld() - // Compute I * I_inv - product := I.Mul3(I_inv) + // Compute I * inverseInertia + product := I.Mul3(inverseInertia) // Should equal identity matrix identity := mgl64.Ident3() @@ -1134,7 +369,7 @@ func TestGetInverseInertiaWorld_DynamicBody(t *testing.T) { for i := 0; i < 3; i++ { for j := 0; j < 3; j++ { if !almostEqual(product[i*3+j], identity[i*3+j], 1e-6) { - t.Errorf("I * I_inv[%d,%d] = %v, want %v (identity)", i, j, product[i*3+j], identity[i*3+j]) + t.Errorf("I * inverseInertia[%d,%d] = %v, want %v (identity)", i, j, product[i*3+j], identity[i*3+j]) t.Logf("POTENTIAL BUG: Inverse inertia calculation incorrect") } } @@ -1196,7 +431,6 @@ func TestSupportWorld_Sphere_WithTranslation(t *testing.T) { func TestSupportWorld_Sphere_WithRotation(t *testing.T) { transform := NewTransform() transform.Rotation = mgl64.QuatRotate(math.Pi/4, mgl64.Vec3{0, 0, 1}) - transform.InverseRotation = transform.Rotation.Inverse() sphere := &Sphere{Radius: 1.0} rb := NewRigidBody(transform, sphere, BodyTypeDynamic, 1.0) @@ -1245,7 +479,6 @@ func TestSupportWorld_Box_WithRotation(t *testing.T) { transform := NewTransform() // Rotate 90° around Z axis transform.Rotation = mgl64.QuatRotate(math.Pi/2, mgl64.Vec3{0, 0, 1}) - transform.InverseRotation = transform.Rotation.Inverse() box := &Box{HalfExtents: mgl64.Vec3{2, 1, 0.5}} rb := NewRigidBody(transform, box, BodyTypeDynamic, 1.0) @@ -1273,7 +506,6 @@ func TestSupportWorld_Box_ArbitraryRotation(t *testing.T) { transform := NewTransform() transform.Position = mgl64.Vec3{5, 10, 15} transform.Rotation = mgl64.QuatRotate(math.Pi/3, mgl64.Vec3{1, 1, 1}.Normalize()) - transform.InverseRotation = transform.Rotation.Inverse() box := &Box{HalfExtents: mgl64.Vec3{1, 2, 3}} rb := NewRigidBody(transform, box, BodyTypeDynamic, 1.0) @@ -1430,71 +662,6 @@ func TestNewRigidBody_InfiniteDensity(t *testing.T) { } } -// TestIntegrate_NegativeTimeStep verifies behavior with negative dt -func TestIntegrate_NegativeTimeStep(t *testing.T) { - transform := NewTransform() - sphere := &Sphere{Radius: 1.0} - rb := NewRigidBody(transform, sphere, BodyTypeDynamic, 1.0) - - rb.Velocity = mgl64.Vec3{10, 0, 0} - initialPos := rb.Transform.Position - - dt := -0.1 // Negative time step - gravity := mgl64.Vec3{0, -10, 0} - - rb.Integrate(dt, gravity) - - // With negative dt, position should move backwards - // This is mathematically valid (time reversal simulation) - t.Logf("Negative dt integration: initial pos = %v, final pos = %v", initialPos, rb.Transform.Position) - t.Logf("This tests if the engine supports time-reversal (uncommon but valid)") -} - -// TestIntegrate_VerySmallMass verifies behavior with near-zero mass -func TestIntegrate_VerySmallMass(t *testing.T) { - transform := NewTransform() - sphere := &Sphere{Radius: 1.0} - rb := NewRigidBody(transform, sphere, BodyTypeDynamic, 1e-10) // Very small density - - dt := 0.1 - gravity := mgl64.Vec3{0, -10, 0} - - rb.Integrate(dt, gravity) - - // With very small mass, acceleration should be normal (F=ma, a=F/m) - // Gravity force = g*m, acceleration = g*m/m = g (mass cancels) - // So even tiny mass should fall at normal rate - expectedVelocity := mgl64.Vec3{0, -1, 0} // g*dt - if !vec3AlmostEqual(rb.Velocity, expectedVelocity, 1e-8) { - t.Errorf("Very small mass velocity = %v, want %v (mass should cancel in gravity)", - rb.Velocity, expectedVelocity) - } -} - -// TestIntegrate_InfiniteMass_Dynamic verifies dynamic body with infinite mass -func TestIntegrate_InfiniteMass_Dynamic(t *testing.T) { - transform := NewTransform() - sphere := &Sphere{Radius: 1.0} - rb := NewRigidBody(transform, sphere, BodyTypeDynamic, math.Inf(1)) - - rb.Velocity = mgl64.Vec3{10, 0, 0} - - dt := 0.1 - gravity := mgl64.Vec3{0, -10, 0} - - rb.Integrate(dt, gravity) - - // With infinite mass and gravity force = g*m = g*inf = inf - // Acceleration = F/m = inf/inf = undefined (NaN expected) - // This is an edge case - behavior depends on implementation - - if math.IsNaN(rb.Velocity.X()) || math.IsNaN(rb.Velocity.Y()) || math.IsNaN(rb.Velocity.Z()) { - t.Logf("Infinite mass produced NaN velocity (expected edge case)") - } else { - t.Logf("Infinite mass velocity = %v (no NaN)", rb.Velocity) - } -} - // TestSupportWorld_UnnormalizedQuaternion verifies behavior with bad quaternion func TestSupportWorld_UnnormalizedQuaternion(t *testing.T) { transform := NewTransform() @@ -1513,165 +680,10 @@ func TestSupportWorld_UnnormalizedQuaternion(t *testing.T) { t.Logf("This tests edge case handling of invalid quaternions") } -// TestIntegrate_HighAngularVelocity verifies stability with extreme rotation -func TestIntegrate_HighAngularVelocity(t *testing.T) { - transform := NewTransform() - sphere := &Sphere{Radius: 1.0} - rb := NewRigidBody(transform, sphere, BodyTypeDynamic, 1.0) - - // Very high angular velocity (100 rad/s = ~16 revolutions/second) - rb.AngularVelocity = mgl64.Vec3{100, 0, 0} - - dt := 0.01 - gravity := mgl64.Vec3{0, 0, 0} - - // Integrate many steps - for i := 0; i < 1000; i++ { - rb.Integrate(dt, gravity) - } - - // Check for numerical instability - quatMagnitude := math.Sqrt(rb.Transform.Rotation.W*rb.Transform.Rotation.W + - rb.Transform.Rotation.V.X()*rb.Transform.Rotation.V.X() + - rb.Transform.Rotation.V.Y()*rb.Transform.Rotation.V.Y() + - rb.Transform.Rotation.V.Z()*rb.Transform.Rotation.V.Z()) - - if !almostEqual(quatMagnitude, 1.0, 1e-3) { - t.Errorf("After high angular velocity, quaternion magnitude = %v, want 1.0 (BUG: drift)", quatMagnitude) - } - - // Check for NaN - if math.IsNaN(rb.Transform.Rotation.W) { - t.Error("High angular velocity produced NaN quaternion (BUG: numerical instability)") - } -} - // ============================================================================= // PHASE 7: Mathematical Consistency Tests // ============================================================================= -// TestIntegrate_EnergyConservation verifies energy conservation without damping -func TestIntegrate_EnergyConservation(t *testing.T) { - transform := NewTransform() - box := &Box{HalfExtents: mgl64.Vec3{1, 2, 3}} - rb := NewRigidBody(transform, box, BodyTypeDynamic, 1.0) - - // Set initial velocities - rb.Velocity = mgl64.Vec3{5, 0, 0} - rb.AngularVelocity = mgl64.Vec3{0, 2, 0} - - // Compute initial kinetic energy: E = (1/2)*m*v² + (1/2)*ω^T*I*ω - mass := rb.Material.GetMass() - v2 := rb.Velocity.Dot(rb.Velocity) - linearKE := 0.5 * mass * v2 - - I := rb.GetInertiaWorld() - I_omega := I.Mul3x1(rb.AngularVelocity) - angularKE := 0.5 * rb.AngularVelocity.Dot(I_omega) - - initialEnergy := linearKE + angularKE - - dt := 0.01 - gravity := mgl64.Vec3{0, 0, 0} // No gravity - // No damping (already default 0) - - // Integrate multiple steps - for i := 0; i < 100; i++ { - rb.Integrate(dt, gravity) - } - - // Compute final energy - v2_final := rb.Velocity.Dot(rb.Velocity) - linearKE_final := 0.5 * mass * v2_final - - I_final := rb.GetInertiaWorld() - I_omega_final := I_final.Mul3x1(rb.AngularVelocity) - angularKE_final := 0.5 * rb.AngularVelocity.Dot(I_omega_final) - - finalEnergy := linearKE_final + angularKE_final - - // Energy should be conserved (no forces, no damping) - energyDiff := math.Abs(finalEnergy - initialEnergy) - relativeError := energyDiff / initialEnergy - - if relativeError > 0.01 { // Allow 1% error for numerical integration - t.Logf("Energy conservation test:") - t.Logf("Initial energy = %v", initialEnergy) - t.Logf("Final energy = %v", finalEnergy) - t.Logf("Difference = %v (%.2f%%)", energyDiff, relativeError*100) - t.Logf("Note: Some drift expected due to numerical integration") - } -} - -// TestIntegrate_MomentumConservation verifies linear momentum conservation -func TestIntegrate_MomentumConservation(t *testing.T) { - transform := NewTransform() - sphere := &Sphere{Radius: 1.0} - rb := NewRigidBody(transform, sphere, BodyTypeDynamic, 1.0) - - rb.Velocity = mgl64.Vec3{10, 5, 3} - - // Initial momentum: p = m*v - mass := rb.Material.GetMass() - initialMomentum := rb.Velocity.Mul(mass) - - dt := 0.01 - gravity := mgl64.Vec3{0, 0, 0} // No external forces - - // Integrate - for i := 0; i < 100; i++ { - rb.Integrate(dt, gravity) - } - - // Final momentum - finalMomentum := rb.Velocity.Mul(mass) - - // Momentum should be conserved - if !vec3AlmostEqual(finalMomentum, initialMomentum, 1e-6) { - t.Logf("Momentum conservation:") - t.Logf("Initial = %v", initialMomentum) - t.Logf("Final = %v", finalMomentum) - t.Logf("Difference = %v", finalMomentum.Sub(initialMomentum)) - } -} - -// TestIntegrate_AngularMomentumConservation verifies angular momentum conservation -func TestIntegrate_AngularMomentumConservation(t *testing.T) { - transform := NewTransform() - box := &Box{HalfExtents: mgl64.Vec3{1, 2, 0.5}} - rb := NewRigidBody(transform, box, BodyTypeDynamic, 1.0) - - rb.AngularVelocity = mgl64.Vec3{3, 2, 1} - - // Initial angular momentum: L = I*ω - I_initial := rb.GetInertiaWorld() - L_initial := I_initial.Mul3x1(rb.AngularVelocity) - - dt := 0.01 - gravity := mgl64.Vec3{0, 0, 0} // No external torques - - // Integrate - for i := 0; i < 100; i++ { - rb.Integrate(dt, gravity) - } - - // Final angular momentum - I_final := rb.GetInertiaWorld() - L_final := I_final.Mul3x1(rb.AngularVelocity) - - // Angular momentum should be conserved (approximately) - diff := L_final.Sub(L_initial) - diffMagnitude := diff.Len() - - if diffMagnitude > 0.1 { - t.Logf("Angular momentum conservation:") - t.Logf("Initial L = %v", L_initial) - t.Logf("Final L = %v", L_final) - t.Logf("Difference magnitude = %v", diffMagnitude) - t.Logf("Note: Some drift expected with gyroscopic term") - } -} - // TestGetInertiaWorld_Symmetry verifies inertia tensor is symmetric func TestGetInertiaWorld_Symmetry(t *testing.T) { transform := NewTransform() @@ -1718,124 +730,6 @@ func TestGetInertiaWorld_PositiveDefinite(t *testing.T) { // PHASE 8: Regression Tests // ============================================================================= -// TestIntegrate_LongSimulation verifies numerical stability over many steps -func TestIntegrate_LongSimulation(t *testing.T) { - transform := NewTransform() - sphere := &Sphere{Radius: 1.0} - rb := NewRigidBody(transform, sphere, BodyTypeDynamic, 1.0) - - rb.Velocity = mgl64.Vec3{1, 2, 3} - rb.AngularVelocity = mgl64.Vec3{0.5, 0.5, 0.5} - - dt := 0.01 - gravity := mgl64.Vec3{0, -10, 0} - - // Simulate 1000 steps (10 seconds) - for i := 0; i < 1000; i++ { - rb.Integrate(dt, gravity) - - // Check for NaN at each step - if math.IsNaN(rb.Velocity.X()) || math.IsNaN(rb.Velocity.Y()) || math.IsNaN(rb.Velocity.Z()) { - t.Fatalf("NaN velocity at step %d (BUG: numerical instability)", i) - } - - if math.IsNaN(rb.Transform.Position.X()) { - t.Fatalf("NaN position at step %d (BUG: numerical instability)", i) - } - - if math.IsNaN(rb.Transform.Rotation.W) { - t.Fatalf("NaN quaternion at step %d (BUG: numerical instability)", i) - } - } - - // Verify quaternion still normalized - quatMag := math.Sqrt(rb.Transform.Rotation.W*rb.Transform.Rotation.W + - rb.Transform.Rotation.V.X()*rb.Transform.Rotation.V.X() + - rb.Transform.Rotation.V.Y()*rb.Transform.Rotation.V.Y() + - rb.Transform.Rotation.V.Z()*rb.Transform.Rotation.V.Z()) - - if !almostEqual(quatMag, 1.0, 1e-4) { - t.Errorf("After 1000 steps, quaternion magnitude = %v, want 1.0", quatMag) - } -} - -// TestIntegrate_CompareIntegrationMethods documents current integration method -func TestIntegrate_CompareIntegrationMethods(t *testing.T) { - // This test documents that we use semi-implicit Euler integration - // (velocity updated first, then position uses new velocity) - - transform := NewTransform() - sphere := &Sphere{Radius: 1.0} - rb := NewRigidBody(transform, sphere, BodyTypeDynamic, 1.0) - - rb.Velocity = mgl64.Vec3{0, 0, 0} - - dt := 1.0 - gravity := mgl64.Vec3{0, -10, 0} - - rb.Integrate(dt, gravity) - - // Semi-implicit Euler: - // v_new = v_old + a*dt = 0 + (-10)*1 = -10 - // p_new = p_old + v_new*dt = 0 + (-10)*1 = -10 - - // Explicit Euler would give: - // v_new = 0 + (-10)*1 = -10 - // p_new = p_old + v_old*dt = 0 + 0*1 = 0 - - expectedVelocity := mgl64.Vec3{0, -10, 0} - expectedPosition := mgl64.Vec3{0, -10, 0} // Semi-implicit uses new velocity - - if !vec3AlmostEqual(rb.Velocity, expectedVelocity, 1e-10) { - t.Errorf("Velocity = %v, want %v", rb.Velocity, expectedVelocity) - } - - if !vec3AlmostEqual(rb.Transform.Position, expectedPosition, 1e-10) { - t.Logf("Position = %v", rb.Transform.Position) - t.Logf("Expected (semi-implicit) = %v", expectedPosition) - t.Logf("This documents the integration method used") - } -} - -// TestIntegrate_HighAngularVelocity_Stability verifies no explosion with fast rotation -func TestIntegrate_HighAngularVelocity_Stability(t *testing.T) { - transform := NewTransform() - box := &Box{HalfExtents: mgl64.Vec3{1, 2, 3}} - rb := NewRigidBody(transform, box, BodyTypeDynamic, 1.0) - - // Extreme angular velocity - rb.AngularVelocity = mgl64.Vec3{100, 50, 75} - - dt := 0.01 - gravity := mgl64.Vec3{0, 0, 0} - - initialSpeed := rb.AngularVelocity.Len() - - // Integrate - for i := 0; i < 1000; i++ { - rb.Integrate(dt, gravity) - } - - finalSpeed := rb.AngularVelocity.Len() - - // Angular speed should remain roughly constant (no external torque, no damping) - // Allow some drift due to gyroscopic effects - speedRatio := finalSpeed / initialSpeed - - if speedRatio > 2.0 || speedRatio < 0.5 { - t.Logf("High angular velocity stability:") - t.Logf("Initial speed = %v rad/s", initialSpeed) - t.Logf("Final speed = %v rad/s", finalSpeed) - t.Logf("Ratio = %v (expected ~1.0)", speedRatio) - t.Logf("POTENTIAL BUG: Angular velocity unstable with high speeds") - } - - // Check for NaN - if math.IsNaN(finalSpeed) { - t.Error("High angular velocity produced NaN (BUG: numerical instability)") - } -} - // Helper function to compare floats with epsilon tolerance func almostEqual(a, b, epsilon float64) bool { return math.Abs(a-b) < epsilon @@ -1848,10 +742,16 @@ func vec3AlmostEqual(a, b mgl64.Vec3, epsilon float64) bool { almostEqual(a.Z(), b.Z(), epsilon) } -// Helper function to compare quaternions with epsilon tolerance -func quatAlmostEqual(a, b mgl64.Quat, epsilon float64) bool { - return almostEqual(a.W, b.W, epsilon) && - almostEqual(a.V.X(), b.V.X(), epsilon) && - almostEqual(a.V.Y(), b.V.Y(), epsilon) && - almostEqual(a.V.Z(), b.V.Z(), epsilon) +// A static body has no inertia: no infinite or NaN value, and a unique serial like every body +func TestStaticBodyInertia(t *testing.T) { + static := NewRigidBody(NewTransform(), &Box{HalfExtents: mgl64.Vec3{1, 1, 1}}, BodyTypeStatic, 0) + for i := 0; i < 9; i++ { + if static.InertiaLocal[i] != 0 || static.InverseInertiaLocal[i] != 0 { + t.Fatalf("static inertia %v, inverse %v: want 0", static.InertiaLocal, static.InverseInertiaLocal) + } + } + other := NewRigidBody(NewTransform(), &Box{HalfExtents: mgl64.Vec3{1, 1, 1}}, BodyTypeStatic, 0) + if static.Serial() == 0 || static.Serial() == other.Serial() { + t.Errorf("serials %d and %d", static.Serial(), other.Serial()) + } } diff --git a/actor/shape.go b/actor/shape.go index 40e046d..9cb2ea8 100644 --- a/actor/shape.go +++ b/actor/shape.go @@ -6,45 +6,38 @@ import ( "github.com/go-gl/mathgl/mgl64" ) -// ShapeType represents the type of collision shape -type ShapeType int - -const ( - ShapeTypeSphere ShapeType = iota - ShapeTypeBox - ShapeTypePlane -) - +// ContactPoint is a contact against a plane: Position lies halfway between the shape's +// surface and the plane, Separation is their signed distance (negative when overlapping). type ContactPoint struct { - Position mgl64.Vec3 - Penetration float64 + Position mgl64.Vec3 + Separation float64 } type PlaneContact []ContactPoint // ShapeInterface is the interface that all collision shapes must implement type ShapeInterface interface { - // ComputeAABB calculates the axis-aligned bounding box for the shape - // at the given transform - ComputeAABB(transform Transform) - GetAABB() AABB + // ComputeAABB returns the axis-aligned bounding box of the shape at the transform. + // A shape has no state: it can be shared by several bodies, each body keeps its AABB + ComputeAABB(transform Transform) AABB // ComputeMass calculates mass data for the shape given a density ComputeMass(density float64) float64 ComputeInertia(mass float64) mgl64.Mat3 Support(direction mgl64.Vec3) mgl64.Vec3 GetContactFeature(direction mgl64.Vec3, output *[8]mgl64.Vec3, count *int) - CollideWithPlane(planeNormal mgl64.Vec3, planeDistance float64, myTransform Transform) (bool, PlaneContact) + // CollideWithPlane appends to contacts the points of the shape closer to the plane than the margin + // (plane: planeNormal·p + planeDistance = 0). contacts is a buffer given by the caller, to avoid allocations + CollideWithPlane(planeNormal mgl64.Vec3, planeDistance float64, myTransform Transform, margin float64, contacts PlaneContact) PlaneContact } // Box represents an oriented box collision shape // The box is defined by its half-extents (half-width, half-height, half-depth) type Box struct { HalfExtents mgl64.Vec3 - aabb AABB } -func (b *Box) ComputeAABB(transform Transform) { - // Les 8 coins de la boîte en espace local +func (b *Box) ComputeAABB(transform Transform) AABB { + // the 8 corners of the box, in the local space corners := [8]mgl64.Vec3{ {-b.HalfExtents.X(), -b.HalfExtents.Y(), -b.HalfExtents.Z()}, {+b.HalfExtents.X(), -b.HalfExtents.Y(), -b.HalfExtents.Z()}, @@ -56,29 +49,24 @@ func (b *Box) ComputeAABB(transform Transform) { {+b.HalfExtents.X(), +b.HalfExtents.Y(), +b.HalfExtents.Z()}, } - // Transformer le premier coin pour initialiser min/max - worldCorner := transform.Rotation.Rotate(corners[0]).Add(transform.Position) + // the first corner initializes min & max, the other corners extend the AABB + q, position := &transform.Rotation, transform.Position + worldCorner := Rotate(q, corners[0]).Add(position) min := worldCorner max := worldCorner - - // Transformer tous les autres coins et étendre l'AABB for i := 1; i < 8; i++ { - worldCorner = transform.Rotation.Rotate(corners[i]).Add(transform.Position) - - min[0] = math.Min(min[0], worldCorner[0]) - min[1] = math.Min(min[1], worldCorner[1]) - min[2] = math.Min(min[2], worldCorner[2]) - - max[0] = math.Max(max[0], worldCorner[0]) - max[1] = math.Max(max[1], worldCorner[1]) - max[2] = math.Max(max[2], worldCorner[2]) + worldCorner = Rotate(q, corners[i]).Add(position) + for k := 0; k < 3; k++ { + if worldCorner[k] < min[k] { + min[k] = worldCorner[k] + } + if worldCorner[k] > max[k] { + max[k] = worldCorner[k] + } + } } - b.aabb = AABB{Min: min, Max: max} -} - -func (b *Box) GetAABB() AABB { - return b.aabb + return AABB{Min: min, Max: max} } // ComputeMass calculates mass data for the box @@ -90,12 +78,12 @@ func (b *Box) ComputeMass(density float64) float64 { } func (b *Box) ComputeInertia(mass float64) mgl64.Mat3 { - // Dimensions complètes + // full dimensions x := b.HalfExtents.X() * 2 y := b.HalfExtents.Y() * 2 z := b.HalfExtents.Z() * 2 - // Formule pour une boîte : I = (m/12) * (dimension1² + dimension2²) + // box: I = (m/12) * (dimension1² + dimension2²) factor := mass / 12.0 ix := factor * (y*y + z*z) iy := factor * (x*x + z*z) @@ -125,13 +113,12 @@ func (b *Box) Support(direction mgl64.Vec3) mgl64.Vec3 { } func (b *Box) GetContactFeature(direction mgl64.Vec3, output *[8]mgl64.Vec3, count *int) { - // Trouver la face la plus alignée + // the face the most aligned with the direction axes := [3]mgl64.Vec3{ {1, 0, 0}, {0, 1, 0}, {0, 0, 1}, } - // ========== FIX : Comparer les valeurs absolues directement ========== - maxAbsDot := 0.0 // Commence à 0, pas -∞ + maxAbsDot := 0.0 bestAxisIdx := 0 sign := 1.0 @@ -152,7 +139,7 @@ func (b *Box) GetContactFeature(direction mgl64.Vec3, output *[8]mgl64.Vec3, cou halfSize := b.HalfExtents - // Générer les 4 coins selon la face + // the 4 corners of the face switch bestAxisIdx { case 0: x := sign * halfSize.X() @@ -178,73 +165,46 @@ func (b *Box) GetContactFeature(direction mgl64.Vec3, output *[8]mgl64.Vec3, cou } } -// CollideWithPlane - Collision Box/Plane -func (b *Box) CollideWithPlane(planeNormal mgl64.Vec3, planeDistance float64, myTransform Transform) (bool, PlaneContact) { - h := b.HalfExtents - localVertices := [8]mgl64.Vec3{ - {-h.X(), -h.Y(), -h.Z()}, - {-h.X(), -h.Y(), h.Z()}, - {-h.X(), h.Y(), -h.Z()}, - {-h.X(), h.Y(), h.Z()}, - {h.X(), -h.Y(), -h.Z()}, - {h.X(), -h.Y(), h.Z()}, - {h.X(), h.Y(), -h.Z()}, - {h.X(), h.Y(), h.Z()}, - } - - var contactPoints []ContactPoint - maxDepth := 0.0 - - for _, vertex := range localVertices { - worldVertex := myTransform.Rotation.Rotate(vertex).Add(myTransform.Position) - distance := worldVertex.Sub(planeNormal.Mul(-planeDistance)).Dot(planeNormal) - - if distance < 0 { - depth := -distance - if depth > maxDepth { - maxDepth = depth - } - pointOnPlane := worldVertex.Sub(planeNormal.Mul(distance)) - - contactPoints = append(contactPoints, ContactPoint{ - Position: pointOnPlane, - Penetration: depth, - }) +// CollideWithPlane returns the corners of the supporting face of the box (the face the most opposed to the normal of the +// plane, as the incident face of Jolt) closer to the plane than the margin. The other corners are behind this face: +// with a large margin, a thin box would give the corners of its top face instead of the deepest ones +func (b *Box) CollideWithPlane(planeNormal mgl64.Vec3, planeDistance float64, myTransform Transform, margin float64, contacts PlaneContact) PlaneContact { + var face [8]mgl64.Vec3 + var count int + q := &myTransform.Rotation + b.GetContactFeature(RotateInverse(q, planeNormal.Mul(-1)), &face, &count) + + for _, vertex := range face[:count] { + worldVertex := myTransform.Position.Add(Rotate(q, vertex)) + separation := worldVertex.Dot(planeNormal) + planeDistance + if separation > margin { + continue } + contacts = append(contacts, ContactPoint{ + Position: worldVertex.Sub(planeNormal.Mul(separation / 2)), + Separation: separation, + }) } - if len(contactPoints) == 0 { - return false, PlaneContact{} - } - - if len(contactPoints) > 4 { - contactPoints = reduceTo4ContactPoints(contactPoints, planeNormal) - } - - return true, contactPoints + return contacts } // Sphere represents a spherical collision shape type Sphere struct { Radius float64 - aabb AABB } // ComputeAABB calculates the axis-aligned bounding box for the sphere -func (s *Sphere) ComputeAABB(transform Transform) { +func (s *Sphere) ComputeAABB(transform Transform) AABB { // Sphere AABB is not affected by rotation, only by position radiusVec := mgl64.Vec3{s.Radius, s.Radius, s.Radius} - s.aabb = AABB{ + return AABB{ Min: transform.Position.Sub(radiusVec), Max: transform.Position.Add(radiusVec), } } -func (s *Sphere) GetAABB() AABB { - return s.aabb -} - // ComputeMass calculates mass data for the sphere func (s *Sphere) ComputeMass(density float64) float64 { // Volume of sphere = (4/3) * π * r³ @@ -254,10 +214,10 @@ func (s *Sphere) ComputeMass(density float64) float64 { } func (s *Sphere) ComputeInertia(mass float64) mgl64.Mat3 { - // Pour une sphère : I = (2/5) * m * r² + // sphere: I = (2/5) * m * r² i := (2.0 / 5.0) * mass * s.Radius * s.Radius - // Une sphère a la même inertie sur tous les axes + // the same inertia on all the axes return mgl64.Mat3{ i, 0, 0, 0, i, 0, @@ -266,7 +226,11 @@ func (s *Sphere) ComputeInertia(mass float64) mgl64.Mat3 { } func (s *Sphere) Support(direction mgl64.Vec3) mgl64.Vec3 { - return direction.Normalize().Mul(s.Radius) + length := direction.Len() + if length == 0 { + return mgl64.Vec3{0, s.Radius, 0} + } + return direction.Mul(s.Radius / length) } func (s *Sphere) GetContactFeature(direction mgl64.Vec3, output *[8]mgl64.Vec3, count *int) { @@ -274,22 +238,18 @@ func (s *Sphere) GetContactFeature(direction mgl64.Vec3, output *[8]mgl64.Vec3, *count = 1 } -func (s *Sphere) CollideWithPlane(planeNormal mgl64.Vec3, planeDistance float64, myTransform Transform) (bool, PlaneContact) { +// CollideWithPlane returns the lowest point of the sphere, if closer to the plane than the margin +func (s *Sphere) CollideWithPlane(planeNormal mgl64.Vec3, planeDistance float64, myTransform Transform, margin float64, contacts PlaneContact) PlaneContact { center := myTransform.Position - distance := center.Sub(planeNormal.Mul(-planeDistance)).Dot(planeNormal) - depth := s.Radius - distance - - if depth <= 0 { - return false, PlaneContact{} + separation := center.Dot(planeNormal) + planeDistance - s.Radius + if separation > margin { + return contacts } - contactPoint := center.Sub(planeNormal.Mul(distance)) - - return true, []ContactPoint{{ - Position: contactPoint, - Penetration: depth, - }, - } + return append(contacts, ContactPoint{ + Position: center.Sub(planeNormal.Mul(s.Radius + separation/2)), + Separation: separation, + }) } // Plane represents an infinite plane collision shape @@ -299,52 +259,12 @@ func (s *Sphere) CollideWithPlane(planeNormal mgl64.Vec3, planeDistance float64, type Plane struct { Normal mgl64.Vec3 // Plane normal (must be normalized) Distance float64 // Plane constant (signed distance from origin) - aabb AABB -} - -// This method is bypassed, because planes are automatically included from the broad phase to the narrow phase -// We use specific functions for plane / convex shapes collision -func (p *Plane) ComputeAABB(transform Transform) { - const thickness = 10.0 // épaisseur de détection du plan - const infinity = 100.0 // grande valeur pour les dimensions infinies - - // Point on the plane closest to the origin - // Assumes p.Normal is normalized - planePoint := p.Normal.Mul(-p.Distance) - - // Create base bounds with thickness along the normal - min := planePoint.Sub(p.Normal.Mul(thickness)).Add(transform.Position) - max := planePoint.Add(transform.Position) - - // Extend the AABB to infinity in directions perpendicular to the normal - absNormal := mgl64.Vec3{ - math.Abs(p.Normal.X()), - math.Abs(p.Normal.Y()), - math.Abs(p.Normal.Z()), - } - - // Find the dominant axis (the one aligned with the normal) - threshold := 1.0 // threshold to consider an axis as dominant - - // For NON-dominant axes, extend to infinity - if absNormal.X() < threshold { - min[0] = -infinity - max[0] = infinity - } - if absNormal.Y() < threshold { - min[1] = -infinity - max[1] = infinity - } - if absNormal.Z() < threshold { - min[2] = -infinity - max[2] = infinity - } - - p.aabb = AABB{Min: min, Max: max} } -func (p *Plane) GetAABB() AABB { - return p.aabb +// ComputeAABB: a plane is infinite, its AABB is the whole space. The planes are tested with every body +func (p *Plane) ComputeAABB(transform Transform) AABB { + infinity := math.Inf(1) + return AABB{Min: mgl64.Vec3{-infinity, -infinity, -infinity}, Max: mgl64.Vec3{infinity, infinity, infinity}} } // ComputeMass calculates mass data for the plane @@ -359,7 +279,7 @@ func (p *Plane) ComputeInertia(mass float64) mgl64.Mat3 { return mgl64.Mat3{} } -// For simplicity, we use a 10000 width/height box. Can obviously break for bigger planes +// Support: a plane has no support point, the narrow phase uses CollideWithPlane of the other shape func (p *Plane) Support(direction mgl64.Vec3) mgl64.Vec3 { return mgl64.Vec3{} } @@ -371,60 +291,6 @@ func (p *Plane) GetContactFeature(direction mgl64.Vec3, output *[8]mgl64.Vec3, c } // CollideWithPlane - Plane/Plane collision (not supported) -func (p *Plane) CollideWithPlane(planeNormal mgl64.Vec3, planeDistance float64, myTransform Transform) (bool, PlaneContact) { - return false, PlaneContact{} -} - -// Helper to generate the tangent basis -func getTangentBasis(normal mgl64.Vec3) (mgl64.Vec3, mgl64.Vec3) { - var tangent1 mgl64.Vec3 - if math.Abs(normal.X()) > 0.9 { - tangent1 = mgl64.Vec3{0, 1, 0} - } else { - tangent1 = mgl64.Vec3{1, 0, 0} - } - - tangent1 = tangent1.Sub(normal.Mul(tangent1.Dot(normal))).Normalize() - tangent2 := normal.Cross(tangent1).Normalize() - - return tangent1, tangent2 -} - -func reduceTo4ContactPoints(points []ContactPoint, normal mgl64.Vec3) []ContactPoint { - tangent1, tangent2 := getTangentBasis(normal) - - minX, maxX, minY, maxY := 0, 0, 0, 0 - minXval, maxXval := math.Inf(1), math.Inf(-1) - minYval, maxYval := math.Inf(1), math.Inf(-1) - - for i, p := range points { - x := p.Position.Dot(tangent1) - y := p.Position.Dot(tangent2) - - if x < minXval { - minXval, minX = x, i - } - if x > maxXval { - maxXval, maxX = x, i - } - if y < minYval { - minYval, minY = y, i - } - if y > maxYval { - maxYval, maxY = y, i - } - } - - indices := [4]int{minX, maxX, minY, maxY} - seen := make(map[int]bool) - result := make([]ContactPoint, 0, 4) - - for _, idx := range indices { - if !seen[idx] { - seen[idx] = true - result = append(result, points[idx]) - } - } - - return result +func (p *Plane) CollideWithPlane(planeNormal mgl64.Vec3, planeDistance float64, myTransform Transform, margin float64, contacts PlaneContact) PlaneContact { + return contacts } diff --git a/actor/shape_test.go b/actor/shape_test.go index 818ab6a..db2741b 100644 --- a/actor/shape_test.go +++ b/actor/shape_test.go @@ -191,8 +191,7 @@ func TestBoxComputeAABBWithRotation(t *testing.T) { for _, tt := range tests { t.Run(tt.name, func(t *testing.T) { - tt.box.ComputeAABB(tt.transform) - aabb := tt.box.GetAABB() + aabb := tt.box.ComputeAABB(tt.transform) // Vérifications de base if !vec3Equal(aabb.Min, tt.expectedMin, 1e-3) { @@ -344,8 +343,7 @@ func TestShapeConsistency(t *testing.T) { Rotation: mgl64.QuatRotate(mgl64.DegToRad(45), mgl64.Vec3{0, 0, 1}), } - box.ComputeAABB(transform) - aabb := box.GetAABB() + aabb := box.ComputeAABB(transform) // L'AABB doit contenir tous les coins transformés corners := [8]mgl64.Vec3{ @@ -429,8 +427,7 @@ func TestSphereComputeAABB(t *testing.T) { for _, tt := range tests { t.Run(tt.name, func(t *testing.T) { - tt.sphere.ComputeAABB(tt.transform) - aabb := tt.sphere.GetAABB() + aabb := tt.sphere.ComputeAABB(tt.transform) // Vérifications de base if !vec3Equal(aabb.Min, tt.expectedMin, 1e-9) { @@ -452,8 +449,7 @@ func TestSphereComputeAABB(t *testing.T) { Rotation: mgl64.QuatIdent(), } - tt.sphere.ComputeAABB(transformNoRotation) - aabbNoRotation := tt.sphere.GetAABB() + aabbNoRotation := tt.sphere.ComputeAABB(transformNoRotation) if !aabb.Min.ApproxEqual(aabbNoRotation.Min) || !aabb.Max.ApproxEqual(aabbNoRotation.Max) { t.Errorf("Sphere AABB affected by rotation, but should not be") } @@ -461,69 +457,18 @@ func TestSphereComputeAABB(t *testing.T) { } } -func TestGetTangentBasis(t *testing.T) { - tests := []struct { - name string - normal mgl64.Vec3 - expectedLengths [2]float64 // longueur attendue des deux tangents - }{ - { - name: "X-axis normal", - normal: mgl64.Vec3{1, 0, 0}, - expectedLengths: [2]float64{1, 1}, - }, - { - name: "Y-axis normal", - normal: mgl64.Vec3{0, 1, 0}, - expectedLengths: [2]float64{1, 1}, - }, - { - name: "Z-axis normal", - normal: mgl64.Vec3{0, 0, 1}, - expectedLengths: [2]float64{1, 1}, - }, - { - name: "diagonal normal", - normal: mgl64.Vec3{1, 1, 1}.Normalize(), - expectedLengths: [2]float64{1, 1}, - }, - { - name: "arbitrary normal", - normal: mgl64.Vec3{0.5, 0.8, 0.3}.Normalize(), - expectedLengths: [2]float64{1, 1}, - }, +// A thin box tilted over a plane, with a margin larger than its thickness: only the corners of its bottom face touch +func TestBoxCollideWithPlaneKeepsSupportingFace(t *testing.T) { + box := &Box{HalfExtents: mgl64.Vec3{0.3, 0.02, 0.3}} + transform := Transform{Position: mgl64.Vec3{0, 0.1, 0}, Rotation: mgl64.QuatRotate(0.05, mgl64.Vec3{1, 0, 0})} + contacts := box.CollideWithPlane(mgl64.Vec3{0, 1, 0}, 0, transform, 1, nil) + if len(contacts) != 4 { + t.Fatalf("%d contacts, want 4", len(contacts)) } - - for _, tt := range tests { - t.Run(tt.name, func(t *testing.T) { - tangent1, tangent2 := getTangentBasis(tt.normal) - - // Les tangents doivent avoir une longueur de 1 - if !floatEqual(tangent1.Len(), tt.expectedLengths[0], 1e-6) { - t.Errorf("Tangent1 length = %v, want %v", tangent1.Len(), tt.expectedLengths[0]) - } - if !floatEqual(tangent2.Len(), tt.expectedLengths[1], 1e-6) { - t.Errorf("Tangent2 length = %v, want %v", tangent2.Len(), tt.expectedLengths[1]) - } - - // Les tangents doivent être perpendiculaires à la normale - if math.Abs(tangent1.Dot(tt.normal)) > 1e-6 { - t.Errorf("Tangent1 not perpendicular to normal: dot = %v", tangent1.Dot(tt.normal)) - } - if math.Abs(tangent2.Dot(tt.normal)) > 1e-6 { - t.Errorf("Tangent2 not perpendicular to normal: dot = %v", tangent2.Dot(tt.normal)) - } - - // Les deux tangents doivent être perpendiculaires entre elles - if math.Abs(tangent1.Dot(tangent2)) > 1e-6 { - t.Errorf("Tangents not perpendicular to each other: dot = %v", tangent1.Dot(tangent2)) - } - - // Le produit vectoriel normal x tangent1 doit donner tangent2 (ou son opposé) - cross := tt.normal.Cross(tangent1) - if !vec3Equal(cross, tangent2, 1e-6) && !vec3Equal(cross, tangent2.Mul(-1), 1e-6) { - t.Errorf("Cross product not equal to tangent2: cross=%v, tangent2=%v", cross, tangent2) - } - }) + for _, contact := range contacts { + local := transform.Rotation.Conjugate().Rotate(contact.Position.Add(mgl64.Vec3{0, contact.Separation / 2, 0}).Sub(transform.Position)) + if math.Abs(local.Y()+0.02) > 1e-9 { + t.Errorf("the corner %v is not on the bottom face", local) + } } } diff --git a/actor/transform.go b/actor/transform.go index 6a51f80..8c11d36 100644 --- a/actor/transform.go +++ b/actor/transform.go @@ -2,11 +2,11 @@ package actor import "github.com/go-gl/mathgl/mgl64" -// Transform represents a position in 3D space +// Transform represents a position in 3D space. +// The inverse of the rotation is its conjugate: no need to store it type Transform struct { - Position mgl64.Vec3 - Rotation mgl64.Quat - InverseRotation mgl64.Quat + Position mgl64.Vec3 + Rotation mgl64.Quat } // NewTransform creates an identity transform @@ -16,3 +16,13 @@ func NewTransform() Transform { Rotation: mgl64.QuatIdent(), } } + +// ToWorld maps a point from the local space of the transform to world space. +func (t Transform) ToWorld(local mgl64.Vec3) mgl64.Vec3 { + return t.Position.Add(t.Rotation.Rotate(local)) +} + +// ToLocal maps a point from world space to the local space of the transform. +func (t Transform) ToLocal(world mgl64.Vec3) mgl64.Vec3 { + return t.Rotation.Conjugate().Rotate(world.Sub(t.Position)) +} diff --git a/articulation.go b/articulation.go new file mode 100644 index 0000000..f290224 --- /dev/null +++ b/articulation.go @@ -0,0 +1,376 @@ +package feather + +import ( + "github.com/akmonengine/feather/actor" + "github.com/go-gl/mathgl/mgl64" +) + +// ========== ARTICULATIONS ========== +// The point constraints of the joints linking dynamic bodies are solved together, exactly, by tree of joints: the linear +// time dynamics of Baraff ("Linear-Time Dynamics using Lagrange Multipliers", SIGGRAPH 1996), the joints eliminated from +// the leaves to the root. Solved one by one, the soft spring of a joint acts on the mass of its own bodies: a heavy body +// hanging on light links stretches them (a ball 670 times heavier than a link stretched each joint by 7 cm). Solved +// together, the spring acts on the mass of the whole system (the frequency whatever the mass, of Catto's soft +// constraints, for the system): the chain holds the ball. +// +// The joints of a net (a loop between dynamic bodies) are solved one by one, as the other rows of the joints (axes, +// limits, motors, springs). A chain taut between 2 fixed points has a redundant row: the proximal term of the contacts +// (blockRegularization) keeps the system invertible, the impulses closest to the previous ones. + +// articulations: the joints of the trees, in the order of elimination, and the factorization of their mass matrix +type articulations struct { + // joints in the order of elimination, the trees one after the other; tree[i] is the first joint of the tree of i + joints []*JointBase + // later: the neighbors of each joint eliminated after it (sharing a body, or filled by the elimination), from + // start[i] to start[i+1] + start []int + later []int + // the lower blocks of the matrix below each joint, then its factor L; diag: the diagonal block D, inverse: D⁻¹ + lower []mgl64.Mat3 + diag []mgl64.Mat3 + inverse []mgl64.Mat3 + // anchors of the joints during the pass, and the right-hand side, then the solution + anchorA, anchorB []mgl64.Vec3 + vector []mgl64.Vec3 + + // buffers of build + bodyJoints [][]int // the joints of each state + position []int // the position of each joint of s.joints in the order, -1 if not articulated + visited []bool + stack [][3]int // depth first search: body, joint to its parent, next joint to visit + adjacency [][]int +} + +// buildArticulations orders the joints of the trees of dynamic bodies from the leaves to the root, and finds the fill +// of the elimination (none for a chain, the siblings of a body with several children) +func (s *solver) buildArticulations() { + a := &s.articulations + a.joints = a.joints[:0] + bodies := len(s.states) + a.bodyJoints = resizeSlices(a.bodyJoints, bodies) + a.visited = resizeBools(a.visited, bodies) + a.position = resizeInts(a.position, len(s.joints)) + for i, joint := range s.joints { + j := joint.base() + j.inArticulation = false + a.position[i] = -1 + if j.indexA >= 0 { + a.bodyJoints[j.indexA] = append(a.bodyJoints[j.indexA], i) + } + if j.indexB >= 0 { + a.bodyJoints[j.indexB] = append(a.bodyJoints[j.indexB], i) + } + } + + // ========== order: post-order of the trees of bodies ========== + // A tree starts at a body attached to a static body if any. A joint is placed once all the joints below it are: the + // joints to the static bodies of a body, then the joint to its parent + for pass := 0; pass < 2; pass++ { + for root := 0; root < bodies; root++ { + if a.visited[root] || len(a.bodyJoints[root]) == 0 || (pass == 0 && !s.attachedToStatic(root)) { + continue + } + first := len(a.joints) + loop := s.orderTree(root) + if len(a.joints)-first < 2 || loop { + // a single joint: solved alone. A net (a loop between dynamic bodies): its joints are solved one by one, + // the tree exact and the joints closing the loops alone converge slowly (a stiff subsystem against the rows + // coupled to it: a net of 60 x 60 opened by 357 mm, 320 by joint) + for i, position := range a.position { + if position >= first { + a.position[i] = -1 + s.joints[i].base().inArticulation = false + } + } + a.joints = a.joints[:first] + } + } + } + + // ========== fill ========== + n := len(a.joints) + a.adjacency = resizeSlices(a.adjacency, n) + local := a.position + for _, joints := range a.bodyJoints[:bodies] { + for _, x := range joints { + for _, y := range joints { + px, py := local[x], local[y] + if px >= 0 && py > px { + a.adjacency[px] = appendUnique(a.adjacency[px], py) + } + } + } + } + a.start = resizeInts(a.start, n+1) + a.later = a.later[:0] + for i := 0; i < n; i++ { + a.start[i] = len(a.later) + neighbors := a.adjacency[i] + a.later = append(a.later, neighbors...) + // eliminating i links all its later neighbors together + for u, x := range neighbors { + for _, y := range neighbors[u+1:] { + lo, hi := min(x, y), max(x, y) + a.adjacency[lo] = appendUnique(a.adjacency[lo], hi) + } + } + } + a.start[n] = len(a.later) + a.lower = resizeMats(a.lower, len(a.later)) + a.diag = resizeMats(a.diag, n) + a.inverse = resizeMats(a.inverse, n) + a.anchorA = resizeVecs(a.anchorA, n) + a.anchorB = resizeVecs(a.anchorB, n) + a.vector = resizeVecs(a.vector, n) + for i := range a.bodyJoints[:bodies] { + a.bodyJoints[i] = a.bodyJoints[i][:0] + a.visited[i] = false + } +} + +func (s *solver) attachedToStatic(body int) bool { + for _, i := range s.articulations.bodyJoints[body] { + if j := s.joints[i].base(); j.indexA < 0 || j.indexB < 0 { + return true + } + } + return false +} + +// orderTree: depth first from the root, each joint placed after the subtree of its child body. Returns true if a joint +// reaching a body already in the tree closes a loop +func (s *solver) orderTree(root int) bool { + loop := false + a := &s.articulations + a.stack = append(a.stack[:0], [3]int{root, -1, 0}) + a.visited[root] = true + for len(a.stack) > 0 { + top := &a.stack[len(a.stack)-1] + body, parentJoint := top[0], top[1] + joints := a.bodyJoints[body] + if top[2] < len(joints) { + i := joints[top[2]] + top[2]++ + if i == parentJoint || a.position[i] != -1 { + continue + } + j := s.joints[i].base() + other := j.indexA + if other == body { + other = j.indexB + } + switch { + case other < 0: + // attached to a static body: a leaf + s.place(i) + case !a.visited[other]: + a.visited[other] = true + a.stack = append(a.stack, [3]int{other, i, 0}) + default: + loop = true + a.position[i] = -2 + } + continue + } + a.stack = a.stack[:len(a.stack)-1] + if parentJoint >= 0 { + s.place(parentJoint) + } + } + for i := range a.position { + if a.position[i] == -2 { + a.position[i] = -1 + } + } + return loop +} + +func (s *solver) place(i int) { + a := &s.articulations + j := s.joints[i].base() + a.position[i] = len(a.joints) + a.joints = append(a.joints, j) + j.inArticulation = true +} + +// solveArticulations: for each tree, K Δλ = -(ċ + bias), K = J M⁻¹ Jᵀ the mass matrix of the point constraints of all +// its joints, factored from the leaves (block LDLᵀ). The soft spring acts on the whole system: +// Δλ = -(K⁻¹ (ċ + bias) + γ λ) / (1 + γ) +func (s *solver) solveArticulations(useBias bool) { + a := &s.articulations + n := len(a.joints) + if n == 0 { + return + } + for i, j := range a.joints { + stateA, stateB := s.state(j.indexA), s.state(j.indexB) + rA, rB := j.currentAnchors(stateA, stateB) + a.anchorA[i], a.anchorB[i] = rA, rB + cdot := relativeVelocity(stateA, stateB, rA, rB) + if useBias { + separation := stateB.deltaPosition.Sub(stateA.deltaPosition).Add(rB.Sub(rA)).Add(j.deltaCenter) + cdot = cdot.Add(separation.Mul(j.spring.biasRate)) + } + a.vector[i] = cdot + a.diag[i] = s.coupling(j, j, rA, rB, rA, rB) + } + for i, j := range a.joints { + for t := a.start[i]; t < a.start[i+1]; t++ { + k := a.later[t] + a.lower[t] = s.coupling(a.joints[k], j, a.anchorA[k], a.anchorB[k], a.anchorA[i], a.anchorB[i]) + } + // the proximal term + for c := 0; c < 3; c++ { + a.diag[i].Set(c, c, a.diag[i].At(c, c)*(1+blockRegularization)) + } + } + + // ========== factor: A = L D Lᵀ, from the leaves ========== + for i := 0; i < n; i++ { + inverse := actor.Inv3(&a.diag[i]) + a.inverse[i] = inverse + for t := a.start[i]; t < a.start[i+1]; t++ { + // update the later blocks with -A_ki D⁻¹ A_li + k := a.later[t] + transposed := actor.Transpose3(&a.lower[t]) + for u := a.start[i]; u < a.start[i+1]; u++ { + l := a.later[u] + if l < k { + continue + } + partial := actor.Mul3(&a.lower[u], &inverse) + update := actor.Mul3(&partial, &transposed) + if l == k { + a.diag[k] = actor.Sub3(&a.diag[k], &update) + } else { + block := &a.lower[a.find(k, l)] + *block = actor.Sub3(block, &update) + } + } + } + for t := a.start[i]; t < a.start[i+1]; t++ { + a.lower[t] = actor.Mul3(&a.lower[t], &inverse) + } + } + + // ========== solve ========== + for i := 0; i < n; i++ { + for t := a.start[i]; t < a.start[i+1]; t++ { + k := a.later[t] + a.vector[k] = a.vector[k].Sub(actor.MulMat3(&a.lower[t], a.vector[i])) + } + } + for i := 0; i < n; i++ { + a.vector[i] = actor.MulMat3(&a.inverse[i], a.vector[i]) + } + for i := n - 1; i >= 0; i-- { + for t := a.start[i]; t < a.start[i+1]; t++ { + transposed := actor.Transpose3(&a.lower[t]) + a.vector[i] = a.vector[i].Sub(actor.MulMat3(&transposed, a.vector[a.later[t]])) + } + } + + // ========== impulses ========== + for i, j := range a.joints { + row := rigid + if useBias { + row = j.spring + } + impulse := a.vector[i].Add(j.linearImpulse.Mul(row.gamma)).Mul(-1 / (1 + row.gamma)) + j.linearImpulse = j.linearImpulse.Add(impulse) + applyLinear(s.state(j.indexA), s.state(j.indexB), a.anchorA[i], a.anchorB[i], impulse) + } +} + +// find the block (row l, column k) below k +func (a *articulations) find(k, l int) int { + for t := a.start[k]; t < a.start[k+1]; t++ { + if a.later[t] == l { + return t + } + } + panic("feather: articulation fill missing") +} + +// coupling: the block of K between the point constraints of the joints x (row) and y (column), through their shared +// dynamic bodies: s_x s_y (m⁻¹ I - [r_x]× I⁻¹ [r_y]×), s = -1 on A, +1 on B +func (s *solver) coupling(x, y *JointBase, rAx, rBx, rAy, rBy mgl64.Vec3) mgl64.Mat3 { + var block mgl64.Mat3 + add := func(body int, signX float64, rX mgl64.Vec3, signY float64, rY mgl64.Vec3) { + state := s.state(body) + m := state.invMass + identity := mgl64.Mat3{1 * m, 0 * m, 0 * m, 0 * m, 1 * m, 0 * m, 0 * m, 0 * m, 1 * m} + angular := skewTerm(&state.inverseInertia, rX, rY) + term := actor.Sub3(&identity, &angular) + sign := signX * signY + for c := range term { + block[c] += term[c] * sign + } + } + if x.indexA >= 0 && x.indexA == y.indexA { + add(x.indexA, -1, rAx, -1, rAy) + } + if x.indexA >= 0 && x.indexA == y.indexB { + add(x.indexA, -1, rAx, 1, rBy) + } + if x.indexB >= 0 && x.indexB == y.indexA { + add(x.indexB, 1, rBx, -1, rAy) + } + if x.indexB >= 0 && x.indexB == y.indexB { + add(x.indexB, 1, rBx, 1, rBy) + } + return block +} + +// ========== buffers, reused from a step to the next ========== + +func appendUnique(list []int, value int) []int { + for _, v := range list { + if v == value { + return list + } + } + return append(list, value) +} + +func resizeInts(s []int, n int) []int { + if cap(s) < n { + return make([]int, n, 2*n) + } + return s[:n] +} + +func resizeBools(s []bool, n int) []bool { + if cap(s) < n { + return make([]bool, n, 2*n) + } + s = s[:n] + clear(s) + return s +} + +func resizeMats(s []mgl64.Mat3, n int) []mgl64.Mat3 { + if cap(s) < n { + return make([]mgl64.Mat3, n, 2*n) + } + return s[:n] +} + +func resizeVecs(s []mgl64.Vec3, n int) []mgl64.Vec3 { + if cap(s) < n { + return make([]mgl64.Vec3, n, 2*n) + } + return s[:n] +} + +// resizeSlices: n empty slices, keeping the capacity of the ones already there +func resizeSlices(s [][]int, n int) [][]int { + for len(s) < n { + s = append(s, nil) + } + s = s[:n] + for i := range s { + s[i] = s[i][:0] + } + return s +} diff --git a/bench/accuracy.go b/bench/accuracy.go new file mode 100644 index 0000000..d306dca --- /dev/null +++ b/bench/accuracy.go @@ -0,0 +1,227 @@ +package main + +import ( + "fmt" + "math" + "math/rand" + "sort" + + "github.com/akmonengine/feather/actor" + "github.com/go-gl/mathgl/mgl64" +) + +// h is the support function of the body: max over its points of p·n. +func h(b *actor.RigidBody, n mgl64.Vec3) float64 { return b.SupportWorld(n).Dot(n) } + +// depthAlong is how far B must move along n (A→B) to separate. +func depthAlong(a, b *actor.RigidBody, n mgl64.Vec3) float64 { return h(a, n) + h(b, n.Mul(-1)) } + +// reference estimates the penetration (minimum over all directions) and its direction, +// by dense sampling then local refinement, for pairs without an exact formula: the +// sampling can miss the minimum, which is why exact() is preferred wherever it exists. +func reference(a, b *actor.RigidBody, seeds []mgl64.Vec3) (float64, mgl64.Vec3) { + type cand struct { + d float64 + n mgl64.Vec3 + } + var cs []cand + for _, n := range seeds { + cs = append(cs, cand{depthAlong(a, b, n), n}) + } + sort.Slice(cs, func(i, j int) bool { return cs[i].d < cs[j].d }) + best, bn := math.Inf(1), mgl64.Vec3{} + for k := 0; k < len(cs) && k < 4; k++ { + if d, n := refine(a, b, cs[k].n); d < best { + best, bn = d, n + } + } + return best, bn +} + +func refine(a, b *actor.RigidBody, bn mgl64.Vec3) (float64, mgl64.Vec3) { + best := depthAlong(a, b, bn) + step := 0.05 + for step > 1e-10 { + improved := false + t1 := bn.Cross(mgl64.Vec3{1, 0, 0}) + if t1.Len() < 0.5 { + t1 = bn.Cross(mgl64.Vec3{0, 1, 0}) + } + t1 = t1.Normalize() + t2 := bn.Cross(t1) + for _, dir := range []mgl64.Vec3{t1, t1.Mul(-1), t2, t2.Mul(-1), t1.Add(t2).Normalize(), t1.Sub(t2).Normalize(), t2.Sub(t1).Normalize(), t1.Add(t2).Mul(-1).Normalize()} { + n := bn.Add(dir.Mul(step)).Normalize() + if d := depthAlong(a, b, n); d < best { + best, bn, improved = d, n, true + } + } + if !improved { + step /= 2 + } + } + return best, bn +} + +func fibonacci(n int) []mgl64.Vec3 { + var out []mgl64.Vec3 + ga := math.Pi * (3 - math.Sqrt(5)) + for i := 0; i < n; i++ { + y := 1 - 2*(float64(i)+0.5)/float64(n) + r := math.Sqrt(1 - y*y) + out = append(out, mgl64.Vec3{math.Cos(ga*float64(i)) * r, y, math.Sin(ga*float64(i)) * r}) + } + return out +} + +func randQuat(r *rand.Rand) mgl64.Quat { + u1, u2, u3 := r.Float64(), r.Float64(), r.Float64() + return mgl64.Quat{W: math.Sqrt(1-u1) * math.Sin(2*math.Pi*u2), V: mgl64.Vec3{math.Sqrt(1-u1) * math.Cos(2*math.Pi*u2), math.Sqrt(u1) * math.Sin(2*math.Pi*u3), math.Sqrt(u1) * math.Cos(2*math.Pi*u3)}}.Normalize() +} + +type shapeMaker func(r *rand.Rand) actor.ShapeInterface + +func boxMaker(r *rand.Rand) actor.ShapeInterface { + return &actor.Box{HalfExtents: mgl64.Vec3{0.1 + 0.5*r.Float64(), 0.1 + 0.5*r.Float64(), 0.1 + 0.5*r.Float64()}} +} +func sphereMaker(r *rand.Rand) actor.ShapeInterface { + return &actor.Sphere{Radius: 0.1 + 0.5*r.Float64()} +} + +func pct(v []float64, p float64) float64 { + if len(v) == 0 { + return math.NaN() + } + s := append([]float64(nil), v...) + sort.Float64s(s) + return s[int(math.Min(float64(len(s)-1), p*float64(len(s))))] +} + +// exact returns the exact penetration depth and normal (A→B) when a closed form exists: +// SAT over the 15 axes for two boxes, the closest point for a sphere and a box. +func exact(a, b *actor.RigidBody) (float64, mgl64.Vec3, bool) { + ba, okA := a.Shape.(*actor.Box) + bb, okB := b.Shape.(*actor.Box) + if okA && okB { + axesA := [3]mgl64.Vec3{a.Transform.Rotation.Rotate(mgl64.Vec3{1, 0, 0}), a.Transform.Rotation.Rotate(mgl64.Vec3{0, 1, 0}), a.Transform.Rotation.Rotate(mgl64.Vec3{0, 0, 1})} + axesB := [3]mgl64.Vec3{b.Transform.Rotation.Rotate(mgl64.Vec3{1, 0, 0}), b.Transform.Rotation.Rotate(mgl64.Vec3{0, 1, 0}), b.Transform.Rotation.Rotate(mgl64.Vec3{0, 0, 1})} + cands := append([]mgl64.Vec3{}, axesA[:]...) + cands = append(cands, axesB[:]...) + for _, x := range axesA { + for _, y := range axesB { + if c := x.Cross(y); c.Len() > 1e-9 { + cands = append(cands, c.Normalize()) + } + } + } + _ = ba + _ = bb + best, bn := math.Inf(1), mgl64.Vec3{} + for _, n := range cands { + for _, s := range []float64{1, -1} { + m := n.Mul(s) + if d := depthAlong(a, b, m); d < best { + best, bn = d, m + } + } + } + return best, bn, true + } + if d, n, ok := exactCapsuleBox(a, b); ok { + return d, n, true + } + sa, okS := a.Shape.(*actor.Sphere) + bx, okX := b.Shape.(*actor.Box) + flip := false + if !okS || !okX { + sb, okS2 := b.Shape.(*actor.Sphere) + ax, okX2 := a.Shape.(*actor.Box) + if !okS2 || !okX2 { + return 0, mgl64.Vec3{}, false + } + sa, bx, flip = sb, ax, true + a, b = b, a + } + // a: sphere, b: box + c := b.Transform.Rotation.Conjugate().Rotate(a.Transform.Position.Sub(b.Transform.Position)) + h := bx.HalfExtents + q := mgl64.Vec3{math.Max(-h[0], math.Min(h[0], c[0])), math.Max(-h[1], math.Min(h[1], c[1])), math.Max(-h[2], math.Min(h[2], c[2]))} + var depth float64 + var nLocal mgl64.Vec3 // from box to sphere + if q != c { + d := c.Sub(q) + depth = sa.Radius - d.Len() + nLocal = d.Normalize() + } else { + best := math.Inf(1) + for i := 0; i < 3; i++ { + for _, s := range []float64{1, -1} { + dist := h[i] - s*c[i] + if dist < best { + best = dist + nLocal = mgl64.Vec3{} + nLocal[i] = s + } + } + } + depth = sa.Radius + best + } + n := b.Transform.Rotation.Rotate(nLocal) // box → sphere + if !flip { + n = n.Mul(-1) // sphere is A: A→B = sphere → box + } + return depth, n, true +} + +// accuracy places pairs at a chosen reference depth and compares the narrow phase. +func accuracy(name string, ma, mb shapeMaker, minDepth, maxDepth float64, n int) { + r := rand.New(rand.NewSource(7)) + seeds := fibonacci(1500) + var angErr, depErr []float64 + missed, wrongSide := 0, 0 + for i := 0; i < n; i++ { + sa, sb := ma(r), mb(r) + qa, qb := randQuat(r), randQuat(r) + dir := mgl64.Vec3{r.NormFloat64(), r.NormFloat64(), r.NormFloat64()}.Normalize() + target := minDepth + (maxDepth-minDepth)*r.Float64() + a := actor.NewRigidBody(tr(mgl64.Vec3{}, qa), sa, actor.BodyTypeDynamic, 1) + b := actor.NewRigidBody(tr(mgl64.Vec3{}, qb), sb, actor.BodyTypeDynamic, 1) + b.Transform.Position = dir.Mul(0.3) + d0, n0 := reference(a, b, seeds[:500]) + b.Transform.Position = b.Transform.Position.Add(n0.Mul(d0 - target)) + a.Shape.ComputeAABB(a.Transform) + b.Shape.ComputeAABB(b.Transform) + want, wn := reference(a, b, seeds) + if d, n, ok := exact(a, b); ok { + want, wn = d, n + } + if want <= 0 { + continue + } + ok, got, depth, _ := narrow(a, b) + if !ok { + missed++ + continue + } + ang := math.Acos(math.Max(-1, math.Min(1, got.Dot(wn)))) * 180 / math.Pi + if ang > 90 { + wrongSide++ + } + angErr = append(angErr, ang) + depErr = append(depErr, math.Abs(depth-want)*1000) + } + fmt.Printf("%-12s depth %4.1f-%4.0f mm n=%d missed=%d wrongSide=%d normal err° median %.3f p99 %.2f max %.1f | depth err mm median %.3f p99 %.2f max %.1f\n", + name, minDepth*1000, maxDepth*1000, n, missed, wrongSide, pct(angErr, 0.5), pct(angErr, 0.99), pct(angErr, 1), pct(depErr, 0.5), pct(depErr, 0.99), pct(depErr, 1)) +} + +func epaAccuracy() { + accuracy("box-box", boxMaker, boxMaker, 0.0001, 0.02, 200) + accuracy("box-box", boxMaker, boxMaker, 0.02, 0.2, 200) + accuracy("sphere-box", sphereMaker, boxMaker, 0.0001, 0.02, 200) + accuracy("box-sphere", boxMaker, sphereMaker, 0.0001, 0.02, 200) + accuracy("sphere-sphr", sphereMaker, sphereMaker, 0.0001, 0.02, 100) + if capsuleMaker != nil { + accuracy("capsule-box", capsuleMaker, boxMaker, 0.0001, 0.02, 200) + accuracy("box-capsule", boxMaker, capsuleMaker, 0.0001, 0.02, 200) + accuracy("capsule-box", capsuleMaker, boxMaker, 0.02, 0.2, 200) + } +} diff --git a/bench/baseline.json b/bench/baseline.json new file mode 100644 index 0000000..6ad530e --- /dev/null +++ b/bench/baseline.json @@ -0,0 +1,717 @@ +{ + "arch": "linux/amd64", + "machine": "AMD Ryzen 7 5800X 8-Core Processor, 16 CPUs", + "scenes": { + "joint chain": { + "fingerprint": "d1a5ea3300908477", + "quality": { + "energy gain": { + "value": 0, + "unit": "%" + }, + "worst stretch": { + "value": 14.543678411172328, + "unit": "mm" + } + }, + "stepMs": 0.328752724, + "phasesMs": { + "broad phase": 0.020677631999999998, + "continuous": 0.0071531039999999995, + "islands": 0.00039, + "narrow phase": 0.002524448, + "prepare": 0.007260608, + "restitution": 0.002520576, + "substeps": 0.288045396 + } + }, + "pile of 500": { + "fingerprint": "d77ecceae0f7eea3", + "quality": { + "landing depth": { + "value": 0.6667135029494731, + "unit": "mm" + } + }, + "stepMs": 0.8717517, + "phasesMs": { + "broad phase": 0.10925642666666666, + "continuous": 0.006046853333333333, + "islands": 0.0031442933333333334, + "narrow phase": 0.19552514666666668, + "prepare": 0.06466241333333334, + "restitution": 0.022133499999999997, + "substeps": 0.4699285266666666 + } + }, + "pyramid": { + "fingerprint": "8e5af8ae9965f091", + "quality": { + "worst drift": { + "value": 15.06227698815896, + "unit": "mm" + } + }, + "stepMs": 0.07744509399999999, + "phasesMs": { + "broad phase": 0.004263816, + "continuous": 0.000223382, + "islands": 0.000181226, + "narrow phase": 0.01077844, + "prepare": 0.005719314, + "restitution": 0.001080206, + "substeps": 0.05503125 + } + }, + "rain on terrain": { + "fingerprint": "1b2a75beae934de5", + "quality": { + "fell through": { + "value": 0, + "unit": "" + }, + "landing depth": { + "value": 9.743546541685353, + "unit": "mm" + } + }, + "stepMs": 11.176997512, + "phasesMs": { + "broad phase": 0.11957176800000001, + "continuous": 4.397838452, + "islands": 0.0061450639999999996, + "narrow phase": 3.8915379320000003, + "prepare": 0.314780984, + "restitution": 0.046029851999999996, + "substeps": 2.398003156 + } + }, + "slope pile": { + "fingerprint": "e807a7a4a0748f09", + "quality": { + "landing depth": { + "value": 1.517463711187439, + "unit": "mm" + }, + "resting depth": { + "value": 0.22507290545181746, + "unit": "mm" + } + }, + "stepMs": 0.7030997449999999, + "phasesMs": { + "broad phase": 0.084426155, + "continuous": 0.030294555, + "islands": 0.00084816, + "narrow phase": 0.07793892, + "prepare": 0.03624791, + "restitution": 0.010420575000000001, + "substeps": 0.46253020999999994 + } + }, + "solver2d ball and chain": { + "fingerprint": "5495df4bcca4c3ab", + "quality": { + "finite": { + "value": 1, + "unit": "" + }, + "worst gap": { + "value": 0.5545802126429944, + "unit": "mm" + } + }, + "stepMs": 0.332302912, + "phasesMs": { + "broad phase": 0.022754055999999998, + "continuous": 0.00524216, + "islands": 0.000386164, + "narrow phase": 0.001125564, + "prepare": 0.008728556, + "restitution": 0.00268974, + "substeps": 0.291197784 + } + }, + "solver2d bridge": { + "fingerprint": "dd9b5224748922e2", + "quality": { + "finite": { + "value": 1, + "unit": "" + }, + "sag": { + "value": 0.25237870005618745, + "unit": "m" + }, + "worst gap": { + "value": 0.23602681912618909, + "unit": "mm" + } + }, + "stepMs": 0.5801432160000001, + "phasesMs": { + "broad phase": 0.004460856, + "continuous": 0.001942376, + "islands": 0.000806564, + "narrow phase": 0.001015528, + "prepare": 0.015942788, + "restitution": 0.009394512, + "substeps": 0.546367072 + } + }, + "solver2d card house": { + "fingerprint": "4fbd38eef3730b31", + "quality": { + "levels": { + "value": 3, + "unit": "" + }, + "worst drift": { + "value": 0.05576265490251589, + "unit": "mm" + } + }, + "stepMs": 0.08392965714285715, + "phasesMs": { + "broad phase": 0.0016234914285714287, + "continuous": 0.0006242857142857143, + "islands": 0.00016634285714285714, + "narrow phase": 0.02821218285714286, + "prepare": 0.005434262857142857, + "restitution": 0.0008903542857142858, + "substeps": 0.046826508571428574 + } + }, + "solver2d centered impact": { + "fingerprint": "ccfa947ac592a6a4", + "quality": { + "bounce": { + "value": 1.7398077180601144, + "unit": "m/s" + }, + "lateral drift": { + "value": 0.0007475795228636179, + "unit": "mm" + } + }, + "stepMs": 0.006930726666666667, + "phasesMs": { + "broad phase": 0.00024186666666666667, + "continuous": 0.00010059999999999999, + "islands": 0.00007826666666666667, + "narrow phase": 0.0008808133333333333, + "prepare": 0.00075174, + "restitution": 0.000316, + "substeps": 0.00441984 + } + }, + "solver2d circle stack": { + "fingerprint": "0a3f1b475a777524", + "quality": { + "horizontal drift": { + "value": 0, + "unit": "mm" + }, + "top height error": { + "value": 7.103344548649915, + "unit": "mm" + } + }, + "stepMs": 0.004133155, + "phasesMs": { + "broad phase": 0.0007682750000000001, + "continuous": 0.0001083275, + "islands": 0.00007675000000000001, + "narrow phase": 0.000311125, + "prepare": 0.0005085325000000001, + "restitution": 0.00018435, + "substeps": 0.00205999 + } + }, + "solver2d confined": { + "fingerprint": "421bb7b06f13eb23", + "quality": { + "escaped": { + "value": 0, + "unit": "" + }, + "max speed": { + "value": 5.695985977206491, + "unit": "m/s" + }, + "row": { + "value": 6, + "unit": "" + } + }, + "stepMs": 0.482003888, + "phasesMs": { + "broad phase": 0.04847136, + "continuous": 0.00294756, + "islands": 0.001493372, + "narrow phase": 0.103083644, + "prepare": 0.074621552, + "restitution": 0.008087508, + "substeps": 0.24247100800000002 + } + }, + "solver2d double domino": { + "fingerprint": "130e3ab1df7a73eb", + "quality": { + "fallen": { + "value": 15, + "unit": "" + }, + "time of the last": { + "value": 6.016666666666651, + "unit": "s" + } + }, + "stepMs": 0.11528807875, + "phasesMs": { + "broad phase": 0.00183274875, + "continuous": 0.00070396875, + "islands": 0.00020990124999999997, + "narrow phase": 0.0206642175, + "prepare": 0.00883972875, + "restitution": 0.0019020525, + "substeps": 0.08095218125 + } + }, + "solver2d far chain": { + "fingerprint": "7569d5ae36a9d526", + "quality": { + "far deviation": { + "value": 1.0331229789970546e-7, + "unit": "mm" + }, + "finite": { + "value": 1, + "unit": "" + }, + "worst gap": { + "value": 25.937047921567068, + "unit": "mm" + } + }, + "stepMs": 0.6462906420000001, + "phasesMs": { + "broad phase": 0.051685273999999996, + "continuous": 0.017276807999999998, + "islands": 0.000647306, + "narrow phase": 0.010573994000000001, + "prepare": 0.014943628, + "restitution": 0.004993612, + "substeps": 0.545955958 + } + }, + "solver2d far pyramid": { + "fingerprint": "9a28b30be3a8991b", + "quality": { + "far deviation": { + "value": 8.907780632216337, + "unit": "mm" + }, + "worst drift": { + "value": 0, + "unit": "mm" + } + }, + "stepMs": 2.8456944539999998, + "phasesMs": { + "broad phase": 0.087139968, + "continuous": 0.00620242, + "islands": 0.003377798, + "narrow phase": 0.5585856760000001, + "prepare": 0.22202198, + "restitution": 0.037133066, + "substeps": 1.9297350340000001 + } + }, + "solver2d far recovery": { + "fingerprint": "998f0b0d592fb07d", + "quality": { + "far deviation": { + "value": 0.00036292906023460486, + "unit": "mm" + }, + "max speed": { + "value": 3.5716880471398262, + "unit": "m/s" + } + }, + "stepMs": 0.5423482933333333, + "phasesMs": { + "broad phase": 0.006252366666666667, + "continuous": 0.0008281133333333333, + "islands": 0.0005372066666666667, + "narrow phase": 0.16870949666666665, + "prepare": 0.03308945666666667, + "restitution": 0.004388276666666667, + "substeps": 0.32827197333333336 + } + }, + "solver2d far stack": { + "fingerprint": "9bdfc2ae4956f529", + "quality": { + "far deviation": { + "value": 0.11290130794691286, + "unit": "mm" + }, + "worst drift": { + "value": 0, + "unit": "mm" + } + }, + "stepMs": 0.018295106000000002, + "phasesMs": { + "broad phase": 0.000473244, + "continuous": 0.00014394, + "islands": 0.00010472, + "narrow phase": 0.003934344, + "prepare": 0.0016116939999999999, + "restitution": 0.000529246, + "substeps": 0.011373557999999999 + } + }, + "solver2d friction ramp": { + "fingerprint": "e49861060e0a7f62", + "quality": { + "sliding error": { + "value": 6.0675788535160535, + "unit": "mm" + }, + "stopped slide": { + "value": 0, + "unit": "mm" + } + }, + "stepMs": 0.028031592, + "phasesMs": { + "broad phase": 0.001023616, + "continuous": 0.00017544000000000001, + "islands": 0.00010688, + "narrow phase": 0.011025952, + "prepare": 0.002257776, + "restitution": 0.00071656, + "substeps": 0.012594568 + } + }, + "solver2d high mass ratio 1": { + "fingerprint": "e7874f6ac25fd537", + "quality": { + "heavy cube sag": { + "value": 127.99081789071032, + "unit": "mm" + }, + "layers": { + "value": 4, + "unit": "" + }, + "worst drift": { + "value": 105.79202475015144, + "unit": "mm" + } + }, + "stepMs": 1.698726344, + "phasesMs": { + "broad phase": 0.01593834, + "continuous": 0.002268616, + "islands": 0.00188574, + "narrow phase": 0.562861172, + "prepare": 0.11869384399999999, + "restitution": 0.014986652, + "substeps": 0.981310976 + } + }, + "solver2d high mass ratio 2": { + "fingerprint": "b0c580b92f03cacf", + "quality": { + "bounce": { + "value": 2.2252620543727213, + "unit": "m/s" + }, + "slab sag": { + "value": 67.77438017502391, + "unit": "mm" + }, + "small cubes drift": { + "value": 21.56214604243897, + "unit": "mm" + }, + "small cubes into ground": { + "value": 577.2579911571269, + "unit": "mm" + } + }, + "stepMs": 0.017935904, + "phasesMs": { + "broad phase": 0.00041252, + "continuous": 0.00019628, + "islands": 0.0000992, + "narrow phase": 0.005752452, + "prepare": 0.001354568, + "restitution": 0.000573644, + "substeps": 0.009424 + } + }, + "solver2d high mass ratio 3": { + "fingerprint": "162be214ceca1077", + "quality": { + "bounce": { + "value": 2.2253312335268416, + "unit": "m/s" + }, + "slab sag": { + "value": 67.77361632275891, + "unit": "mm" + }, + "small cubes drift": { + "value": 21.520070758926067, + "unit": "mm" + }, + "small cubes into ground": { + "value": 577.2579914400629, + "unit": "mm" + } + }, + "stepMs": 0.018962556, + "phasesMs": { + "broad phase": 0.00039088, + "continuous": 0.00017580000000000002, + "islands": 0.00009392399999999999, + "narrow phase": 0.006965624, + "prepare": 0.001326616, + "restitution": 0.0005552399999999999, + "substeps": 0.009322272 + } + }, + "solver2d joint grid": { + "fingerprint": "3bf73d7661991d94", + "quality": { + "finite": { + "value": 1, + "unit": "" + }, + "max speed": { + "value": 30.115023171399358, + "unit": "m/s" + }, + "worst gap": { + "value": 88.35962353634609, + "unit": "mm" + } + }, + "stepMs": 4.44351414, + "phasesMs": { + "broad phase": 0.8097767066666667, + "continuous": 0.06023162000000001, + "islands": 0.00831516, + "narrow phase": 0.2033266, + "prepare": 0.25451162, + "restitution": 0.0297918, + "substeps": 3.07573754 + } + }, + "solver2d overlap recovery": { + "fingerprint": "5bbb7588f8e57051", + "quality": { + "final overlap": { + "value": 3.321096845759746, + "unit": "mm" + }, + "layers": { + "value": 4, + "unit": "" + }, + "max speed": { + "value": 3.5716880409799106, + "unit": "m/s" + } + }, + "stepMs": 0.31934376400000003, + "phasesMs": { + "broad phase": 0.004773896, + "continuous": 0.000481972, + "islands": 0.00036328, + "narrow phase": 0.101049792, + "prepare": 0.019754956, + "restitution": 0.002655388, + "substeps": 0.19003628 + } + }, + "solver2d pyramid": { + "fingerprint": "6966ec7612fd5700", + "quality": { + "layers": { + "value": 8, + "unit": "" + }, + "worst drift": { + "value": 1.45745153162969, + "unit": "mm" + } + }, + "stepMs": 1.170504028, + "phasesMs": { + "broad phase": 0.024777076000000002, + "continuous": 0.001236692, + "islands": 0.0012278880000000001, + "narrow phase": 0.371713392, + "prepare": 0.090282104, + "restitution": 0.008843400000000001, + "substeps": 0.6716893119999999 + } + }, + "solver2d rush": { + "fingerprint": "13c44583001f28b6", + "quality": { + "final overlap": { + "value": 2.386494028450481, + "unit": "mm" + }, + "final speed": { + "value": 0.020914631998823915, + "unit": "m/s" + }, + "max speed": { + "value": 12.987043356298601, + "unit": "m/s" + } + }, + "stepMs": 0.066552784, + "phasesMs": { + "broad phase": 0.017071128, + "continuous": 0.00398104, + "islands": 0.0008597240000000001, + "narrow phase": 0.00011212, + "prepare": 0.007568648, + "restitution": 0.006720804, + "substeps": 0.03012368 + } + }, + "solver2d single box": { + "fingerprint": "7ac48fd94a2b753c", + "quality": { + "height error": { + "value": 0.06902504482564353, + "unit": "mm" + }, + "rest drift": { + "value": 0.00010860480059715627, + "unit": "mm" + } + }, + "stepMs": 0.004723846666666667, + "phasesMs": { + "broad phase": 0.00027573333333333333, + "continuous": 0.00014033333333333332, + "islands": 0.0001222, + "narrow phase": 0.00025140666666666667, + "prepare": 0.0004670066666666667, + "restitution": 0.00026307333333333333, + "substeps": 0.0030406266666666666 + } + }, + "solver2d stretched chain": { + "fingerprint": "f6e92f3665ce4bd4", + "quality": { + "final gap": { + "value": 0.07178605861213327, + "unit": "mm" + }, + "finite": { + "value": 1, + "unit": "" + }, + "max speed": { + "value": 254.50238414789507, + "unit": "m/s" + }, + "rest gap": { + "value": 0.07178605861213327, + "unit": "mm" + } + }, + "stepMs": 0.043703542, + "phasesMs": { + "broad phase": 0.003121604, + "continuous": 0.00040764, + "islands": 0.00017752000000000002, + "narrow phase": 0.000200042, + "prepare": 0.001950956, + "restitution": 0.00043378, + "substeps": 0.037176274 + } + }, + "solver2d vertical stack": { + "fingerprint": "51fef4fd3c94bf8e", + "quality": { + "horizontal drift": { + "value": 2.845671443000555, + "unit": "mm" + }, + "layers": { + "value": 10, + "unit": "" + } + }, + "stepMs": 0.022261464, + "phasesMs": { + "broad phase": 0.000911124, + "continuous": 0.0001988, + "islands": 0.00012328, + "narrow phase": 0.0049292, + "prepare": 0.00218402, + "restitution": 0.000839008, + "substeps": 0.012949312000000001 + } + }, + "solver2d warm start energy": { + "fingerprint": "8dcfb49e8c691bf3", + "quality": { + "overshoot": { + "value": 0, + "unit": "mm" + } + }, + "stepMs": 0.0036926679999999996, + "phasesMs": { + "broad phase": 0.00024568, + "continuous": 0.000091444, + "islands": 0.00007408, + "narrow phase": 0.00032404, + "prepare": 0.00057076, + "restitution": 0.000163084, + "substeps": 0.002104612 + } + }, + "terrain piles": { + "fingerprint": "4a6b6d3833b795ba", + "quality": { + "median landing depth": { + "value": 7.875508690151446, + "unit": "mm" + }, + "worst landing depth": { + "value": 17.85679629331417, + "unit": "mm" + }, + "worst resting depth": { + "value": 7.342632951171865, + "unit": "mm" + } + }, + "stepMs": 3.997912273, + "phasesMs": { + "broad phase": 0.031085574, + "continuous": 1.954000463, + "islands": 0.0015134764999999999, + "narrow phase": 1.0432447784999999, + "prepare": 0.089018852, + "restitution": 0.013083258, + "substeps": 0.865058801 + } + } + } +} diff --git a/bench/capsule_current.go b/bench/capsule_current.go new file mode 100644 index 0000000..518c42a --- /dev/null +++ b/bench/capsule_current.go @@ -0,0 +1,76 @@ +//go:build !v020 + +package main + +import ( + "math" + + "github.com/akmonengine/feather/actor" + "github.com/go-gl/mathgl/mgl64" +) + +// exactCapsuleBox: PD(capsule, box) = r + PD(segment, box) when the segment enters the box +// (SAT over the box normals and segment × box axes, exact for these polytopes); otherwise +// r - distance(segment, box), a convex problem solved by local refinement. +func exactCapsuleBox(a, b *actor.RigidBody) (float64, mgl64.Vec3, bool) { + flip := false + c, okC := a.Shape.(*actor.Capsule) + _, okB := b.Shape.(*actor.Box) + if !okC || !okB { + c2, okC2 := b.Shape.(*actor.Capsule) + _, okB2 := a.Shape.(*actor.Box) + if !okC2 || !okB2 { + return 0, mgl64.Vec3{}, false + } + c, flip = c2, true + a, b = b, a + } + // a: capsule, b: box. Segment support: h_seg(n) = h_capsule(n) - r. + axes := [3]mgl64.Vec3{b.Transform.Rotation.Rotate(mgl64.Vec3{1, 0, 0}), b.Transform.Rotation.Rotate(mgl64.Vec3{0, 1, 0}), b.Transform.Rotation.Rotate(mgl64.Vec3{0, 0, 1})} + d := a.Transform.Rotation.Rotate(mgl64.Vec3{0, 1, 0}) + cands := axes[:] + for _, x := range axes { + if cr := d.Cross(x); cr.Len() > 1e-9 { + cands = append(cands, cr.Normalize()) + } + } + segDepth := func(n mgl64.Vec3) float64 { return depthAlong(a, b, n) - c.Radius } + best, bn := math.Inf(1), mgl64.Vec3{} + for _, n := range cands { + for _, s := range []float64{1, -1} { + if v := segDepth(n.Mul(s)); v < best { + best, bn = v, n.Mul(s) + } + } + } + if best < 0 { + // Segment outside the box: exact distance, minimised along the segment (convex). + box := b.Shape.(*actor.Box) + p0 := a.Transform.Position.Sub(d.Mul(c.HalfHeight)) + p1 := a.Transform.Position.Add(d.Mul(c.HalfHeight)) + closest := func(t float64) (mgl64.Vec3, mgl64.Vec3) { + p := p0.Add(p1.Sub(p0).Mul(t)) + l := b.Transform.Rotation.Conjugate().Rotate(p.Sub(b.Transform.Position)) + h := box.HalfExtents + q := mgl64.Vec3{math.Max(-h[0], math.Min(h[0], l[0])), math.Max(-h[1], math.Min(h[1], l[1])), math.Max(-h[2], math.Min(h[2], l[2]))} + return p, b.Transform.Position.Add(b.Transform.Rotation.Rotate(q)) + } + dist := func(t float64) float64 { p, q := closest(t); return p.Sub(q).Len() } + lo, hi := 0.0, 1.0 + for k := 0; k < 200; k++ { + m1, m2 := lo+(hi-lo)/3, hi-(hi-lo)/3 + if dist(m1) < dist(m2) { + hi = m2 + } else { + lo = m1 + } + } + p, q := closest((lo + hi) / 2) + best, bn = -p.Sub(q).Len(), q.Sub(p).Normalize() + } + depth := best + c.Radius + if flip { + bn = bn.Mul(-1) + } + return depth, bn, true +} diff --git a/bench/capsule_v020.go b/bench/capsule_v020.go new file mode 100644 index 0000000..31ffa01 --- /dev/null +++ b/bench/capsule_v020.go @@ -0,0 +1,12 @@ +//go:build v020 + +package main + +import ( + "github.com/akmonengine/feather/actor" + "github.com/go-gl/mathgl/mgl64" +) + +func exactCapsuleBox(a, b *actor.RigidBody) (float64, mgl64.Vec3, bool) { + return 0, mgl64.Vec3{}, false +} diff --git a/bench/current.go b/bench/current.go new file mode 100644 index 0000000..d7ce7b9 --- /dev/null +++ b/bench/current.go @@ -0,0 +1,48 @@ +//go:build !v020 + +package main + +import ( + "math/rand" + + "github.com/akmonengine/feather" + "github.com/akmonengine/feather/actor" + "github.com/akmonengine/feather/constraint" + "github.com/akmonengine/feather/epa" + "github.com/akmonengine/feather/gjk" + "github.com/go-gl/mathgl/mgl64" +) + +const version = "current (soft step)" + +func tr(p mgl64.Vec3, q mgl64.Quat) actor.Transform { + return actor.Transform{Position: p, Rotation: q} +} + +var capsuleMaker shapeMaker = func(r *rand.Rand) actor.ShapeInterface { + return &actor.Capsule{HalfHeight: 0.05 + 0.5*r.Float64(), Radius: 0.05 + 0.3*r.Float64()} +} + +// narrow measures EPA itself (depth and normal), not the clipped manifold. +func narrow(a, b *actor.RigidBody) (bool, mgl64.Vec3, float64, int) { + var m constraint.Manifold + if !feather.Collide(a, b, 0, &m) { + return false, mgl64.Vec3{}, 0, 0 + } + s := &gjk.Simplex{} + if gjk.GJK(a, b, s) { + if r, err := epa.EPA(a, b, s, 0); err == nil { + return true, r.Normal, r.Depth, m.Count + } + } + return true, m.Normal, -m.MinSeparation(), m.Count +} + +func world(workers int) *feather.World { + return &feather.World{ + Gravity: mgl64.Vec3{0, -g, 0}, + Substeps: substeps, + Workers: workers, + Events: feather.NewEvents(), + } +} diff --git a/bench/go.mod b/bench/go.mod new file mode 100644 index 0000000..50b0144 --- /dev/null +++ b/bench/go.mod @@ -0,0 +1,12 @@ +module github.com/akmonengine/feather/bench + +go 1.24 + +require ( + github.com/akmonengine/feather v0.2.0 + github.com/go-gl/mathgl v1.2.0 +) + +// The bench runs against the working tree; go.v020.mod runs it against the published +// v0.2.0 (the XPBD solver) for comparison. +replace github.com/akmonengine/feather => ../ diff --git a/bench/go.sum b/bench/go.sum new file mode 100644 index 0000000..a4c6c44 --- /dev/null +++ b/bench/go.sum @@ -0,0 +1,2 @@ +github.com/go-gl/mathgl v1.2.0 h1:v2eOj/y1B2afDxF6URV1qCYmo1KW08lAMtTbOn3KXCY= +github.com/go-gl/mathgl v1.2.0/go.mod h1:pf9+b5J3LFP7iZ4XXaVzZrCle0Q/vNpB/vDe5+3ulRE= diff --git a/bench/go.v020.mod b/bench/go.v020.mod new file mode 100644 index 0000000..d0d711b --- /dev/null +++ b/bench/go.v020.mod @@ -0,0 +1,8 @@ +module github.com/akmonengine/feather/bench + +go 1.24 + +require ( + github.com/akmonengine/feather v0.2.0 + github.com/go-gl/mathgl v1.2.0 +) diff --git a/bench/go.v020.sum b/bench/go.v020.sum new file mode 100644 index 0000000..4c99215 --- /dev/null +++ b/bench/go.v020.sum @@ -0,0 +1,4 @@ +github.com/akmonengine/feather v0.2.0 h1:2kRwoBzNSh/n5camz1raeqiHnjWb2wS8dXcn+k5yxEk= +github.com/akmonengine/feather v0.2.0/go.mod h1:jjnzyih0Fstd5s3fWkUiv2/aHE+h5mVs/RlAabzoXV0= +github.com/go-gl/mathgl v1.2.0 h1:v2eOj/y1B2afDxF6URV1qCYmo1KW08lAMtTbOn3KXCY= +github.com/go-gl/mathgl v1.2.0/go.mod h1:pf9+b5J3LFP7iZ4XXaVzZrCle0Q/vNpB/vDe5+3ulRE= diff --git a/bench/main.go b/bench/main.go new file mode 100644 index 0000000..1235421 --- /dev/null +++ b/bench/main.go @@ -0,0 +1,325 @@ +// Command bench measures Feather on physical scenarios with a known answer, on the accuracy +// of its narrow phase against exact references, and on its speed. +// +// go run . [-only sim|epa|speed] # the working tree +// go run -tags v020 -modfile=go.v020.mod . [-only ...] # v0.2.0 (XPBD), for comparison +// go run . -check # the regressions against baseline.json (regression.go) +// go run . -update # write baseline.json, after a wanted change +// go run . -scenes ; go run . -compare # the scenes of Solver2D (reference.go) +// +// Every scene runs at AkmonEngine's rate: 50 Hz, 12 sub-steps, one worker. +package main + +import ( + "flag" + "fmt" + "math" + "math/rand" + "os" + "time" + + "github.com/akmonengine/feather" + "github.com/akmonengine/feather/actor" + "github.com/go-gl/mathgl/mgl64" +) + +const ( + dt = 1.0 / 50 + substeps = 12 + g = 9.81 +) + +func body(w *feather.World, t actor.Transform, s actor.ShapeInterface, typ actor.BodyType, mu, e float64) *actor.RigidBody { + b := actor.NewRigidBody(t, s, typ, 500) + b.Material.StaticFriction, b.Material.DynamicFriction, b.Material.Restitution = mu, mu, e + w.AddBody(b) + return b +} + +func ground(w *feather.World, mu float64) { + body(w, tr(mgl64.Vec3{}, mgl64.QuatIdent()), &actor.Plane{Normal: mgl64.Vec3{0, 1, 0}}, actor.BodyTypeStatic, mu, 0) +} + +func run(w *feather.World, seconds float64, each func()) { + for i := 0; i < int(math.Round(seconds/dt)); i++ { + w.Step(dt) + if each != nil { + each() + } + } +} + +func bad(v float64) bool { return math.IsNaN(v) || math.IsInf(v, 0) } + +func angle(q mgl64.Quat) float64 { + w := math.Min(1, math.Abs(q.W)) + return 2 * math.Acos(w) * 180 / math.Pi +} + +// rest: a body resting 10 s; drift of position and rotation after 1 s of settling. +func rest(name string, setup func(w *feather.World) *actor.RigidBody) { + w := world(1) + b := setup(w) + run(w, 1, nil) + p0, q0 := b.Transform.Position, b.Transform.Rotation + maxD, maxA := 0.0, 0.0 + run(w, 10, func() { + maxD = math.Max(maxD, b.Transform.Position.Sub(p0).Len()) + maxA = math.Max(maxA, angle(b.Transform.Rotation.Mul(q0.Inverse()))) + }) + fmt.Printf("%-34s drift %9.3f mm rotation %7.3f°\n", name, maxD*1000, maxA) +} + +// stack of n boxes (0.5 m cubes, 1 mm gaps) on the ground; top box displacement over 10 s. +func stack(n int, workers int) { + w := world(workers) + ground(w, 0.6) + var bs []*actor.RigidBody + for i := 0; i < n; i++ { + bs = append(bs, body(w, tr(mgl64.Vec3{0, 0.25 + float64(i)*0.501, 0}, mgl64.QuatIdent()), &actor.Box{HalfExtents: mgl64.Vec3{0.25, 0.25, 0.25}}, actor.BodyTypeDynamic, 0.6, 0)) + } + top := bs[n-1] + start := top.Transform.Position + run(w, 10, nil) + d := top.Transform.Position.Sub(start) + fell := false + for i, b := range bs { + if math.Abs(b.Transform.Position.Y()-(0.25+float64(i)*0.5)) > 0.1 || bad(b.Transform.Position.X()) { + fell = true + } + } + fmt.Printf("stack of %2d (workers %d) top moved %8.3f mm (horizontal %8.3f mm) fell=%v\n", n, workers, d.Len()*1000, math.Hypot(d.X(), d.Z())*1000, fell) +} + +func pyramid() { + w := world(1) + ground(w, 0.6) + var bs []*actor.RigidBody + for row := 0; row < 4; row++ { + for i := 0; i < 4-row; i++ { + x := (float64(i) - float64(3-row)/2) * 0.52 + y := 0.25 + float64(row)*0.501 + bs = append(bs, body(w, tr(mgl64.Vec3{x, y, 0}, mgl64.QuatIdent()), &actor.Box{HalfExtents: mgl64.Vec3{0.25, 0.25, 0.25}}, actor.BodyTypeDynamic, 0.6, 0)) + } + } + x0 := make([]mgl64.Vec3, len(bs)) + for i, b := range bs { + x0[i] = b.Transform.Position + } + run(w, 10, nil) + maxD := 0.0 + for i, b := range bs { + maxD = math.Max(maxD, b.Transform.Position.Sub(x0[i]).Len()) + } + fmt.Printf("pyramid of 10 worst box moved %8.3f mm\n", maxD*1000) +} + +// incline: box on a plane tilted by deg; analytic answer from Coulomb friction. +func incline(deg, mu float64) { + w := world(1) + th := deg * math.Pi / 180 + q := mgl64.QuatRotate(th, mgl64.Vec3{0, 0, 1}) + n := q.Rotate(mgl64.Vec3{0, 1, 0}) + body(w, tr(mgl64.Vec3{}, mgl64.QuatIdent()), &actor.Plane{Normal: n}, actor.BodyTypeStatic, mu, 0) + b := body(w, tr(n.Mul(0.25), q), &actor.Box{HalfExtents: mgl64.Vec3{0.25, 0.25, 0.25}}, actor.BodyTypeDynamic, mu, 0) + run(w, 0.5, nil) + p0 := b.Transform.Position + T := 2.0 + run(w, T, nil) + d := b.Transform.Position.Sub(p0).Len() + a := g * (math.Sin(th) - mu*math.Cos(th)) + want := 0.0 + if a > 0 { + v0 := a * 0.5 + want = v0*T + 0.5*a*T*T + } + fmt.Printf("incline %2.0f° μ=%.1f slid %8.3f m (expected %6.3f m)\n", deg, mu, d, want) +} + +// bounce: sphere dropped from 1 m (bottom) with restitution e; apex of first rebound. +func bounce(e float64) { + w := world(1) + ground(w, 0) + w.Bodies[0].Material.Restitution = e + b := body(w, tr(mgl64.Vec3{0, 1.25, 0}, mgl64.QuatIdent()), &actor.Sphere{Radius: 0.25}, actor.BodyTypeDynamic, 0, e) + hit, apex := false, 0.0 + run(w, 3, func() { + y := b.Transform.Position.Y() - 0.25 + if y < 0.01 { + hit = true + } + if hit { + apex = math.Max(apex, y) + } + }) + fmt.Printf("bounce e=%.1f from 1 m rebound %6.3f m (expected %6.3f m)\n", e, apex, e*e) +} + +// ramp: sphere dropped on a static box rotated by 30°; it must roll down its surface. +func ramp() { + w := world(1) + q := mgl64.QuatRotate(30*math.Pi/180, mgl64.Vec3{0, 0, 1}) + body(w, tr(mgl64.Vec3{0, 0, 0}, q), &actor.Box{HalfExtents: mgl64.Vec3{3, 0.25, 1}}, actor.BodyTypeStatic, 0.5, 0) + b := body(w, tr(mgl64.Vec3{0, 1.5, 0}, mgl64.QuatIdent()), &actor.Sphere{Radius: 0.25}, actor.BodyTypeDynamic, 0.5, 0) + n := q.Rotate(mgl64.Vec3{0, 1, 0}) + minGap, maxGap, landed := math.Inf(1), math.Inf(-1), false + run(w, 1.5, func() { + gap := b.Transform.Position.Dot(n) - 0.25 - 0.25 + if gap < 0.01 { + landed = true + } + if landed && math.Abs(b.Transform.Position.X()) < 2.2 { + minGap, maxGap = math.Min(minGap, gap), math.Max(maxGap, gap) + } + }) + fmt.Printf("sphere rolling on a rotated static box: gap to its surface between %.4f and %.4f m (expected ~0), rolled to x=%.2f\n", minGap, maxGap, b.Transform.Position.X()) +} + +// force: 1 kg-equivalent body in zero gravity, constant force for 1 s. +func force() { + w := world(1) + w.Gravity = mgl64.Vec3{} + b := body(w, tr(mgl64.Vec3{}, mgl64.QuatIdent()), &actor.Sphere{Radius: 0.25}, actor.BodyTypeDynamic, 0, 0) + m := b.Material.GetMass() + F := 10.0 + for i := 0; i < 50; i++ { + b.AddForce(mgl64.Vec3{F, 0, 0}) + w.Step(dt) + } + fmt.Printf("force %g N for 1 s on %.1f kg velocity %10.3f m/s (expected %6.3f)\n", F, m, b.Velocity.X(), F/m) +} + +func snapshot(w *feather.World) []mgl64.Vec3 { + var s []mgl64.Vec3 + for _, b := range w.Bodies { + s = append(s, b.Transform.Position) + } + return s +} + +func determinism() { + build := func(workers int) *feather.World { + w := world(workers) + ground(w, 0.6) + r := rand.New(rand.NewSource(1)) + for i := 0; i < 40; i++ { + q := mgl64.QuatRotate(r.Float64()*math.Pi, mgl64.Vec3{r.Float64(), r.Float64(), r.Float64()}.Normalize()) + body(w, tr(mgl64.Vec3{r.Float64()*3 - 1.5, 0.5 + float64(i)*0.6, r.Float64()*3 - 1.5}, q), &actor.Box{HalfExtents: mgl64.Vec3{0.25, 0.25, 0.25}}, actor.BodyTypeDynamic, 0.6, 0) + } + run(w, 5, nil) + return w + } + same := func(a, b []mgl64.Vec3) int { + n := 0 + for i := range a { + if a[i] != b[i] { + n++ + } + } + return n + } + a, b := snapshot(build(1)), snapshot(build(1)) + c, d := snapshot(build(8)), snapshot(build(8)) + fmt.Printf("determinism 40 falling boxes, 5 s workers1 run-vs-run: %d/40 differ; workers8 run-vs-run: %d/40 differ; w1 vs w8: %d/40 differ\n", same(a, b), same(c, d), same(a, c)) +} + +func main() { + part := flag.String("only", "", "sim, epa or speed (default: all)") + check := flag.Bool("check", false, "compare to the reference baseline.json, exit 1 on a regression") + update := flag.Bool("update", false, "write the reference baseline.json") + referenceScenes := flag.Bool("scenes", false, "run the scenes of Solver2D (bench/scenes) at their full size") + compare := flag.Bool("compare", false, "print the scenes of the working tree and of v0.2.0 side by side") + flag.Parse() + if *referenceScenes || *compare { + ok := *referenceScenes && runScenes() || *compare && compareScenes() + if !ok { + os.Exit(1) + } + return + } + if *check || *update { + if !regressions(*update) { + os.Exit(1) + } + return + } + if *part == "" || *part == "sim" { + rest("box on ground", func(w *feather.World) *actor.RigidBody { + ground(w, 0.6) + return body(w, tr(mgl64.Vec3{0, 0.25, 0}, mgl64.QuatIdent()), &actor.Box{HalfExtents: mgl64.Vec3{0.25, 0.25, 0.25}}, actor.BodyTypeDynamic, 0.6, 0) + }) + rest("box on static box", func(w *feather.World) *actor.RigidBody { + body(w, tr(mgl64.Vec3{0, -0.5, 0}, mgl64.QuatIdent()), &actor.Box{HalfExtents: mgl64.Vec3{2, 0.5, 2}}, actor.BodyTypeStatic, 0.6, 0) + return body(w, tr(mgl64.Vec3{0, 0.25, 0}, mgl64.QuatIdent()), &actor.Box{HalfExtents: mgl64.Vec3{0.25, 0.25, 0.25}}, actor.BodyTypeDynamic, 0.6, 0) + }) + rest("box off-centre on static box", func(w *feather.World) *actor.RigidBody { + body(w, tr(mgl64.Vec3{0, -0.5, 0}, mgl64.QuatIdent()), &actor.Box{HalfExtents: mgl64.Vec3{2, 0.5, 2}}, actor.BodyTypeStatic, 0.6, 0) + return body(w, tr(mgl64.Vec3{1.3, 0.25, 0.7}, mgl64.QuatRotate(0.5, mgl64.Vec3{0, 1, 0})), &actor.Box{HalfExtents: mgl64.Vec3{0.4, 0.1, 0.2}}, actor.BodyTypeDynamic, 0.6, 0) + }) + rest("sphere on ground", func(w *feather.World) *actor.RigidBody { + ground(w, 0.6) + return body(w, tr(mgl64.Vec3{0, 0.25, 0}, mgl64.QuatIdent()), &actor.Sphere{Radius: 0.25}, actor.BodyTypeDynamic, 0.6, 0) + }) + stack(3, 1) + stack(5, 1) + stack(10, 1) + stack(5, 8) + pyramid() + incline(20, 0.6) + incline(20, 0.2) + incline(35, 0.3) + bounce(0.5) + bounce(0.0) + ramp() + force() + determinism() + } + if *part == "speed" { + speed() + } + if *part == "" || *part == "epa" { + epaAccuracy() + } +} + +// speed times scenes of growing size (wall clock of the whole simulation). +func speed() { + scene := func(n int) *feather.World { + w := world(1) + ground(w, 0.6) + r := rand.New(rand.NewSource(3)) + side := int(math.Ceil(math.Sqrt(float64(n)))) + for i := 0; i < n; i++ { + x, z := float64(i%side)*0.6-float64(side)*0.3, float64((i/side)%side)*0.6-float64(side)*0.3 + y := 0.3 + float64(i/(side*side))*0.6 + r.Float64()*0.2 + var s actor.ShapeInterface = &actor.Box{HalfExtents: mgl64.Vec3{0.25, 0.25, 0.25}} + if i%2 == 1 { + s = &actor.Sphere{Radius: 0.25} + } + body(w, tr(mgl64.Vec3{x, y, z}, mgl64.QuatIdent()), s, actor.BodyTypeDynamic, 0.6, 0) + } + return w + } + for _, n := range []int{10, 100, 500} { + w := scene(n) + start := time.Now() + run(w, 3, nil) + el := time.Since(start) + fmt.Printf("%-14s %4d bodies, 3 s simulated (150 steps x %d substeps): %8.1f ms (%.3f ms/step)\n", version, n, substeps, float64(el.Microseconds())/1000, float64(el.Microseconds())/1000/150) + } + w := world(1) + ground(w, 0.6) + for row := 0; row < 10; row++ { + for i := 0; i < 10-row; i++ { + body(w, tr(mgl64.Vec3{(float64(i) - float64(9-row)/2) * 0.52, 0.25 + float64(row)*0.501, 0}, mgl64.QuatIdent()), &actor.Box{HalfExtents: mgl64.Vec3{0.25, 0.25, 0.25}}, actor.BodyTypeDynamic, 0.6, 0) + } + } + start := time.Now() + run(w, 3, nil) + el := time.Since(start) + maxY := 0.0 + for _, b := range w.Bodies[1:] { + maxY = math.Max(maxY, b.Transform.Position.Y()) + } + fmt.Printf("%-14s pyramid of 55, 3 s: %8.1f ms, top box at y=%.3f (expected %.3f)\n", version, float64(el.Microseconds())/1000, maxY, 0.25+9*0.5) +} diff --git a/bench/reference.go b/bench/reference.go new file mode 100644 index 0000000..18073de --- /dev/null +++ b/bench/reference.go @@ -0,0 +1,146 @@ +package main + +import ( + "encoding/json" + "fmt" + "os" + "path/filepath" + "slices" + "strings" + "time" + + "github.com/akmonengine/feather" + "github.com/akmonengine/feather/bench/scenes" +) + +// ========== REFERENCE SCENES ========== +// The scenes of Solver2D (bench/scenes) at their full size, on this version: +// +// go run . -scenes # the working tree +// go run -tags v020 -modfile=go.v020.mod . -scenes # v0.2.0 +// go run . -compare # both side by side, in Markdown + +// sceneRun: the result of a scene on a version, and its time per step +type sceneRun struct { + Result scenes.Result `json:"result"` + Passed bool `json:"passed"` + // Box3D: "at least as good as Box3D", "known gap" or "" (no reference) + Box3D string `json:"box3d"` + StepMs float64 `json:"stepMs"` +} + +func sceneFile(version string) string { + return filepath.Join(os.TempDir(), "feather-scenes-"+version+".json") +} + +// runScenes at their full size, prints and saves them +func runScenes() bool { + runs := map[string]sceneRun{} + for _, scene := range scenes.All { + if !scene.Supported() { + fmt.Printf("%-18s not supported by %s\n", scene.Name, scenes.Version) + continue + } + steps, elapsed := 0, time.Duration(0) + player := func(w *feather.World, seconds float64, each func()) { + scenes.Step(w, seconds, func() { + steps++ + if each != nil { + each() + } + }) + } + start := time.Now() + result := scene.Run(scenes.Full, player) + elapsed = time.Since(start) + run := sceneRun{Result: result, StepMs: milliseconds(elapsed) / float64(steps)} + run.Passed = scene.Check == nil || scene.Check(result) == nil + if scene.Reference != nil { + run.Box3D = "at least as good as Box3D" + if scene.Reference(result, scenes.Full) != nil { + run.Box3D = "known gap " + scene.Gap + } + } + runs[scene.Name] = run + fmt.Printf("%-18s %-6v %s\n", scene.Name, run.Passed, formatResult(result)) + } + data, err := json.MarshalIndent(runs, "", " ") + if err == nil { + err = os.WriteFile(sceneFile(scenes.Version), data, 0o644) + } + if err != nil { + fmt.Println("cannot save the scenes:", err) + return false + } + return true +} + +func formatResult(result scenes.Result) string { + var parts []string + for _, name := range sortedKeys(result) { + parts = append(parts, fmt.Sprintf("%s %.3g %s", name, result[name].Value, result[name].Unit)) + } + return strings.Join(parts, ", ") +} + +// compareScenes prints the scenes of the working tree and of v0.2.0, side by side +func compareScenes() bool { + load := func(version string) map[string]sceneRun { + runs := map[string]sceneRun{} + if data, err := os.ReadFile(sceneFile(version)); err == nil { + _ = json.Unmarshal(data, &runs) + } + return runs + } + current, old := load("current"), load("v0.2.0") + fmt.Println("| Scene | Measure | current | v0.2.0 |") + fmt.Println("|---|---|---|---|") + for _, scene := range scenes.All { + now, found := current[scene.Name] + if !found { + continue + } + before, oldFound := old[scene.Name] + for _, name := range sortedKeys(now.Result) { + value := func(run sceneRun, found bool) string { + if !found { + return "—" + } + m, ok := run.Result[name] + if !ok { + return "—" + } + return fmt.Sprintf("%.3g %s", m.Value, m.Unit) + } + fmt.Printf("| %s | %s | %s | %s |\n", scene.Name, name, value(now, true), value(before, oldFound)) + } + verdict := func(run sceneRun, found bool) string { + switch { + case !found: + return "not supported" + case run.Passed: + return "passes" + } + return "**fails**" + } + fmt.Printf("| %s | criteria | %s | %s |\n", scene.Name, verdict(now, true), verdict(before, oldFound)) + if now.Box3D != "" { + fmt.Printf("| %s | Box3D | %s | %s |\n", scene.Name, now.Box3D, map[bool]string{true: before.Box3D, false: "—"}[oldFound]) + } + fmt.Printf("| %s | ms per step | %.3g | %s |\n", scene.Name, now.StepMs, map[bool]string{true: fmt.Sprintf("%.3g", before.StepMs), false: "—"}[oldFound]) + } + return true +} + +func milliseconds(d time.Duration) float64 { + return float64(d.Nanoseconds()) / 1e6 +} + +func sortedKeys[V any](m map[string]V) []string { + keys := make([]string, 0, len(m)) + for key := range m { + keys = append(keys, key) + } + slices.Sort(keys) + return keys +} diff --git a/bench/regression.go b/bench/regression.go new file mode 100644 index 0000000..97941f9 --- /dev/null +++ b/bench/regression.go @@ -0,0 +1,585 @@ +//go:build !v020 + +package main + +import ( + "bufio" + "encoding/binary" + "encoding/json" + "fmt" + "hash/fnv" + "math" + "math/rand" + "os" + "runtime" + "slices" + "strings" + "time" + + "github.com/akmonengine/feather" + "github.com/akmonengine/feather/actor" + "github.com/akmonengine/feather/bench/scenes" + "github.com/go-gl/mathgl/mgl64" +) + +// ========== REGRESSIONS ========== +// Scenes run against a reference committed in baseline.json: +// +// go run . -check # compare the working tree to the reference, exit 1 on a regression +// go run . -update # write the reference, after a wanted change +// +// Each scene gives: +// - its fingerprint, a hash of the bits of the positions & rotations at the end: it must be identical on the same +// architecture (GOARCH). A change of behaviour, even tiny, changes it: -update after a wanted change +// - its quality metrics, each with its tolerance (qualityTolerances): only a worse value is a regression +// - its speed: the time of a step and of its phases (feather.Profile), the best of speedRuns runs. Only compared on +// the machine of the reference: a step slower by more than stepTolerance, or a phase taking more than 5 % of the +// step and slower by more than phaseTolerance, is a regression + +const ( + baselineFile = "baseline.json" + + // speedRuns: the speed of a scene is its best run + speedRuns = 3 + + // stepTolerance: a step may be 20 % slower (the noise of a shared machine) + stepTolerance = 0.20 + + // phaseTolerance: a phase may be 30 % slower (shorter, noisier) + phaseTolerance = 0.30 + + // phaseShare: the phases under 5 % of the step are too short to be compared + phaseShare = 0.05 + + // minComparedStepMs: under 0.1 ms per step, the noise of the timer dominates: the speed is not compared + minComparedStepMs = 0.1 +) + +// qualityTolerances: how much worse a quality metric may get, by unit +var qualityTolerances = map[string]float64{ + "mm": 0.5, // a depth, a drift: 0.5 mm, a tenth of LinearSlop + "m": 0.0005, // the same, in m + "%": 0.1, // an energy gain, in % of the energy + "m/s": 0.1, // a speed + "s": 0.02, // a time: one step + "": 1e-9, // a count +} + +// metric: a quality measure of a scene +type metric struct { + Value float64 `json:"value"` + Unit string `json:"unit"` +} + +// sceneResult: what a scene gives +type sceneResult struct { + Fingerprint string `json:"fingerprint"` + Quality map[string]metric `json:"quality"` + // StepMs: the mean time of a step (ms), PhasesMs the mean time of each phase + StepMs float64 `json:"stepMs"` + PhasesMs map[string]float64 `json:"phasesMs"` +} + +// baseline: the reference, and the machine where its speed was measured +type baseline struct { + Arch string `json:"arch"` + Machine string `json:"machine"` + Scenes map[string]sceneResult `json:"scenes"` +} + +// regressionScene builds its worlds, runs them with play, and measures its quality. The same scene gives the same bits +type regressionScene struct { + name string + run func() map[string]metric +} + +// recorder: the worlds of the scene being measured, and the time of their steps +type recorder struct { + // measuring: the quality is measured (the first run), else the scene is only timed + measuring bool + worlds []*feather.World + step time.Duration + phases map[string]time.Duration + steps int +} + +var recording *recorder + +// play steps the world for seconds, recording the time of each step +func play(w *feather.World, seconds float64, each func()) { + if !slices.Contains(recording.worlds, w) { + recording.worlds = append(recording.worlds, w) + } + for i := 0; i < int(math.Round(seconds/dt)); i++ { + w.Step(dt) + profile := w.Profile() + recording.step += profile.Step + for name, phase := range profilePhases(profile) { + recording.phases[name] += phase + } + recording.steps++ + if each != nil && recording.measuring { + each() + } + } +} + +// profilePhases by name +func profilePhases(p feather.Profile) map[string]time.Duration { + return map[string]time.Duration{ + "broad phase": p.BroadPhase, "narrow phase": p.NarrowPhase, "prepare": p.Prepare, "substeps": p.Substeps, + "restitution": p.Restitution, "continuous": p.Continuous, "islands": p.Islands, + } +} + +// ========== SCENES ========== + +var regressionScenes = append([]regressionScene{ + {"slope pile", slopePile}, + {"terrain piles", terrainPiles}, + {"pyramid", pyramidDrift}, + {"pile of 500", pile500}, + {"joint chain", jointChain}, + {"rain on terrain", rainOnTerrain}, +}, solverScenes()...) + +// solverScenes: the scenes of Solver2D (bench/scenes), at their small size (the full size is for -scenes) +func solverScenes() []regressionScene { + var result []regressionScene + for _, scene := range scenes.All { + result = append(result, regressionScene{"solver2d " + scene.Name, func() map[string]metric { + quality := map[string]metric{} + for name, m := range scene.Run(scenes.Small, play) { + quality[name] = metric{m.Value, m.Unit} + } + return quality + }}) + } + return result +} + +// mixedShape: a box, a sphere or a capsule, of 20 to 40 cm +func mixedShape(r *rand.Rand, i int) actor.ShapeInterface { + switch i % 3 { + case 1: + return &actor.Sphere{Radius: 0.2} + case 2: + return &actor.Capsule{HalfHeight: 0.25, Radius: 0.12} + } + return &actor.Box{HalfExtents: mgl64.Vec3{0.2 + 0.2*r.Float64(), 0.15, 0.25}} +} + +func randomTurn(r *rand.Rand) mgl64.Quat { + return mgl64.QuatRotate(r.Float64()*6, mgl64.Vec3{r.Float64(), r.Float64(), r.Float64()}.Normalize()) +} + +// slopePile: 60 bodies dropped on a slope of 30°, hitting each other and tumbling +func slopePile() map[string]metric { + angle := 30 * math.Pi / 180 + normal := mgl64.Vec3{-math.Sin(angle), math.Cos(angle), 0} + w := world(1) + slope := body(w, tr(mgl64.Vec3{}, mgl64.QuatIdent()), &actor.Plane{Normal: normal}, actor.BodyTypeStatic, 0.6, 0) + r := rand.New(rand.NewSource(2)) + for i := 0; i < 60; i++ { + x, z := r.Float64()*8-4, r.Float64()*16-8 + position := mgl64.Vec3{x, x*math.Tan(angle) + 1 + r.Float64()*3, z} + body(w, tr(position, randomTurn(r)), mixedShape(r, i), actor.BodyTypeDynamic, 0.6, 0) + } + landing := 0.0 + play(w, 4, func() { landing = math.Max(landing, deepest(w, slope)) }) + return map[string]metric{ + "landing depth": {landing * 1000, "mm"}, + "resting depth": {deepest(w, slope) * 1000, "mm"}, + } +} + +// terrainPiles: 60 bodies dropped on hills, 10 times (a pile is chaotic: its median & its worst are measured) +func terrainPiles() map[string]metric { + const piles = 10 + landings, restings := make([]float64, piles), make([]float64, piles) + for pile := 0; pile < piles; pile++ { + seed := int64(pile + 1) + w := world(1) + terrain := hills(w, seed) + field := terrain.Shape.(*actor.Heightfield) + r := rand.New(rand.NewSource(seed + 100)) + for i := 0; i < 60; i++ { + x, z := r.Float64()*16-8, r.Float64()*16-8 + ground, _ := field.HeightAt(x, z) + b := body(w, tr(mgl64.Vec3{x, ground + 1 + r.Float64()*3, z}, randomTurn(r)), mixedShape(r, i), actor.BodyTypeDynamic, 0.6, 0) + b.Material.RollingResistance = 0.1 + } + play(w, 4, func() { landings[pile] = math.Max(landings[pile], deepest(w, terrain)) }) + restings[pile] = deepest(w, terrain) + } + slices.Sort(landings) + slices.Sort(restings) + return map[string]metric{ + "median landing depth": {(landings[piles/2-1] + landings[piles/2]) / 2 * 1000, "mm"}, + "worst landing depth": {landings[piles-1] * 1000, "mm"}, + "worst resting depth": {restings[piles-1] * 1000, "mm"}, + } +} + +// pyramidDrift: a pyramid of 55 boxes stands 10 s +func pyramidDrift() map[string]metric { + w := world(1) + ground(w, 0.6) + for row := 0; row < 10; row++ { + for i := 0; i < 10-row; i++ { + position := mgl64.Vec3{(float64(i) - float64(9-row)/2) * 0.52, 0.25 + float64(row)*0.501, 0} + body(w, tr(position, mgl64.QuatIdent()), &actor.Box{HalfExtents: mgl64.Vec3{0.25, 0.25, 0.25}}, actor.BodyTypeDynamic, 0.6, 0) + } + } + start := positions(w) + play(w, 10, nil) + drift := 0.0 + for i, b := range w.Bodies { + drift = math.Max(drift, b.Transform.Position.Sub(start[i]).Len()) + } + return map[string]metric{"worst drift": {drift * 1000, "mm"}} +} + +// pile500: 500 boxes & spheres falling on the ground in a column, landing on each other +func pile500() map[string]metric { + w := world(1) + floor := body(w, tr(mgl64.Vec3{}, mgl64.QuatIdent()), &actor.Plane{Normal: mgl64.Vec3{0, 1, 0}}, actor.BodyTypeStatic, 0.6, 0) + const count, side = 500, 12 + for i := 0; i < count; i++ { + x, z := float64(i%side)*0.55-side*0.275, float64((i/side)%side)*0.55-side*0.275 + var shape actor.ShapeInterface = &actor.Box{HalfExtents: mgl64.Vec3{0.25, 0.25, 0.25}} + if i%2 == 1 { + shape = &actor.Sphere{Radius: 0.25} + } + body(w, tr(mgl64.Vec3{x, 0.3 + float64(i/(side*side))*0.55, z}, mgl64.QuatIdent()), shape, actor.BodyTypeDynamic, 0.6, 0) + } + landing := 0.0 + play(w, 3, func() { landing = math.Max(landing, deepest(w, floor)) }) + return map[string]metric{"landing depth": {landing * 1000, "mm"}} +} + +// jointChain: 20 capsules linked by ball joints, hanging from a static body, released horizontal: the chain swings +// without stretching and without gaining energy +func jointChain() map[string]metric { + w := world(1) + const links, length = 20, 0.3 + anchor := body(w, tr(mgl64.Vec3{0, 10, 0}, mgl64.QuatIdent()), &actor.Box{HalfExtents: mgl64.Vec3{0.1, 0.1, 0.1}}, actor.BodyTypeStatic, 0.5, 0) + turned := mgl64.QuatRotate(math.Pi/2, mgl64.Vec3{0, 0, 1}) + previous := anchor + var chain []*actor.RigidBody + for i := 0; i < links; i++ { + center := mgl64.Vec3{-(float64(i) + 0.5) * length, 10, 0} + link := body(w, tr(center, turned), &actor.Capsule{HalfHeight: length/2 - 0.05, Radius: 0.05}, actor.BodyTypeDynamic, 0.5, 0) + w.AddJoint(feather.NewBallJoint(previous, link, center.Add(mgl64.Vec3{length / 2, 0, 0}), mgl64.Vec3{1, 0, 0})) + chain = append(chain, link) + previous = link + } + energy := func() float64 { + total := 0.0 + for _, b := range chain { + mass := b.Material.GetMass() + total += 0.5*mass*b.Velocity.LenSqr() + 0.5*b.AngularVelocity.Dot(b.GetInertiaWorld().Mul3x1(b.AngularVelocity)) + mass*g*b.Transform.Position.Y() + } + return total + } + start := energy() + stretch, gain := 0.0, 0.0 + play(w, 5, func() { + // the end of a link against the start of the next (the axis Y of a link points away from the anchor) + for i := 0; i+1 < len(chain); i++ { + end := chain[i].Transform.ToWorld(mgl64.Vec3{0, length / 2, 0}) + next := chain[i+1].Transform.ToWorld(mgl64.Vec3{0, -length / 2, 0}) + stretch = math.Max(stretch, end.Sub(next).Len()) + } + gain = math.Max(gain, energy()-start) + }) + return map[string]metric{ + "worst stretch": {stretch * 1000, "mm"}, + "energy gain": {gain / math.Abs(start) * 100, "%"}, + } +} + +// rainOnTerrain: 200 bodies falling on hills, 10 more every 0.1 s +func rainOnTerrain() map[string]metric { + w := world(1) + terrain := hills(w, 7) + field := terrain.Shape.(*actor.Heightfield) + r := rand.New(rand.NewSource(8)) + landing, fell := 0.0, 0 + for step := 0; step < int(math.Round(5/dt)); step++ { + if step%5 == 0 && len(w.Bodies) < 200 { + for i := 0; i < 10; i++ { + x, z := r.Float64()*16-8, r.Float64()*16-8 + ground, _ := field.HeightAt(x, z) + b := body(w, tr(mgl64.Vec3{x, ground + 4, z}, randomTurn(r)), mixedShape(r, i), actor.BodyTypeDynamic, 0.6, 0) + b.Velocity = mgl64.Vec3{0, -8, 0} + } + } + play(w, dt, func() { landing = math.Max(landing, deepest(w, terrain)) }) + } + for _, b := range w.Bodies[1:] { + if b.Transform.Position.Y() < -5 && recording.measuring { + fell++ + } + } + return map[string]metric{ + "landing depth": {landing * 1000, "mm"}, + "fell through": {float64(fell), ""}, + } +} + +// hills: random hills in a bowl, 48x48 samples every 0.5 m (the terrain of the tests) +func hills(w *feather.World, seed int64) *actor.RigidBody { + const samples = 48 + r := rand.New(rand.NewSource(seed)) + heights := make([]float32, samples*samples) + phases := [4]float64{r.Float64() * 6, r.Float64() * 6, r.Float64() * 6, r.Float64() * 6} + for x := 0; x < samples; x++ { + for z := 0; z < samples; z++ { + bowlX, bowlZ := (float64(x)-(samples-1)/2.0)*0.5, (float64(z)-(samples-1)/2.0)*0.5 + heights[x*samples+z] = float32(0.03*(bowlX*bowlX+bowlZ*bowlZ) + 0.6*math.Sin(float64(x)*0.35+phases[0])*math.Cos(float64(z)*0.3+phases[1]) + + 0.3*math.Sin(float64(x+z)*0.8+phases[2]) + 0.05*math.Cos(float64(x-z)*1.7+phases[3])) + } + } + field := actor.NewHeightfield(samples, samples, heights, mgl64.Vec3{0.5, 1, 0.5}) + return body(w, tr(mgl64.Vec3{}, mgl64.QuatIdent()), field, actor.BodyTypeStatic, 0.6, 0) +} + +func positions(w *feather.World) []mgl64.Vec3 { + result := make([]mgl64.Vec3, len(w.Bodies)) + for i, b := range w.Bodies { + result[i] = b.Transform.Position + } + return result +} + +// ========== MEASURES ========== + +// deepest: the deepest dynamic body in the plane or the terrain (m) +func deepest(w *feather.World, surface *actor.RigidBody) float64 { + worst := 0.0 + for _, b := range w.Bodies { + if b.BodyType != actor.BodyTypeDynamic { + continue + } + switch shape := surface.Shape.(type) { + case *actor.Plane: + lowest := b.SupportWorld(shape.Normal.Mul(-1)) + worst = math.Max(worst, -(lowest.Dot(shape.Normal) + shape.Distance)) + case *actor.Heightfield: + worst = math.Max(worst, underTerrain(shape, b)) + } + } + return worst +} + +// underTerrain: the deepest point of the body under the terrain (identity transform), 0 above it +func underTerrain(field *actor.Heightfield, b *actor.RigidBody) float64 { + depth := 0.0 + switch shape := b.Shape.(type) { + case *actor.Box: + for c := 0; c < 8; c++ { + corner := shape.HalfExtents + for k := 0; k < 3; k++ { + if c&(1<= 0 && d4 <= d3 { + return b + } + vc := d1*d4 - d3*d2 + if vc <= 0 && d1 >= 0 && d3 <= 0 { + return a.Add(ab.Mul(d1 / (d1 - d3))) + } + cp := p.Sub(c) + d5, d6 := ab.Dot(cp), ac.Dot(cp) + if d6 >= 0 && d5 <= d6 { + return c + } + vb := d5*d2 - d1*d6 + if vb <= 0 && d2 >= 0 && d6 <= 0 { + return a.Add(ac.Mul(d2 / (d2 - d6))) + } + va := d3*d6 - d5*d4 + if va <= 0 && d4-d3 >= 0 && d5-d6 >= 0 { + return b.Add(c.Sub(b).Mul((d4 - d3) / ((d4 - d3) + (d5 - d6)))) + } + denominator := 1 / (va + vb + vc) + return a.Add(ab.Mul(vb * denominator)).Add(ac.Mul(vc * denominator)) +} + +// ========== RUN & COMPARE ========== + +// measureScene runs the scene speedRuns times: the quality & the fingerprint of the first run (all runs give the same +// bits), the speed of the best run +func measureScene(scene regressionScene) sceneResult { + var result sceneResult + for attempt := 0; attempt < speedRuns; attempt++ { + recording = &recorder{measuring: attempt == 0, phases: map[string]time.Duration{}} + quality := scene.run() + if attempt == 0 { + hash := fnv.New64a() + for _, w := range recording.worlds { + for _, b := range w.Bodies { + p, q := b.Transform.Position, b.Transform.Rotation + for _, x := range []float64{p[0], p[1], p[2], q.W, q.V[0], q.V[1], q.V[2]} { + _ = binary.Write(hash, binary.LittleEndian, math.Float64bits(x)) + } + } + } + result.Fingerprint = fmt.Sprintf("%016x", hash.Sum64()) + result.Quality = quality + } + stepMs := milliseconds(recording.step) / float64(recording.steps) + if attempt == 0 || stepMs < result.StepMs { + result.StepMs = stepMs + result.PhasesMs = map[string]float64{} + for name, phase := range recording.phases { + result.PhasesMs[name] = milliseconds(phase) / float64(recording.steps) + } + } + for _, w := range recording.worlds { + w.Close() + } + } + return result +} + +// machine: the CPU, for the speed +func machine() string { + model := "unknown CPU" + if file, err := os.Open("/proc/cpuinfo"); err == nil { + defer func() { _ = file.Close() }() + scanner := bufio.NewScanner(file) + for scanner.Scan() { + if name, found := strings.CutPrefix(scanner.Text(), "model name"); found { + model = strings.TrimSpace(strings.TrimPrefix(strings.TrimSpace(name), ":")) + break + } + } + } + return fmt.Sprintf("%s, %d CPUs", model, runtime.NumCPU()) +} + +// regressions runs the scenes, then writes the reference (update) or compares to it. Returns false on a regression +func regressions(update bool) bool { + current := baseline{Arch: runtime.GOOS + "/" + runtime.GOARCH, Machine: machine(), Scenes: map[string]sceneResult{}} + for _, scene := range regressionScenes { + current.Scenes[scene.name] = measureScene(scene) + fmt.Printf("%-16s %s %.3f ms/step\n", scene.name, current.Scenes[scene.name].Fingerprint, current.Scenes[scene.name].StepMs) + } + if update { + data, err := json.MarshalIndent(current, "", " ") + if err == nil { + err = os.WriteFile(baselineFile, append(data, '\n'), 0o644) + } + if err != nil { + fmt.Println("cannot write the reference:", err) + return false + } + fmt.Println("reference written in", baselineFile) + return true + } + + var reference baseline + data, err := os.ReadFile(baselineFile) + if err == nil { + err = json.Unmarshal(data, &reference) + } + if err != nil { + fmt.Println("cannot read the reference:", err) + return false + } + return compare(reference, current) +} + +// compare the scenes to the reference, prints every difference. Returns false on a regression +func compare(reference, current baseline) bool { + ok := true + fail := func(format string, args ...any) { + fmt.Printf("REGRESSION "+format+"\n", args...) + ok = false + } + sameArch, sameMachine := reference.Arch == current.Arch, reference.Arch == current.Arch && reference.Machine == current.Machine + if !sameMachine { + fmt.Printf("the reference comes from %s (%s): the speed is not compared\n", reference.Machine, reference.Arch) + } + for _, scene := range regressionScenes { + want, found := reference.Scenes[scene.name] + if !found { + fail("%s: not in the reference (-update)", scene.name) + continue + } + got := current.Scenes[scene.name] + if sameArch && got.Fingerprint != want.Fingerprint { + fail("%s: fingerprint %s, the reference is %s (the behaviour changed: -update if it is wanted)", scene.name, got.Fingerprint, want.Fingerprint) + } + for _, name := range sortedKeys(want.Quality) { + before, after := want.Quality[name], got.Quality[name] + tolerance := qualityTolerances[before.Unit] + switch { + case after.Value > before.Value+tolerance: + fail("%s: %s %.3f %s, the reference is %.3f (tolerance %g)", scene.name, name, after.Value, before.Unit, before.Value, tolerance) + case after.Value < before.Value-tolerance: + fmt.Printf("better %s: %s %.3f %s, the reference is %.3f\n", scene.name, name, after.Value, before.Unit, before.Value) + } + } + if !sameMachine || want.StepMs < minComparedStepMs { + continue + } + if got.StepMs > want.StepMs*(1+stepTolerance) { + fail("%s: %.3f ms per step, the reference is %.3f (tolerance %.0f %%)", scene.name, got.StepMs, want.StepMs, stepTolerance*100) + } + for _, name := range sortedKeys(want.PhasesMs) { + before, after := want.PhasesMs[name], got.PhasesMs[name] + if before >= phaseShare*want.StepMs && after > before*(1+phaseTolerance) { + fail("%s: %s %.3f ms per step, the reference is %.3f (tolerance %.0f %%)", scene.name, name, after, before, phaseTolerance*100) + } + } + } + if ok { + fmt.Println("no regression") + } + return ok +} diff --git a/bench/scenes/adapter_current.go b/bench/scenes/adapter_current.go new file mode 100644 index 0000000..54ee57e --- /dev/null +++ b/bench/scenes/adapter_current.go @@ -0,0 +1,53 @@ +//go:build !v020 + +package scenes + +import ( + "github.com/akmonengine/feather" + "github.com/akmonengine/feather/actor" + "github.com/go-gl/mathgl/mgl64" +) + +// Version of Feather the scenes run on +const Version = "current" + +// slop: the length tolerance of the collision detection (m) +const slop = feather.LinearSlop + +// the features of this version +const ( + hasCapsules = true + hasJoints = true +) + +func pose(position mgl64.Vec3, rotation mgl64.Quat) actor.Transform { + return actor.Transform{Position: position, Rotation: rotation} +} + +func capsule(halfHeight, radius float64) actor.ShapeInterface { + return &actor.Capsule{HalfHeight: halfHeight, Radius: radius} +} + +// hinge links 2 bodies around the axis through the anchor (world space) +func hinge(w *feather.World, a, b *actor.RigidBody, anchor, axis mgl64.Vec3) { + w.AddJoint(feather.NewHingeJoint(a, b, anchor, axis)) +} + +// ball links 2 bodies at the anchor (world space) +func ball(w *feather.World, a, b *actor.RigidBody, anchor mgl64.Vec3) { + w.AddJoint(feather.NewBallJoint(a, b, anchor, mgl64.Vec3{1, 0, 0})) +} + +// moved: the transform of the body was set by hand +func moved(b *actor.RigidBody) { + b.UpdateAABB() +} + +func newWorld() *feather.World { + return &feather.World{ + Gravity: mgl64.Vec3{0, -gravity, 0}, + Substeps: substeps, + Workers: 1, + Events: feather.NewEvents(), + } +} diff --git a/bench/scenes/adapter_v020.go b/bench/scenes/adapter_v020.go new file mode 100644 index 0000000..244e2d7 --- /dev/null +++ b/bench/scenes/adapter_v020.go @@ -0,0 +1,50 @@ +//go:build v020 + +package scenes + +import ( + "github.com/akmonengine/feather" + "github.com/akmonengine/feather/actor" + "github.com/go-gl/mathgl/mgl64" +) + +// Version of Feather the scenes run on +const Version = "v0.2.0" + +// slop: the length tolerance of the collision detection of the current version (m), for the same measures +const slop = 0.005 + +// the features of this version: no capsule, no joint +const ( + hasCapsules = false + hasJoints = false +) + +func pose(position mgl64.Vec3, rotation mgl64.Quat) actor.Transform { + return actor.Transform{Position: position, Rotation: rotation, InverseRotation: rotation.Inverse()} +} + +func capsule(halfHeight, radius float64) actor.ShapeInterface { + panic("scenes: no capsule in v0.2.0") +} + +func hinge(w *feather.World, a, b *actor.RigidBody, anchor, axis mgl64.Vec3) { + panic("scenes: no joint in v0.2.0") +} + +func ball(w *feather.World, a, b *actor.RigidBody, anchor mgl64.Vec3) { + panic("scenes: no joint in v0.2.0") +} + +// moved: v0.2.0 computes the AABB at each step +func moved(b *actor.RigidBody) {} + +func newWorld() *feather.World { + return &feather.World{ + Gravity: mgl64.Vec3{0, -gravity, 0}, + Substeps: substeps, + SpatialGrid: feather.NewSpatialGrid(2, 4096), + Workers: 1, + Events: feather.NewEvents(), + } +} diff --git a/bench/scenes/contact.go b/bench/scenes/contact.go new file mode 100644 index 0000000..d18cdbc --- /dev/null +++ b/bench/scenes/contact.go @@ -0,0 +1,476 @@ +package scenes + +import ( + "fmt" + "math" + + "github.com/akmonengine/feather/actor" + "github.com/go-gl/mathgl/mgl64" +) + +// ========== CONTACT SCENES ========== + +// contactSpeed: the fastest relative speed the solver pushes 2 overlapping bodies apart (feather.ContactSpeed, m/s). A +// body pushed through a chain of n overlapping contacts moves at n × contactSpeed at most: faster, it was thrown +const contactSpeed = 3.0 + +// layersSlop: a stack of n contacts may sink by the tolerance of the detection at each contact +func layersSlop(layers int) float64 { return float64(layers) * slop } + +// A cube of 2 m falls on the ground and rests: it rests at its height, and doesn't move anymore +var singleBox = Scene{ + Name: "single box", + Run: func(size Size, play Player) Result { + w := newWorld() + ground(w, mgl64.Vec3{}, material{friction: 0.5}) + body := box(w, mgl64.Vec3{0, 4, 0}, mgl64.Vec3{1, 1, 1}, material{friction: 0.5, density: 1}) + play(w, 1, nil) + start := body.Transform.Position + drift := 0.0 + play(w, 2, func() { drift = math.Max(drift, body.Transform.Position.Sub(start).Len()) }) + return Result{"height error": mm(math.Abs(body.Transform.Position.Y() - 1)), "rest drift": mm(drift)} + }, + Check: func(r Result) error { + return firstError(atMost(r, "height error", slop*1000, "1 contact"), atMost(r, "rest drift", slop*1000, "1 contact")) + }, +} + +// 3 spheres stacked, the top one 100 times heavier, removed after 2.4 s: the impulses stored for the warm start must not +// throw the 2 others up +var warmStartEnergy = Scene{ + Name: "warm start energy", + Run: func(size Size, play Player) Result { + w := newWorld() + ground(w, mgl64.Vec3{}, defaultMaterial) + light := material{friction: 0.6, density: 1} + sphere(w, mgl64.Vec3{0, 0.5, 0}, 0.5, light) + middle := sphere(w, mgl64.Vec3{0, 1.5, 0}, 0.5, light) + top := sphere(w, mgl64.Vec3{0, 2.5, 0}, 0.5, material{friction: 0.6, density: 100}) + play(w, 2.4, nil) + w.RemoveBody(top) + highest := middle.Transform.Position.Y() + play(w, 2.6, func() { highest = math.Max(highest, middle.Transform.Position.Y()) }) + return Result{"overshoot": mm(highest - middle.Transform.Position.Y())} + }, + Check: func(r Result) error { + return atMost(r, "overshoot", slop*1000, "the spheres rise only by what the load pressed") + }, +} + +// highMassRatio1: 3 pyramids of cubes of 2 m, a cube 100, 200 or 300 times heavier dropped on each +var highMassRatio1 = Scene{ + Name: "high mass ratio 1", + Run: func(size Size, play Player) Result { + count := map[Size]int{Small: 4, Full: 10}[size] + w := newWorld() + ground(w, mgl64.Vec3{}, material{friction: 0.5}) + var bodies, tops []*actor.RigidBody + for j := 0; j < 3; j++ { + origin := mgl64.Vec3{float64(j-1) * float64(2*count+2), 0, 0} + bodies = append(bodies, squarePyramid(w, origin, count, 1, 0, material{friction: 0.5, density: 1})...) + // the heavy cube, 2 m above the top + top := origin.Add(mgl64.Vec3{0, 1 + 2*float64(count) + 2, 0}) + tops = append(tops, box(w, top, mgl64.Vec3{1, 1, 1}, material{friction: 0.5, density: 100 * float64(j+1)})) + } + start := positions(bodies) + play(w, 5, nil) + sag := 0.0 + for _, top := range tops { + sag = math.Max(sag, 1+2*float64(count)-top.Transform.Position.Y()) + } + return Result{"worst drift": mm(worstDrift(bodies, start)), "heavy cube sag": mm(sag), "layers": {float64(count), ""}} + }, + // the pyramids hold: no cube leaves its place (moves by its half size), the heavy cube stays on the top + Check: func(r Result) error { + return firstError(atMost(r, "worst drift", 1000, "less than the half size of a cube"), + atMost(r, "heavy cube sag", 1000, "less than the half size of a cube")) + }, +} + +// highMass: a slab of 20 × 20 × 1 m dropped from 15 m on 2 cubes of 1 m, 400 times lighter, on a plane or on a thick +// static box: the scene of Solver2D extruded by 1 m (the same supports, the same ratio) +func highMass(thickGround bool) func(size Size, play Player) Result { + return func(size Size, play Player) Result { + w := newWorld() + if thickGround { + staticBox(w, mgl64.Vec3{0, -2, 0}, mgl64.QuatIdent(), mgl64.Vec3{40, 2, 40}, defaultMaterial) + } else { + ground(w, mgl64.Vec3{}, defaultMaterial) + } + var small []*actor.RigidBody + for _, x := range []float64{-9, 9} { + small = append(small, box(w, mgl64.Vec3{x, 0.5, 0}, mgl64.Vec3{0.5, 0.5, 0.5}, defaultMaterial)) + } + big := box(w, mgl64.Vec3{0, 26, 0}, mgl64.Vec3{10, 10, 0.5}, defaultMaterial) + start := positions(small) + bounce, into, landed := 0.0, 0.0, false + play(w, 5, func() { + landed = landed || big.Transform.Position.Y() < 11.5 + if landed { + bounce = math.Max(bounce, big.Velocity.Y()) + } + for _, b := range small { + into = math.Max(into, 0.5-b.Transform.Position.Y()) + } + }) + return Result{"slab sag": mm(11 - big.Transform.Position.Y()), "small cubes drift": mm(worstDrift(small, start)), + "bounce": {bounce, "m/s"}, "small cubes into ground": mm(into)} + } +} + +// checkHighMass: the slab stays on the small cubes (sinks less than their size), the small cubes stay in place (move +// less than their half size). A soft contact is a spring of a frequency, whatever the mass: under a load 400 times +// heavier, it sinks by about 400 g / ω² +func checkHighMass(r Result) error { + return firstError(atMost(r, "slab sag", 1000, "less than the size of a small cube"), + atMost(r, "small cubes drift", 500, "less than the half size of a small cube")) +} + +var highMassRatio2 = Scene{Name: "high mass ratio 2", Run: highMass(false), Check: checkHighMass} +var highMassRatio3 = Scene{Name: "high mass ratio 3", Run: highMass(true), Check: checkHighMass} + +// centeredImpact: a cube of 2 m, 10 times heavier, dropped from 2 m exactly on another, without friction: only the order +// of the points of a contact can turn them. Not in Solver2D: the probe of the gap of #821 +var centeredImpact = Scene{ + Name: "centered impact", + Run: func(size Size, play Player) Result { + w := newWorld() + ground(w, mgl64.Vec3{}, material{}) + low := box(w, mgl64.Vec3{0, 1, 0}, mgl64.Vec3{1, 1, 1}, material{density: 1}) + top := box(w, mgl64.Vec3{0, 5, 0}, mgl64.Vec3{1, 1, 1}, material{density: 10}) + lateral, bounce, landed := 0.0, 0.0, false + play(w, 3, func() { + landed = landed || top.Transform.Position.Y() < 3.2 + if landed { + bounce = math.Max(bounce, top.Velocity.Y()) + } + for _, b := range []*actor.RigidBody{low, top} { + lateral = math.Max(lateral, math.Hypot(b.Transform.Position.X(), b.Transform.Position.Z())) + } + }) + return Result{"lateral drift": mm(lateral), "bounce": {bounce, "m/s"}} + }, + // the top cube stays on the low one + Check: func(r Result) error { return atMost(r, "lateral drift", 1000, "less than the half size of a cube") }, +} + +// frictionRamp: 5 cubes of friction 0.75, 0.5, 0.35, 0.1, 0 on a ramp of 0.25 rad and friction 0.2. The friction of 2 +// bodies is their geometric mean: the 3 first stop (√(0.35 × 0.2) = 0.265 > tan 0.25 = 0.255), the 2 others slide with +// the acceleration of Coulomb, g (sin θ - μ cos θ) +var frictionRamp = Scene{ + Name: "friction ramp", + Run: func(size Size, play Player) Result { + const angle, rampFriction, duration = 0.25, 0.2, 2.0 + w := newWorld() + rotation := mgl64.QuatRotate(-angle, mgl64.Vec3{0, 0, 1}) + down, normal := rotation.Rotate(mgl64.Vec3{1, 0, 0}), rotation.Rotate(mgl64.Vec3{0, 1, 0}) + center := mgl64.Vec3{0, 10, 0} + staticBox(w, center, rotation, mgl64.Vec3{13, 0.25, 6}, material{friction: rampFriction}) + frictions := []float64{0.75, 0.5, 0.35, 0.1, 0} + var cubes []*actor.RigidBody + for i, friction := range frictions { + position := center.Add(normal.Mul(0.75)).Add(down.Mul(-10)).Add(mgl64.Vec3{0, 0, float64(i-2) * 2}) + cubes = append(cubes, addBody(w, position, rotation, &actor.Box{HalfExtents: mgl64.Vec3{0.5, 0.5, 0.5}}, actor.BodyTypeDynamic, material{friction: friction, density: 25})) + } + play(w, 0.5, nil) + start, speeds := positions(cubes), make([]float64, len(cubes)) + for i, c := range cubes { + speeds[i] = c.Velocity.Dot(down) + } + play(w, duration, nil) + stopped, sliding := 0.0, 0.0 + for i, c := range cubes { + travelled := c.Transform.Position.Sub(start[i]).Dot(down) + mixed := math.Sqrt(frictions[i] * rampFriction) + if mixed >= math.Tan(angle) { + stopped = math.Max(stopped, math.Abs(travelled)) + continue + } + acceleration := gravity * (math.Sin(angle) - mixed*math.Cos(angle)) + want := speeds[i]*duration + 0.5*acceleration*duration*duration + sliding = math.Max(sliding, math.Abs(travelled-want)) + } + return Result{"stopped slide": mm(stopped), "sliding error": mm(sliding)} + }, + Check: func(r Result) error { + // the constant acceleration a integrated by sub-steps of h during T: the position is late by a h T / 2 at most + const angle, duration = 0.25, 2.0 + bound := gravity*math.Sin(angle)*(Dt/substeps)*duration/2 + slop + return firstError(atMost(r, "stopped slide", slop*1000, "static friction"), atMost(r, "sliding error", bound*1000, "Coulomb")) + }, +} + +// overlapRecovery: a pyramid of cubes created overlapping by 25 %: the solver pushes them apart without throwing them +var overlapRecovery = Scene{ + Name: "overlap recovery", + Run: func(size Size, play Player) Result { + count := map[Size]int{Small: 4, Full: 6}[size] + w := newWorld() + ground(w, mgl64.Vec3{}, defaultMaterial) + cubes := squarePyramid(w, mgl64.Vec3{}, count, 0.5, -0.25, defaultMaterial) + fastest := 0.0 + // the pyramid falls apart as the cubes are pushed out: 6 layers need 4 s to part + play(w, 5, func() { fastest = math.Max(fastest, maxSpeed(cubes)) }) + overlap := 0.0 + for i := range cubes { + for j := i + 1; j < len(cubes); j++ { + overlap = math.Max(overlap, boxOverlap(cubes[i], cubes[j])) + } + } + return Result{"max speed": {fastest, "m/s"}, "final overlap": mm(overlap), "layers": {float64(count), ""}} + }, + Check: func(r Result) error { + // the top cube is pushed through the layers & the ground + return firstError(atMost(r, "max speed", contactSpeed*r["layers"].Value, "ContactSpeed per contact in series"), + atMost(r, "final overlap", slop*1000, "resting contacts")) + }, +} + +// verticalStack: cubes of 1 m dropped by 10 cm on each other, shifted by 1 cm to alternate sides +var verticalStack = Scene{ + Name: "vertical stack", + Run: func(size Size, play Player) Result { + count := map[Size]int{Small: 10, Full: 15}[size] + w := newWorld() + staticBox(w, mgl64.Vec3{0, -1, 0}, mgl64.QuatIdent(), mgl64.Vec3{100, 1, 100}, material{friction: 0.3}) + var cubes []*actor.RigidBody + for i := 0; i < count; i++ { + shift := 0.01 + if i%2 == 0 { + shift = -shift + } + cubes = append(cubes, box(w, mgl64.Vec3{shift, 0.55 + 1.1*float64(i), shift}, mgl64.Vec3{0.5, 0.5, 0.5}, material{friction: 0.3, density: 1})) + } + start := positions(cubes) + play(w, 5, nil) + drift := 0.0 + for i, c := range cubes { + d := c.Transform.Position.Sub(start[i]) + drift = math.Max(drift, math.Hypot(d.X(), d.Z())) + } + return Result{"horizontal drift": mm(drift), "layers": {float64(count), ""}} + }, + Check: func(r Result) error { + return atMost(r, "horizontal drift", layersSlop(int(r["layers"].Value))*1000, "a contact per layer") + }, +} + +// pyramid: a square pyramid of cubes of 1 m, built touching, stands +var pyramid = Scene{ + Name: "pyramid", + Run: func(size Size, play Player) Result { + count := map[Size]int{Small: 8, Full: 20}[size] + w := newWorld() + staticBox(w, mgl64.Vec3{0, -1, 0}, mgl64.QuatIdent(), mgl64.Vec3{100, 1, 100}, defaultMaterial) + cubes := squarePyramid(w, mgl64.Vec3{}, count, 0.5, 0, defaultMaterial) + start := positions(cubes) + play(w, 5, nil) + return Result{"worst drift": mm(worstDrift(cubes, start)), "layers": {float64(count), ""}} + }, + Check: func(r Result) error { + return atMost(r, "worst drift", layersSlop(int(r["layers"].Value))*1000, "a contact per layer") + }, +} + +// rush: spheres pulled towards a static sphere without gravity (12.7 m/s², as in Solver2D), from a spiral of 5 m to +// 25 m: they gather into a ball +var rush = Scene{ + Name: "rush", + Run: func(size Size, play Player) Result { + const pull = 1000 / (100 * math.Pi * 0.25) + count := map[Size]int{Small: 100, Full: 400}[size] + w := newWorld() + w.Gravity = mgl64.Vec3{} + m := material{friction: 0.2, density: 100} + addBody(w, mgl64.Vec3{}, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.5}, actor.BodyTypeStatic, m) + var spheres []*actor.RigidBody + golden := math.Pi * (3 - math.Sqrt(5)) + for i := 0; i < count; i++ { + // the directions of a Fibonacci sphere, further and further + y := 1 - 2*(float64(i)+0.5)/float64(count) + ring := math.Sqrt(1 - y*y) + direction := mgl64.Vec3{ring * math.Cos(golden*float64(i)), y, ring * math.Sin(golden*float64(i))} + spheres = append(spheres, sphere(w, direction.Mul(5+0.05*float64(i)), 0.5, m)) + } + pullAll := func() { + for _, s := range spheres { + if distance := s.Transform.Position.Len(); distance > 0.1 { + s.AddForce(s.Transform.Position.Mul(-pull * s.Material.GetMass() / distance)) + } + } + } + pullAll() + fastest := 0.0 + play(w, 5, func() { + fastest = math.Max(fastest, maxSpeed(spheres)) + pullAll() + }) + overlap := 0.0 + for i := range spheres { + for j := i + 1; j < len(spheres); j++ { + overlap = math.Max(overlap, sphereOverlap(spheres[i], spheres[j])) + } + } + return Result{"max speed": {fastest, "m/s"}, "final speed": {maxSpeed(spheres), "m/s"}, "final overlap": mm(overlap)} + }, + Check: func(r Result) error { + // free fall from 25 m under 12.7 m/s² reaches 25 m/s + return atMost(r, "max speed", math.Sqrt(2*1000/(100*math.Pi*0.25)*25), "no body faster than its fall") + }, +} + +// doubleDomino: 15 dominos 1 m apart, the first pushed at its top: each one topples the next, all fall +var doubleDomino = Scene{ + Name: "double domino", + Run: func(size Size, play Player) Result { + const count = 15 + w := newWorld() + staticBox(w, mgl64.Vec3{0, -1, 0}, mgl64.QuatIdent(), mgl64.Vec3{100, 1, 100}, defaultMaterial) + var dominos []*actor.RigidBody + for i := 0; i < count; i++ { + dominos = append(dominos, box(w, mgl64.Vec3{-0.5*count + float64(i), 0.5, 0}, mgl64.Vec3{0.125, 0.5, 0.5}, defaultMaterial)) + } + // an impulse of 0.2 N·s along X at the top of the first domino + first := dominos[0] + impulse, arm := mgl64.Vec3{0.2, 0, 0}, mgl64.Vec3{0, 0.5, 0} + first.Velocity = impulse.Mul(1 / first.Material.GetMass()) + first.AngularVelocity = first.GetInverseInertiaWorld().Mul3x1(arm.Cross(impulse)) + tipping := math.Atan(0.125 / 0.5) + fallTime, elapsed := 0.0, 0.0 + // 16 s: the last domino falls after 7 s, then the leaning dominos settle flat on the ground + play(w, 16, func() { + elapsed += Dt + if tilt(dominos[count-1]) > tipping && fallTime == 0 { + fallTime = elapsed + } + }) + fallen := 0 + for _, d := range dominos { + if tilt(d) > tipping { + fallen++ + } + } + return Result{"fallen": {float64(fallen), ""}, "time of the last": {fallTime, "s"}} + }, + Check: func(r Result) error { + if r["fallen"].Value != 15 { + return fmt.Errorf("%v dominos fell, want 15", r["fallen"].Value) + } + return nil + }, +} + +// confined: spheres created overlapping in a box too small for them, without gravity: they stay inside, and are not +// thrown +var confined = Scene{ + Name: "confined", + Run: func(size Size, play Player) Result { + count := map[Size]int{Small: 6, Full: 9}[size] + w := newWorld() + w.Gravity = mgl64.Vec3{} + // the inside of the box: 0.8 m per sphere of 1 m + half := 0.4 * float64(count) + for axis := 0; axis < 3; axis++ { + for _, side := range []float64{-1, 1} { + var center, extents mgl64.Vec3 + center[axis] = side * (half + 0.5) + extents = mgl64.Vec3{half + 1, half + 1, half + 1} + extents[axis] = 0.5 + staticBox(w, center, mgl64.QuatIdent(), extents, defaultMaterial) + } + } + var spheres []*actor.RigidBody + pitch := 0.72 + for i := 0; i < count; i++ { + for j := 0; j < count; j++ { + for k := 0; k < count; k++ { + p := mgl64.Vec3{float64(i), float64(j), float64(k)}.Sub(mgl64.Vec3{1, 1, 1}.Mul(float64(count-1) / 2)).Mul(pitch) + spheres = append(spheres, sphere(w, p, 0.5, defaultMaterial)) + } + } + } + fastest := 0.0 + play(w, 5, func() { fastest = math.Max(fastest, maxSpeed(spheres)) }) + escaped := 0 + for _, s := range spheres { + p := s.Transform.Position + if math.Abs(p.X()) > half || math.Abs(p.Y()) > half || math.Abs(p.Z()) > half { + escaped++ + } + } + return Result{"max speed": {fastest, "m/s"}, "escaped": {float64(escaped), ""}, "row": {float64(count), ""}} + }, + Check: func(r Result) error { + if r["escaped"].Value > 0 { + return fmt.Errorf("%v spheres escaped", r["escaped"].Value) + } + // a row of spheres between 2 walls: row + 1 contacts in series + return atMost(r, "max speed", contactSpeed*(r["row"].Value+1), "ContactSpeed per contact in series") + }, +} + +// cardHouse: a house of cards of 40 × 30 cm and 2 mm thick, leaning by 25° (from PEEL, as in Solver2D): it stands +var cardHouse = Scene{ + Name: "card house", + Run: func(size Size, play Player) Result { + levels := map[Size]int{Small: 3, Full: 5}[size] + const height, thickness, depth = 0.2, 0.001, 0.15 + w := newWorld() + m := material{friction: 0.7, density: 1} + staticBox(w, mgl64.Vec3{0, -2, 0}, mgl64.QuatIdent(), mgl64.Vec3{40, 2, 40}, m) + half := mgl64.Vec3{thickness, height, depth} + card := func(x, y, angle float64) *actor.RigidBody { + return addBody(w, mgl64.Vec3{x, y, 0}, mgl64.QuatRotate(angle, mgl64.Vec3{0, 0, 1}), &actor.Box{HalfExtents: half}, actor.BodyTypeDynamic, m) + } + var cards []*actor.RigidBody + lean := 25 * math.Pi / 180 + x0, y := 0.0, height-0.02 + for n := levels; n > 0; n-- { + x := x0 + for i := 0; i < n; i++ { + if i != n-1 { + cards = append(cards, card(x+0.25, y+height-0.015, math.Pi/2)) + } + cards = append(cards, card(x, y, -lean)) + x += 0.175 + cards = append(cards, card(x, y, lean)) + x += 0.175 + } + y += 2*height - 0.03 + x0 += 0.175 + } + play(w, 0.5, nil) + start := positions(cards) + play(w, 3, nil) + return Result{"worst drift": mm(worstDrift(cards, start)), "levels": {float64(levels), ""}} + }, + Check: func(r Result) error { + return atMost(r, "worst drift", layersSlop(int(r["levels"].Value))*1000, "a contact per level") + }, +} + +// circleStack: spheres of 1 m dropped exactly above each other: an aligned stack has no horizontal force, only the +// rounding moves it sideways +var circleStack = Scene{ + Name: "circle stack", + Run: func(size Size, play Player) Result { + count := map[Size]int{Small: 5, Full: 10}[size] + w := newWorld() + ground(w, mgl64.Vec3{}, defaultMaterial) + var spheres []*actor.RigidBody + for i := 0; i < count; i++ { + spheres = append(spheres, sphere(w, mgl64.Vec3{0, 4 + 3*float64(i), 0}, 1, defaultMaterial)) + } + drift := 0.0 + play(w, 8, func() { + for _, s := range spheres { + drift = math.Max(drift, math.Hypot(s.Transform.Position.X(), s.Transform.Position.Z())) + } + }) + top := spheres[count-1].Transform.Position.Y() + return Result{"horizontal drift": mm(drift), "top height error": mm(math.Abs(top - (2*float64(count) - 1)))} + }, + Check: func(r Result) error { + return atMost(r, "horizontal drift", 1e-6*1000, "no horizontal force") + }, +} diff --git a/bench/scenes/far.go b/bench/scenes/far.go new file mode 100644 index 0000000..74c79c8 --- /dev/null +++ b/bench/scenes/far.go @@ -0,0 +1,118 @@ +package scenes + +import ( + "math" + + "github.com/akmonengine/feather/actor" + "github.com/go-gl/mathgl/mgl64" +) + +// ========== FAR SCENES ========== +// A scene far from the origin (tens of km, as in Solver2D) keeps the criteria of the same scene at the origin. Both are +// run: "far deviation" is the largest difference between the positions of a body in both (relative to their origin). +// It is followed by the bench, not checked: a chaotic scene (a pile falling) amplifies the rounding far from the origin + +// farScene builds its bodies around origin and returns them, with the result of the scene +type farScene func(origin mgl64.Vec3, play Player) ([]*actor.RigidBody, Result) + +// far runs the scene at the origin and far from it: the result far, and the deviation between both +func far(origin mgl64.Vec3, scene farScene) func(size Size, play Player) Result { + return func(size Size, play Player) Result { + near, _ := scene(mgl64.Vec3{}, play) + farBodies, result := scene(origin, play) + deviation := 0.0 + for i := range near { + deviation = math.Max(deviation, farBodies[i].Transform.Position.Sub(origin).Sub(near[i].Transform.Position).Len()) + } + result["far deviation"] = mm(deviation) + return result + } +} + +// farPyramid: a pyramid of 10 layers of cubes of 1 m, 25 cm apart: they fall on each other, then stand +var farPyramid = Scene{ + Name: "far pyramid", + Run: far(mgl64.Vec3{100000, -80000, 60000}, func(origin mgl64.Vec3, play Player) ([]*actor.RigidBody, Result) { + const count = 10 + w := newWorld() + staticBox(w, origin.Add(mgl64.Vec3{0, -1, 0}), mgl64.QuatIdent(), mgl64.Vec3{100, 1, 100}, defaultMaterial) + cubes := squarePyramid(w, origin.Add(mgl64.Vec3{0, 0.5, 0}), count, 0.5, 0.25, defaultMaterial) + play(w, 2, nil) + start := positions(cubes) + play(w, 3, nil) + return cubes, Result{"worst drift": mm(worstDrift(cubes, start))} + }), + Check: func(r Result) error { + return atMost(r, "worst drift", layersSlop(10)*1000, "a contact per layer") + }, +} + +// farStack: a plank on a small roller and a small box, 2 cubes on the plank. The roller is a capsule lying across the +// plank, the circle of Solver2D in 3D (a sphere would hold the plank on a point) +var farStack = Scene{ + Name: "far stack", + capsules: true, + Run: far(mgl64.Vec3{40000, -25000, 30000}, func(origin mgl64.Vec3, play Player) ([]*actor.RigidBody, Result) { + w := newWorld() + staticBox(w, origin.Add(mgl64.Vec3{0, -1, 0}), mgl64.QuatIdent(), mgl64.Vec3{10, 1, 10}, defaultMaterial) + bodies := []*actor.RigidBody{ + addBody(w, origin.Add(mgl64.Vec3{1.875, 0.1, 0}), mgl64.QuatRotate(math.Pi/2, mgl64.Vec3{1, 0, 0}), capsule(0.4, 0.1), actor.BodyTypeDynamic, defaultMaterial), + box(w, origin.Add(mgl64.Vec3{-1.875, 0.15, 0}), mgl64.Vec3{0.1, 0.125, 0.1}, defaultMaterial), + box(w, origin.Add(mgl64.Vec3{0, 0.325, 0}), mgl64.Vec3{2, 0.05, 0.5}, defaultMaterial), + box(w, origin.Add(mgl64.Vec3{-0.5, 0.9, 0}), mgl64.Vec3{0.25, 0.25, 0.25}, defaultMaterial), + box(w, origin.Add(mgl64.Vec3{-0.55, 1.7, 0}), mgl64.Vec3{0.5, 0.5, 0.5}, defaultMaterial), + } + play(w, 2, nil) + start := positions(bodies) + play(w, 3, nil) + return bodies, Result{"worst drift": mm(worstDrift(bodies, start))} + }), + Check: func(r Result) error { + return atMost(r, "worst drift", layersSlop(3)*1000, "a contact per layer") + }, +} + +// farRecovery: the overlap recovery, far from the origin +var farRecovery = Scene{ + Name: "far recovery", + Run: far(mgl64.Vec3{80000, -70000, 50000}, func(origin mgl64.Vec3, play Player) ([]*actor.RigidBody, Result) { + w := newWorld() + ground(w, origin, defaultMaterial) + cubes := squarePyramid(w, origin, 4, 0.5, -0.25, defaultMaterial) + fastest := 0.0 + play(w, 3, func() { fastest = math.Max(fastest, maxSpeed(cubes)) }) + return cubes, Result{"max speed": {fastest, "m/s"}} + }), + Check: func(r Result) error { + return atMost(r, "max speed", contactSpeed*4, "ContactSpeed per contact in series") + }, +} + +// farChain: a chain of 40 capsules of 20 cm, starting horizontal, far from the origin +var farChain = Scene{ + Name: "far chain", + capsules: true, + joints: true, + Run: far(mgl64.Vec3{40000, -35000, 30000}, func(origin mgl64.Vec3, play Player) ([]*actor.RigidBody, Result) { + const count, hx, radius = 40, 0.1, 0.025 + w := newWorld() + m := material{friction: 0.6, density: 20} + height := float64(count) * hx + previous := addBody(w, origin.Add(mgl64.Vec3{-0.05, height, 0}), mgl64.QuatIdent(), &actor.Sphere{Radius: 0.02}, actor.BodyTypeStatic, m) + lying := mgl64.QuatRotate(math.Pi/2, mgl64.Vec3{0, 0, 1}) + var bodies []*actor.RigidBody + var links []link + for i := 0; i < count; i++ { + anchor := origin.Add(mgl64.Vec3{2 * float64(i) * hx, height, 0}) + body := damped(addBody(w, origin.Add(mgl64.Vec3{(1 + 2*float64(i)) * hx, height, 0}), lying, capsule(hx, radius), actor.BodyTypeDynamic, m)) + hinge(w, previous, body, anchor, mgl64.Vec3{0, 0, 1}) + links = append(links, newLink(previous, body, anchor)) + bodies = append(bodies, body) + previous = body + } + gap := 0.0 + play(w, 5, func() { gap = math.Max(gap, worstGap(links)) }) + return bodies, Result{"worst gap": mm(gap), "finite": finiteMetric(bodies)} + }), + Check: func(r Result) error { return holds(r, 0.1) }, +} diff --git a/bench/scenes/joints.go b/bench/scenes/joints.go new file mode 100644 index 0000000..ad21e84 --- /dev/null +++ b/bench/scenes/joints.go @@ -0,0 +1,202 @@ +package scenes + +import ( + "fmt" + "math" + + "github.com/akmonengine/feather/actor" + "github.com/go-gl/mathgl/mgl64" +) + +// ========== JOINT SCENES ========== +// The tests check that a joint holds (its gap stays under half a link: the chain is not broken), the bench follows the +// exact gaps + +// linkDamping of the bodies of the chains (as in Solver2D) +const linkDamping = 0.1 + +func damped(b *actor.RigidBody) *actor.RigidBody { + b.Material.LinearDamping, b.Material.AngularDamping = linkDamping, linkDamping + return b +} + +// holds: the joints are not broken, and no body is thrown +func holds(r Result, halfLink float64) error { + if r["finite"].Value != 1 { + return fmt.Errorf("a body is not finite") + } + return atMost(r, "worst gap", halfLink*1000, "the chain is not broken") +} + +func finiteMetric(bodies []*actor.RigidBody) Metric { + if finiteBodies(bodies) { + return Metric{1, ""} + } + return Metric{0, ""} +} + +// bridge: planks of 1 m linked by hinges, both ends on static bodies +var bridge = Scene{ + Name: "bridge", + joints: true, + Run: func(size Size, play Player) Result { + count := map[Size]int{Small: 40, Full: 160}[size] + w := newWorld() + x0 := -0.5 * float64(count) + m := material{friction: 0.6, density: 20} + left := staticBox(w, mgl64.Vec3{x0 - 0.5, 20, 0}, mgl64.QuatIdent(), mgl64.Vec3{0.5, 0.125, 0.5}, m) + right := staticBox(w, mgl64.Vec3{-x0 + 0.5, 20, 0}, mgl64.QuatIdent(), mgl64.Vec3{0.5, 0.125, 0.5}, m) + previous := left + var planks []*actor.RigidBody + var links []link + axis := mgl64.Vec3{0, 0, 1} + for i := 0; i <= count; i++ { + anchor := mgl64.Vec3{x0 + float64(i), 20, 0} + next := right + if i < count { + next = damped(box(w, mgl64.Vec3{x0 + 0.5 + float64(i), 20, 0}, mgl64.Vec3{0.5, 0.125, 0.5}, m)) + planks = append(planks, next) + } + hinge(w, previous, next, anchor, axis) + links = append(links, newLink(previous, next, anchor)) + previous = next + } + gap := 0.0 + play(w, 5, func() { gap = math.Max(gap, worstGap(links)) }) + sag := 20 - planks[count/2].Transform.Position.Y() + return Result{"worst gap": mm(gap), "sag": {sag, "m"}, "finite": finiteMetric(planks)} + }, + Check: func(r Result) error { return holds(r, 0.5) }, +} + +// ballAndChain: a chain of capsules of 1 m, free at its end, carrying a ball of 8 m. It starts horizontal and swings. +// In 3D the ball would be 37000 times heavier than a link: its density keeps the ratio of Solver2D, 672 +var ballAndChain = Scene{ + Name: "ball and chain", + capsules: true, + joints: true, + Run: func(size Size, play Player) Result { + count := map[Size]int{Small: 20, Full: 40}[size] + const hx, radius, ballRadius = 0.5, 0.125, 8.0 + w := newWorld() + m := material{friction: 0.6, density: 20} + height := float64(count) * hx + anchorBody := addBody(w, mgl64.Vec3{-0.5, height, 0}, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.1}, actor.BodyTypeStatic, m) + lying := mgl64.QuatRotate(math.Pi/2, mgl64.Vec3{0, 0, 1}) + previous := anchorBody + var bodies []*actor.RigidBody + var links []link + axis := mgl64.Vec3{0, 0, 1} + for i := 0; i < count; i++ { + anchor := mgl64.Vec3{2 * float64(i) * hx, height, 0} + body := damped(addBody(w, mgl64.Vec3{(1 + 2*float64(i)) * hx, height, 0}, lying, capsule(hx, radius), actor.BodyTypeDynamic, m)) + hinge(w, previous, body, anchor, axis) + links = append(links, newLink(previous, body, anchor)) + bodies = append(bodies, body) + previous = body + } + anchor := mgl64.Vec3{2 * float64(count) * hx, height, 0} + const ratio = 672 + linkVolume := math.Pi*radius*radius*2*hx + 4.0/3*math.Pi*radius*radius*radius + ballVolume := 4.0 / 3 * math.Pi * ballRadius * ballRadius * ballRadius + ball := damped(sphere(w, anchor.Add(mgl64.Vec3{ballRadius, 0, 0}), ballRadius, material{friction: 0.6, density: ratio * m.density * linkVolume / ballVolume})) + hinge(w, previous, ball, anchor, axis) + links = append(links, newLink(previous, ball, anchor)) + bodies = append(bodies, ball) + gap := 0.0 + play(w, 5, func() { gap = math.Max(gap, worstGap(links)) }) + return Result{"worst gap": mm(gap), "finite": finiteMetric(bodies)} + }, + Check: func(r Result) error { return holds(r, 0.5) }, +} + +// jointGrid: a net of spheres linked by ball joints to their 4 neighbours, held by 7 × 7 nodes in its middle, falling +// under twice the gravity (as in Solver2D) +var jointGrid = Scene{ + Name: "joint grid", + joints: true, + Run: func(size Size, play Player) Result { + count := map[Size]int{Small: 20, Full: 60}[size] + w := newWorld() + w.Gravity = w.Gravity.Mul(2) + m := material{friction: 0.6, density: 1} + nodes := make([]*actor.RigidBody, count*count) + var links []link + middle := count / 2 + for i := 0; i < count; i++ { + for k := 0; k < count; k++ { + bodyType := actor.BodyTypeDynamic + if i >= middle-3 && i <= middle+3 && k >= middle-3 && k <= middle+3 { + bodyType = actor.BodyTypeStatic + } + position := mgl64.Vec3{float64(k - middle), 0, float64(i - middle)} + node := addBody(w, position, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.4}, bodyType, m) + nodes[i*count+k] = node + for _, neighbour := range []int{(i-1)*count + k, i*count + k - 1} { + if (neighbour == (i-1)*count+k && i == 0) || (neighbour == i*count+k-1 && k == 0) { + continue + } + other := nodes[neighbour] + if other.BodyType == actor.BodyTypeStatic && bodyType == actor.BodyTypeStatic { + continue + } + anchor := other.Transform.Position.Add(position).Mul(0.5) + ball(w, other, node, anchor) + links = append(links, newLink(other, node, anchor)) + } + } + } + gap, fastest := 0.0, 0.0 + play(w, 3, func() { + gap = math.Max(gap, worstGap(links)) + fastest = math.Max(fastest, maxSpeed(nodes)) + }) + return Result{"worst gap": mm(gap), "max speed": {fastest, "m/s"}, "finite": finiteMetric(nodes)} + }, + Check: func(r Result) error { return holds(r, 0.5) }, +} + +// stretchedChain: a chain of 40 links of 1 m hanging from a static body, created stretched twice: it recovers the state +// of the same chain created at rest. A hanging chain sags at rest: its joints are springs of 60 Hz (as in Box2D v3) +var stretchedChain = Scene{ + Name: "stretched chain", + joints: true, + Run: func(size Size, play Player) Result { + chain := func(stretch float64) ([]*actor.RigidBody, []link, float64) { + const count, length = 40, 1.0 + w := newWorld() + top := float64(count) * length + m := material{friction: 0.6, density: 1} + previous := addBody(w, mgl64.Vec3{0, top, 0}, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.1}, actor.BodyTypeStatic, m) + var bodies []*actor.RigidBody + var links []link + // the joints are made at the rest length, then the links are moved + for i := 0; i < count; i++ { + anchor := mgl64.Vec3{0, top - float64(i)*length, 0} + body := sphere(w, anchor.Sub(mgl64.Vec3{0, 0.5 * length, 0}), 0.2, m) + ball(w, previous, body, anchor) + links = append(links, newLink(previous, body, anchor)) + bodies = append(bodies, body) + previous = body + } + for i, body := range bodies { + body.Transform.Position = mgl64.Vec3{0, top - (float64(i)+0.5)*length*stretch, 0} + moved(body) + } + fastest := 0.0 + play(w, 5, func() { fastest = math.Max(fastest, maxSpeed(bodies)) }) + return bodies, links, fastest + } + _, restLinks, _ := chain(1) + bodies, links, fastest := chain(2) + return Result{"final gap": mm(worstGap(links)), "rest gap": mm(worstGap(restLinks)), "max speed": {fastest, "m/s"}, + "finite": finiteMetric(bodies)} + }, + Check: func(r Result) error { + if r["finite"].Value != 1 { + return fmt.Errorf("a body is not finite") + } + // the joints pull the links back at 60 Hz (as in Box2D v3): the speed is followed by the bench, not bounded + return atMost(r, "final gap", r["rest gap"].Value+slop*1000, "the chain recovered its state at rest") + }, +} diff --git a/bench/scenes/references.go b/bench/scenes/references.go new file mode 100644 index 0000000..afaa8ff --- /dev/null +++ b/bench/scenes/references.go @@ -0,0 +1,87 @@ +//go:build !v020 + +package scenes + +import "fmt" + +// ========== REFERENCES ========== +// The scenes run on Box3D (Erin Catto, https://github.com/erincatto/box3d, commit 5643cd8 of 25/09/2026) with the same +// bodies and the same measures: bench/box3d/scenes.c, in its large world mode (double precision positions), at the rate +// of the bench (60 Hz, 4 sub-steps). Feather must do at least as well. +// +// Only the measures where lower is better are compared. Not compared: +// - the fastest body of "overlap recovery" & "far recovery": the push of the overlapping layers adds up along the pile, +// a solver converging less throws the top slower +// - the final speed of "rush": the packed ball never rests (both engines jitter between 0.7 & 3.6 m/s from 5 to 10 s) +// - the deviation of the far scenes: Box3D solves in float32 relative to the bodies, the rounding of the positions far +// from the origin vanishes in it; Feather keeps it (1e-11 m) and TestPlaceIndependence bounds its effect +// - the speeds of "joint grid" & "stretched chain": the swing of the net, the pull of the joints, physics not errors + +const ( + // box3dTolerance & box3dMargin: at least as good, to the measure: 2 % and 0.01 (in the unit of the measure) + box3dTolerance = 1.02 + box3dMargin = 0.01 +) + +// box3dValue: a measure of a scene, and its value in Box3D at both sizes +type box3dValue struct { + measure string + small, full float64 +} + +var box3dValues = map[string][]box3dValue{ + "single box": {{"height error", 0.0689445, 0.0689445}, {"rest drift", 0.0262277, 0.0262277}}, + "warm start energy": {{"overshoot", 12.4302, 12.4302}}, + "high mass ratio 1": {{"worst drift", 729.466, 499.656}, {"heavy cube sag", 126.722, 132.724}}, + "high mass ratio 2": {{"slab sag", 96.7198, 96.7198}, {"small cubes drift", 140.703, 140.703}, {"bounce", 4.33959, 4.33959}, + {"small cubes into ground", 594.57, 594.57}}, + "high mass ratio 3": {{"slab sag", 96.9108, 96.9108}, {"small cubes drift", 138.45, 138.45}, {"bounce", 4.43974, 4.43974}, + {"small cubes into ground", 593.361, 593.361}}, + "friction ramp": {{"stopped slide", 0, 0}, {"sliding error", 64.9105, 64.9105}}, + "overlap recovery": {{"final overlap", 4.9123, 3.7884}}, + "vertical stack": {{"horizontal drift", 6.45517, 14.1358}}, + "pyramid": {{"worst drift", 1.62748, 26.9122}}, + "rush": {{"final overlap", 4.01148, 32.734}}, + "confined": {{"max speed", 5.74906, 9.92108}}, + "card house": {{"worst drift", 0.0594942, 815.902}}, + "circle stack": {{"horizontal drift", 0, 0}, {"top height error", 6.86616, 26.7069}}, + "centered impact": {{"lateral drift", 308.96, 308.96}, {"bounce", 2.29705, 2.29705}}, + "bridge": {{"worst gap", 26.0555, 69.2763}}, + "ball and chain": {{"worst gap", 235.321, 232.72}}, + "joint grid": {{"worst gap", 64.768, 319.21}}, + "stretched chain": {{"final gap", 12.2763, 12.2763}, {"rest gap", 5.46556, 5.46556}}, + "far pyramid": {{"worst drift", 0, 0}}, + "far stack": {{"worst drift", 0.105557, 0.105557}}, + "far chain": {{"worst gap", 49.4286, 49.4286}}, +} + +// box3dGaps: the known gaps to Box3D, and the ticket following them. Feather runs 8 substeps (its setting for the games), +// Box3D its default 4: the gaps of the sinking under a load (high mass ratio 2 & 3), of the far stack, of the confined +// spheres, of the pyramid and of high mass ratio 1 closed at 8 substeps +var box3dGaps = map[string]string{ + // at the small size: the cards of 2 mm move by 0.14 mm, 0.06 in Box3D (at the full size, the house of Box3D falls) + "card house": "#821", + // at the small size: the net of 20 x 20 opens by 85 mm, 65 in Box3D (at the full size, 60 x 60, it is as good) + "joint grid": "#821", + // the stack of 5 spheres sinks by 7.10 mm on its springs, 6.87 in Box3D (within 3 %) + "circle stack": "#821", +} + +func init() { + for i := range All { + values, found := box3dValues[All[i].Name] + if !found { + continue + } + All[i].Reference = func(r Result, size Size) error { + for _, value := range values { + reference := map[Size]float64{Small: value.small, Full: value.full}[size] + if err := atMost(r, value.measure, reference*box3dTolerance+box3dMargin, fmt.Sprintf("Box3D %.3g", reference)); err != nil { + return err + } + } + return nil + } + All[i].Gap = box3dGaps[All[i].Name] + } +} diff --git a/bench/scenes/scenes.go b/bench/scenes/scenes.go new file mode 100644 index 0000000..f7d835a --- /dev/null +++ b/bench/scenes/scenes.go @@ -0,0 +1,267 @@ +// Package scenes holds the reference scenes of the solver, after the samples of Solver2D (Erin Catto, 2024, +// https://box2d.org/posts/2024/02/solver2d/), rewritten in 3D and compared to Box3D (references.go). Each scene isolates +// a known difficulty of a solver (a stack, a high mass ratio, a chain, bodies created overlapping...) and measures it +// with numbers, not by eye. +// +// The same code runs on the working tree and on v0.2.0 (-tags v020): the scenes needing a feature missing in v0.2.0 +// (capsules, joints) are skipped there. Every scene has 2 sizes: Small for the tests, Full for the bench. +package scenes + +import ( + "fmt" + "math" + + "github.com/akmonengine/feather" + "github.com/akmonengine/feather/actor" + "github.com/go-gl/mathgl/mgl64" +) + +const ( + // Dt of a step (s): the default rate of Box3D, the reference. 8 substeps: the setting of Feather for the games + // (Box3D runs its default, 4). The contacts are springs of 30 Hz whatever the substeps: a light body under a load + // sinks by (mass ratio) g / ω² at rest, and more substeps make the stacks, the impacts and the far scenes better + Dt = 1.0 / 60 + substeps = 8 + gravity = 9.81 +) + +// Size of a scene +type Size int + +const ( + // Small for the tests + Small Size = iota + // Full for the bench + Full +) + +// Metric: a measure of a scene +type Metric struct { + Value float64 `json:"value"` + Unit string `json:"unit"` +} + +// Result: the measures of a scene, by name +type Result map[string]Metric + +// Player steps the world for seconds, calling each (if not nil) after every step. The bench times the steps +type Player func(w *feather.World, seconds float64, each func()) + +// Scene of reference +type Scene struct { + Name string + // capsules & joints: the features the scene needs + capsules, joints bool + // Run builds the scene at its size, plays it and measures it + Run func(size Size, play Player) Result + // Check the result against the criteria of the scene: nil if it passes + Check func(Result) error + // Reference: at least as good as Box3D on the same scene, at the size (nil if no measure is compared, see + // references.go). Gap: the ticket following a known gap to Box3D, if any + Reference func(Result, Size) error + Gap string +} + +// Supported: the scene runs on this version +func (s Scene) Supported() bool { + return (!s.capsules || hasCapsules) && (!s.joints || hasJoints) +} + +// All the scenes, in the order of Solver2D +var All = []Scene{ + singleBox, warmStartEnergy, highMassRatio1, highMassRatio2, highMassRatio3, frictionRamp, overlapRecovery, + verticalStack, pyramid, rush, doubleDomino, confined, cardHouse, circleStack, centeredImpact, + bridge, ballAndChain, jointGrid, stretchedChain, + farPyramid, farStack, farRecovery, farChain, +} + +// Step: a Player without timing +func Step(w *feather.World, seconds float64, each func()) { + for i := 0; i < int(math.Round(seconds/Dt)); i++ { + w.Step(Dt) + if each != nil { + each() + } + } +} + +// ========== BUILDING ========== + +// material of a body +type material struct { + friction, restitution, density float64 +} + +var defaultMaterial = material{friction: 0.6, density: 1} + +func addBody(w *feather.World, position mgl64.Vec3, rotation mgl64.Quat, shape actor.ShapeInterface, bodyType actor.BodyType, m material) *actor.RigidBody { + body := actor.NewRigidBody(pose(position, rotation), shape, bodyType, m.density) + body.Material.StaticFriction, body.Material.DynamicFriction, body.Material.Restitution = m.friction, m.friction, m.restitution + w.AddBody(body) + return body +} + +func box(w *feather.World, position mgl64.Vec3, half mgl64.Vec3, m material) *actor.RigidBody { + return addBody(w, position, mgl64.QuatIdent(), &actor.Box{HalfExtents: half}, actor.BodyTypeDynamic, m) +} + +func staticBox(w *feather.World, position mgl64.Vec3, rotation mgl64.Quat, half mgl64.Vec3, m material) *actor.RigidBody { + return addBody(w, position, rotation, &actor.Box{HalfExtents: half}, actor.BodyTypeStatic, m) +} + +func sphere(w *feather.World, position mgl64.Vec3, radius float64, m material) *actor.RigidBody { + return addBody(w, position, mgl64.QuatIdent(), &actor.Sphere{Radius: radius}, actor.BodyTypeDynamic, m) +} + +// ground: a plane through origin, normal Y +func ground(w *feather.World, origin mgl64.Vec3, m material) *actor.RigidBody { + return addBody(w, origin, mgl64.QuatIdent(), &actor.Plane{Normal: mgl64.Vec3{0, 1, 0}, Distance: -origin.Y()}, actor.BodyTypeStatic, m) +} + +// squarePyramid: layers of cubes of half size h, the base of count × count, the cubes spaced by gap (0 = touching) +func squarePyramid(w *feather.World, origin mgl64.Vec3, count int, h, gap float64, m material) []*actor.RigidBody { + var bodies []*actor.RigidBody + pitch := 2*h + gap + for layer := 0; layer < count; layer++ { + side := count - layer + for i := 0; i < side; i++ { + for k := 0; k < side; k++ { + x := (float64(i) - float64(side-1)/2) * pitch + z := (float64(k) - float64(side-1)/2) * pitch + y := h + float64(layer)*pitch + bodies = append(bodies, box(w, origin.Add(mgl64.Vec3{x, y, z}), mgl64.Vec3{h, h, h}, m)) + } + } + } + return bodies +} + +// ========== MEASURES ========== + +func positions(bodies []*actor.RigidBody) []mgl64.Vec3 { + result := make([]mgl64.Vec3, len(bodies)) + for i, b := range bodies { + result[i] = b.Transform.Position + } + return result +} + +// worstDrift: the largest move of a body since start (m) +func worstDrift(bodies []*actor.RigidBody, start []mgl64.Vec3) float64 { + worst := 0.0 + for i, b := range bodies { + worst = math.Max(worst, b.Transform.Position.Sub(start[i]).Len()) + } + return worst +} + +// maxSpeed of the bodies (m/s) +func maxSpeed(bodies []*actor.RigidBody) float64 { + worst := 0.0 + for _, b := range bodies { + worst = math.Max(worst, b.Velocity.Len()) + } + return worst +} + +// tilt of a body: the angle of its Y axis from the world Y (rad) +func tilt(b *actor.RigidBody) float64 { + return math.Acos(math.Max(-1, math.Min(1, b.Transform.Rotation.Rotate(mgl64.Vec3{0, 1, 0}).Y()))) +} + +// boxOverlap: how much 2 boxes overlap along their best separating axis (the 15 axes of the separating axis theorem, +// exact for 2 boxes), 0 if they are apart +func boxOverlap(a, b *actor.RigidBody) float64 { + axesOf := func(body *actor.RigidBody) [3]mgl64.Vec3 { + r := body.Transform.Rotation + return [3]mgl64.Vec3{r.Rotate(mgl64.Vec3{1, 0, 0}), r.Rotate(mgl64.Vec3{0, 1, 0}), r.Rotate(mgl64.Vec3{0, 0, 1})} + } + axesA, axesB := axesOf(a), axesOf(b) + axes := append(axesA[:], axesB[:]...) + for _, x := range axesA { + for _, y := range axesB { + if cross := x.Cross(y); cross.Len() > 1e-9 { + axes = append(axes, cross.Normalize()) + } + } + } + overlap := math.Inf(1) + for _, n := range axes { + maxA, minA := a.SupportWorld(n).Dot(n), a.SupportWorld(n.Mul(-1)).Dot(n) + maxB, minB := b.SupportWorld(n).Dot(n), b.SupportWorld(n.Mul(-1)).Dot(n) + overlap = math.Min(overlap, math.Min(maxA-minB, maxB-minA)) + } + return math.Max(0, overlap) +} + +// sphereOverlap: how much 2 spheres overlap, 0 if they are apart +func sphereOverlap(a, b *actor.RigidBody) float64 { + ra, rb := a.Shape.(*actor.Sphere).Radius, b.Shape.(*actor.Sphere).Radius + return math.Max(0, ra+rb-a.Transform.Position.Sub(b.Transform.Position).Len()) +} + +// link: a joint seen by the measures, its anchor in the local space of each body +type link struct { + a, b *actor.RigidBody + localA, localB mgl64.Vec3 +} + +// newLink from the anchor in world space +func newLink(a, b *actor.RigidBody, anchor mgl64.Vec3) link { + local := func(body *actor.RigidBody) mgl64.Vec3 { + return body.Transform.Rotation.Conjugate().Rotate(anchor.Sub(body.Transform.Position)) + } + return link{a: a, b: b, localA: local(a), localB: local(b)} +} + +// gap: the distance between the anchor seen by both bodies (m), 0 for a joint kept exactly +func (l link) gap() float64 { + return toWorld(l.a, l.localA).Sub(toWorld(l.b, l.localB)).Len() +} + +// toWorld: a point of the body in world space +func toWorld(b *actor.RigidBody, local mgl64.Vec3) mgl64.Vec3 { + return b.Transform.Position.Add(b.Transform.Rotation.Rotate(local)) +} + +func worstGap(links []link) float64 { + worst := 0.0 + for _, l := range links { + worst = math.Max(worst, l.gap()) + } + return worst +} + +// finiteBodies: no NaN or infinity +func finiteBodies(bodies []*actor.RigidBody) bool { + for _, b := range bodies { + for _, x := range []float64{b.Transform.Position.X(), b.Transform.Position.Y(), b.Transform.Position.Z(), b.Velocity.Len()} { + if math.IsNaN(x) || math.IsInf(x, 0) { + return false + } + } + } + return true +} + +func mm(meters float64) Metric { return Metric{meters * 1000, "mm"} } + +// ========== CRITERIA ========== + +// atMost: the metric must not exceed the bound (in the unit of the metric) +func atMost(r Result, name string, bound float64, why string) error { + if r[name].Value > bound { + return fmt.Errorf("%s %.3f %s, at most %.3f (%s)", name, r[name].Value, r[name].Unit, bound, why) + } + return nil +} + +// firstError of the checks +func firstError(errs ...error) error { + for _, err := range errs { + if err != nil { + return err + } + } + return nil +} diff --git a/bench/scenes/scenes_test.go b/bench/scenes/scenes_test.go new file mode 100644 index 0000000..0a8e1d8 --- /dev/null +++ b/bench/scenes/scenes_test.go @@ -0,0 +1,30 @@ +//go:build !v020 + +package scenes + +import "testing" + +// Every scene, at its small size, meets its criteria. The scenes run in parallel +func TestScenes(t *testing.T) { + for _, scene := range All { + t.Run(scene.Name, func(t *testing.T) { + t.Parallel() + result := scene.Run(Small, Step) + t.Logf("%v", result) + if err := scene.Check(result); err != nil { + t.Error(err) + } + if scene.Reference == nil { + return + } + switch err := scene.Reference(result, Small); { + case err != nil && scene.Gap != "": + t.Logf("known gap (%s): %v", scene.Gap, err) + case err != nil: + t.Error(err) + case scene.Gap != "": + t.Errorf("the gap %s is closed: remove it", scene.Gap) + } + }) + } +} diff --git a/bench/v020.go b/bench/v020.go new file mode 100644 index 0000000..eb1cb4d --- /dev/null +++ b/bench/v020.go @@ -0,0 +1,56 @@ +//go:build v020 + +package main + +import ( + "fmt" + "math" + + "github.com/akmonengine/feather" + "github.com/akmonengine/feather/actor" + "github.com/go-gl/mathgl/mgl64" +) + +var capsuleMaker shapeMaker + +const version = "v0.2.0 (XPBD)" + +func tr(p mgl64.Vec3, q mgl64.Quat) actor.Transform { + return actor.Transform{Position: p, Rotation: q, InverseRotation: q.Inverse()} +} + +func narrow(a, b *actor.RigidBody) (bool, mgl64.Vec3, float64, int) { + ch := make(chan feather.Pair, 1) + ch <- feather.Pair{BodyA: a, BodyB: b} + close(ch) + cs := feather.NarrowPhase(ch, 1) + if len(cs) == 0 { + return false, mgl64.Vec3{}, 0, 0 + } + c := cs[0] + n := c.Normal + if c.BodyA != a { + n = n.Mul(-1) + } + depth := 0.0 + for _, p := range c.Points { + depth = math.Max(depth, p.Penetration) + } + return true, n, depth, len(c.Points) +} + +// regressions: the reference is measured on the working tree only +func regressions(update bool) bool { + fmt.Println("the regressions run on the working tree, not on v0.2.0") + return false +} + +func world(workers int) *feather.World { + return &feather.World{ + Gravity: mgl64.Vec3{0, -g, 0}, + Substeps: substeps, + SpatialGrid: feather.NewSpatialGrid(2.0, 4096), + Workers: workers, + Events: feather.NewEvents(), + } +} diff --git a/ccd.go b/ccd.go new file mode 100644 index 0000000..a1c45fe --- /dev/null +++ b/ccd.go @@ -0,0 +1,227 @@ +package feather + +import ( + "math" + "sync" + + "github.com/akmonengine/feather/actor" + "github.com/akmonengine/feather/gjk" + "github.com/go-gl/mathgl/mgl64" +) + +// ========== CONTINUOUS COLLISION ========== +// The speculative contacts stop most of the fast bodies. They can miss a body accelerated by the solver during the step: +// as in Box2D v3, after the solver, a fast body is moved back to its first impact with a static body (or with any body +// for a bullet), found along its motion. Its velocity is kept: the contact of the next step stops it. + +const ( + // continuousSafetyFactor: a body is fast when it moves more than half of its smallest extent during a step (Box2D) + continuousSafetyFactor = 0.5 + + // toiTarget: the moving body is stopped at this distance from the other body (m) + toiTarget = LinearSlop + + // toiTolerance around the target (m) + toiTolerance = 0.25 * LinearSlop + + // toiIterations of the conservative advancement + toiIterations = 32 + + // coreFraction: if the body already touches the other body at the start, only its core (a sphere of this fraction of + // its smallest extent, at its center) is stopped (B2_CORE_FRACTION of Box2D main, after v3.1) + coreFraction = 0.25 +) + +// sweep: the motion of a body during the step +type sweep struct { + start actor.Transform + end actor.Transform + // angle of the rotation from start to end (rad) + angle float64 +} + +// at: the transform at the fraction t of the motion (position lerp, rotation nlerp) +func (s *sweep) at(t float64) actor.Transform { + rotation := s.end.Rotation + if s.start.Rotation.Dot(rotation) < 0 { + rotation = rotation.Scale(-1) + } + return actor.Transform{ + Position: s.start.Position.Add(s.end.Position.Sub(s.start.Position).Mul(t)), + Rotation: s.start.Rotation.Scale(1 - t).Add(rotation.Scale(t)).Normalize(), + } +} + +// ccdScratch: the buffers of the continuous collision, reused to avoid the allocations +type ccdScratch struct { + stack []int32 + candidates []int32 + cells []int32 + shape triangleShape + core actor.Sphere +} + +var ccdPool = sync.Pool{New: func() any { return &ccdScratch{} }} + +// continuous collision of the fast bodies: first the bodies against the static bodies, then the bullets against all +// the bodies (at their final position). The result doesn't depend on the order of the bodies +func (w *World) continuous(s *solver, dt float64) { + scratch := ccdPool.Get().(*ccdScratch) + defer ccdPool.Put(scratch) + for _, bullets := range [2]bool{false, true} { + for i := range s.states { + state := &s.states[i] + body := state.body + if body.IsBullet != bullets || body.IsTrigger { + continue + } + minExtent, maxExtent := shapeExtents(body.Shape) + motion := sweep{ + start: actor.Transform{Position: body.Transform.Position.Sub(state.deltaPosition), Rotation: s.starts[i].rotation}, + end: body.Transform, + } + motion.angle = rotationAngle(state.deltaRotation) + // the farthest point of the body moves at most by the translation + the rotation * its extent + if state.deltaPosition.Len()+motion.angle*maxExtent <= continuousSafetyFactor*minExtent { + continue + } + scratch.core.Radius = coreFraction * minExtent + w.stopAtImpact(body, &motion, maxExtent, scratch) + } + } +} + +// stopAtImpact moves the body back to its first impact during its motion +func (w *World) stopAtImpact(body *actor.RigidBody, motion *sweep, radius float64, scratch *ccdScratch) { + // the bodies around the motion + swept := body.Shape.ComputeAABB(motion.start) + end := body.Shape.ComputeAABB(motion.end) + for k := 0; k < 3; k++ { + swept.Min[k] = math.Min(swept.Min[k], end.Min[k]) + swept.Max[k] = math.Max(swept.Max[k], end.Max[k]) + } + scratch.stack, scratch.candidates = w.tree.queryCandidates(swept, scratch.stack, scratch.candidates[:0]) + + fraction := 1.0 + for _, index := range scratch.candidates { + other := w.Bodies[index] + if other == body || other.IsTrigger || w.jointPairs[makePairKey(body, other)] > 0 { + continue + } + // the bullets against all the bodies, the other bodies against the static bodies only + if other.BodyType != actor.BodyTypeStatic && (!body.IsBullet || other.IsBullet) { + continue + } + if !swept.Overlaps(other.AABB()) { + continue + } + switch shape := other.Shape.(type) { + case *actor.Plane: + fraction = math.Min(fraction, impact(body.Shape, motion, radius, nil, shape, fraction, scratch)) + case *actor.Heightfield: + fraction = math.Min(fraction, w.heightfieldImpact(body.Shape, motion, radius, other, shape, swept, fraction, scratch)) + default: + proxy := gjk.NewProxy(other) + fraction = math.Min(fraction, impact(body.Shape, motion, radius, &proxy, nil, fraction, scratch)) + } + } + + if fraction < 1 { + body.Transform = motion.at(fraction) + } + body.UpdateAABB() +} + +// impact: the fraction of the motion at the first impact with the convex shape or the plane, 1 if there is none. +// If the body already touches at the start, its core is used (as in Box2D): a body resting on the ground is not stopped, +// a body going through is +func impact(shape actor.ShapeInterface, motion *sweep, radius float64, other *gjk.Proxy, plane *actor.Plane, maxFraction float64, scratch *ccdScratch) float64 { + t := timeOfImpact(shape, motion, radius, other, plane, maxFraction) + if t == 0 { + t = timeOfImpact(&scratch.core, motion, scratch.core.Radius, other, plane, maxFraction) + if t == 0 { + return 1 + } + } + return t +} + +// timeOfImpact: the fraction of the motion where the moving shape gets to toiTarget from the static shape (or plane), +// found by conservative advancement (Mirtich 1996, as in Bullet): at each iteration, the shape moves forward by the +// distance divided by the fastest approach speed of its points (translation along the normal + rotation * radius), +// so it never goes through. Returns 1 if there is no impact before maxFraction, 0 if the shapes touch at the start +func timeOfImpact(shape actor.ShapeInterface, motion *sweep, radius float64, other *gjk.Proxy, plane *actor.Plane, maxFraction float64) float64 { + translation := motion.end.Position.Sub(motion.start.Position) + t := 0.0 + for i := 0; i < toiIterations; i++ { + transform := motion.at(t) + var distance float64 + var normal mgl64.Vec3 + if plane != nil { + lowest := transform.ToWorld(shape.Support(transform.Rotation.Conjugate().Rotate(plane.Normal.Mul(-1)))) + distance, normal = lowest.Dot(plane.Normal)+plane.Distance, plane.Normal.Mul(-1) + } else { + proxy := gjk.NewProxyAt(transform, shape) + result := gjk.Distance(&proxy, other) + distance, normal = result.Distance, result.Normal + if result.Overlap { + distance = 0 + } + } + if distance <= toiTarget+toiTolerance { + return t + } + approach := translation.Dot(normal) + motion.angle*radius + if approach <= 0 { + return 1 + } + t += (distance - toiTarget) / approach + if t >= maxFraction { + return 1 + } + } + return t +} + +// heightfieldImpact: the first impact with the triangles under the motion +func (w *World) heightfieldImpact(shape actor.ShapeInterface, motion *sweep, radius float64, terrain *actor.RigidBody, field *actor.Heightfield, swept actor.AABB, maxFraction float64, scratch *ccdScratch) float64 { + scratch.cells = field.OverlapCells(localBounds(terrain.Transform, swept), scratch.cells[:0]) + fraction := maxFraction + cellsZ := field.ZSamples - 1 + for _, cell := range scratch.cells { + x, z := int(cell)/cellsZ, int(cell)%cellsZ + for t := 0; t < 2; t++ { + local, _ := field.Triangle(x, z, t) + for i := range local { + scratch.shape.vertices[i] = terrain.Transform.ToWorld(local[i]) + } + if !triangleAABB(scratch.shape.vertices).Overlaps(swept) { + continue + } + proxy := gjk.NewProxyAt(actor.NewTransform(), &scratch.shape) + fraction = math.Min(fraction, impact(shape, motion, radius, &proxy, nil, fraction, scratch)) + } + } + return fraction +} + +// shapeExtents: the smallest half size of the shape, and the distance of its farthest point from its center +func shapeExtents(shape actor.ShapeInterface) (float64, float64) { + switch shape := shape.(type) { + case *actor.Sphere: + return shape.Radius, shape.Radius + case *actor.Capsule: + return shape.Radius, shape.HalfHeight + shape.Radius + case *actor.Box: + h := shape.HalfExtents + return math.Min(h.X(), math.Min(h.Y(), h.Z())), h.Len() + } + aabb := shape.ComputeAABB(actor.NewTransform()) + size := aabb.Max.Sub(aabb.Min).Mul(0.5) + return math.Min(size.X(), math.Min(size.Y(), size.Z())), size.Len() +} + +// rotationAngle of a unit quaternion (rad) +func rotationAngle(q mgl64.Quat) float64 { + return 2 * math.Acos(math.Min(1, math.Abs(q.W))) +} diff --git a/ccd_test.go b/ccd_test.go new file mode 100644 index 0000000..85cdcc7 --- /dev/null +++ b/ccd_test.go @@ -0,0 +1,65 @@ +package feather + +import ( + "math" + "testing" + + "github.com/akmonengine/feather/actor" + "github.com/akmonengine/feather/gjk" + "github.com/go-gl/mathgl/mgl64" +) + +// The time of impact of a sphere moving towards a box: the exact fraction where the gap is toiTarget +func TestTimeOfImpact(t *testing.T) { + box := actor.NewRigidBody(actor.Transform{Position: mgl64.Vec3{3, 0, 0}, Rotation: mgl64.QuatIdent()}, &actor.Box{HalfExtents: mgl64.Vec3{0.5, 1, 1}}, actor.BodyTypeStatic, 0) + sphere := &actor.Sphere{Radius: 0.25} + motion := sweep{ + start: actor.Transform{Position: mgl64.Vec3{0, 0, 0}, Rotation: mgl64.QuatIdent()}, + end: actor.Transform{Position: mgl64.Vec3{4, 0, 0}, Rotation: mgl64.QuatIdent()}, + } + proxy := gjk.NewProxy(box) + fraction := timeOfImpact(sphere, &motion, 0.25, &proxy, nil, 1) + // the sphere touches the face x = 2.5 when its center is at 2.25, stopped toiTarget before + want := (2.25 - toiTarget) / 4 + if math.Abs(fraction-want)*4 > toiTolerance { + t.Errorf("fraction %.6f, want %.6f", fraction, want) + } + + // against a plane, with a rotation: a box falling on a corner + plane := &actor.Plane{Normal: mgl64.Vec3{0, 1, 0}} + falling := sweep{ + start: actor.Transform{Position: mgl64.Vec3{0, 2, 0}, Rotation: mgl64.QuatIdent()}, + end: actor.Transform{Position: mgl64.Vec3{0, -1, 0}, Rotation: mgl64.QuatRotate(0.5, mgl64.Vec3{0, 0, 1})}, + } + falling.angle = 0.5 + cube := &actor.Box{HalfExtents: mgl64.Vec3{0.25, 0.25, 0.25}} + fraction = timeOfImpact(cube, &falling, cube.HalfExtents.Len(), nil, plane, 1) + at := falling.at(fraction) + lowest := at.ToWorld(cube.Support(at.Rotation.Conjugate().Rotate(mgl64.Vec3{0, -1, 0}))).Y() + if fraction <= 0 || fraction >= 1 || lowest < toiTarget-1e-9 || lowest > toiTarget+toiTolerance { + t.Errorf("fraction %.4f, the lowest corner at %.5f m", fraction, lowest) + } +} + +// A normal body goes through the dynamic bodies during the continuous collision, a bullet stops on them +func TestBulletStopsOnDynamicBodies(t *testing.T) { + for _, bullet := range []bool{false, true} { + w := newScene(1) + w.Gravity = mgl64.Vec3{} + addBody(w, mgl64.Vec3{5, 0, 0}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{0.01, 1, 1}}, actor.BodyTypeDynamic, 0.5, 0) + ball := addBody(w, mgl64.Vec3{0, 0, 0}, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.02}, actor.BodyTypeDynamic, 0.5, 0) + ball.IsBullet = bullet + w.Step(sceneDt) + + // a motion through the plate, as if the solver had accelerated the ball + motion := sweep{start: ball.Transform, end: actor.Transform{Position: mgl64.Vec3{10, 0, 0}, Rotation: mgl64.QuatIdent()}} + ball.Transform = motion.end + scratch := ccdPool.Get().(*ccdScratch) + scratch.core.Radius = coreFraction * 0.02 + w.stopAtImpact(ball, &motion, 0.02, scratch) + stopped := ball.Transform.Position.X() < 5 + if stopped != bullet { + t.Errorf("bullet %v: the ball is at x=%.3f", bullet, ball.Transform.Position.X()) + } + } +} diff --git a/collision.go b/collision.go index 4368074..d0e8c4f 100644 --- a/collision.go +++ b/collision.go @@ -1,6 +1,7 @@ package feather import ( + "math" "sync" "github.com/akmonengine/feather/actor" @@ -10,221 +11,276 @@ import ( "github.com/go-gl/mathgl/mgl64" ) -const STIFF_COMPLIANCE = CONCRETE_COMPLIANCE - +// The pair cache and its thresholds are those of the body pair cache of Jolt (PhysicsSettings: 1 mm, 2°) const ( - CONCRETE_COMPLIANCE = 0.04e-9 - WOOD_COMPLIANCE = 0.16e-9 - LEATHER_COMPLIANCE = 14e-8 - TENDON_COMPLIANCE = 0.2e-7 - RUBBER_COMPLIANCE = 1e-6 - MUSCLE_COMPLIANCE = 0.2e-3 - FAT_COMPLIANCE = 1e-3 + // pairCacheMaxDeltaPosition: the contact of a pair is computed again if B moved more than 1 mm relative to A (m) + pairCacheMaxDeltaPosition = 0.001 + + // pairCacheMaxDeltaRotation: or if B turned more than 2° relative to A (rad) + pairCacheMaxDeltaRotation = 2 * math.Pi / 180 ) -// CollisionPair represents a pair of rigid bodies that potentially collide -type CollisionPair struct { - BodyA *actor.RigidBody - BodyB *actor.RigidBody - simplex *gjk.Simplex -} +// pairCacheCosMaxDeltaRotationDiv2: the dot product of 2 unit quaternions is the cosine of half their angle +var pairCacheCosMaxDeltaRotationDiv2 = math.Cos(pairCacheMaxDeltaRotation / 2) -// BroadPhase performs broad-phase collision detection using AABB overlap tests -// It returns pairs of bodies whose AABBs overlap and might be colliding -// This is an O(n²) brute-force approach suitable for small numbers of bodies -func BroadPhase(spatialGrid *SpatialGrid, bodies []*actor.RigidBody, workersCount int) <-chan Pair { - spatialGrid.Clear() +// BroadPhase returns the pairs of bodies whose AABBs overlap, always in the same order (whatever the workers) +func BroadPhase(bodies []*actor.RigidBody, workersCount int) []Pair { + boxes := make([]actor.AABB, len(bodies)) for i, body := range bodies { - spatialGrid.Insert(i, body) + boxes[i] = body.AABB() } - spatialGrid.SortCells() - - checkingPairs := spatialGrid.FindPairsParallel(bodies, workersCount) + var tree Tree + tree.rebuild(bodies, boxes) + pool := &workerPool{} + if workersCount > 1 { + pool.begin(workersCount) + defer pool.end() + } + return tree.findPairs(bodies, boxes, pool) +} - return checkingPairs +// NarrowPhase returns the contacts of the overlapping pairs (without speculative contacts), in the order of the pairs +func NarrowPhase(pairs []Pair, workersCount int) []constraint.Manifold { + return narrowPhase(pairs, workersCount, func(a, b *actor.RigidBody) float64 { return 0 }) } -func NarrowPhase(pairs <-chan Pair, workersCount int) []*constraint.ContactConstraint { - // Dispatcher: separate pairs with planes, and normal convex objects - planePairs := make(chan Pair, workersCount) - gjkPairs := make(chan Pair, workersCount) +// narrowPhase runs Collide on each pair in parallel. +// Each pair writes its manifolds at its own offset, so the order never depends on the workers +func narrowPhase(pairs []Pair, workersCount int, margin func(a, b *actor.RigidBody) float64) []constraint.Manifold { + offsets := make([]int, len(pairs)+1) + for i, pair := range pairs { + offsets[i+1] = offsets[i] + manifoldsOf(pair) + } + manifolds := make([]constraint.Manifold, offsets[len(pairs)]) + counts := make([]int, len(pairs)) + parallelFor(len(pairs), workersCount, func(i int) { + counts[i] = collidePair(pairs[i], margin(pairs[i].BodyA, pairs[i].BodyB), manifolds[offsets[i]:offsets[i+1]]) + }) + return compactManifolds(manifolds, offsets, counts) +} - go func() { - defer close(planePairs) - defer close(gjkPairs) +// manifoldsOf: the count of manifolds a pair can have, MaxManifoldsPerPair against a heightfield +func manifoldsOf(pair Pair) int { + if isHeightfield(pair.BodyA) || isHeightfield(pair.BodyB) { + return MaxManifoldsPerPair + } + return 1 +} - for pair := range pairs { - _, aIsPlane := pair.BodyA.Shape.(*actor.Plane) - _, bIsPlane := pair.BodyB.Shape.(*actor.Plane) +func isHeightfield(body *actor.RigidBody) bool { + _, ok := body.Shape.(*actor.Heightfield) + return ok +} - if aIsPlane || bIsPlane { - planePairs <- pair - } else { - gjkPairs <- pair +// collidePair: triggers keep only the real overlaps +func collidePair(pair Pair, margin float64, out []constraint.Manifold) int { + a, b := pair.BodyA, pair.BodyB + count := CollideAll(a, b, margin, out) + if a.IsTrigger || b.IsTrigger { + n := 0 + for k := 0; k < count; k++ { + if out[k].MinSeparation() < 0 { + out[n] = out[k] + n++ } } - }() - - // Canal pour collecter tous les contacts - allContacts := make(chan *constraint.ContactConstraint, workersCount*2) - var wg sync.WaitGroup - // Path 1: GJK/EPA for convex objects - wg.Add(1) - go func() { - defer wg.Done() - collisionPairs := GJK(gjkPairs, workersCount) - contactsChan := EPA(collisionPairs, workersCount) - for contact := range contactsChan { - allContacts <- contact + count = n + } + for k := 0; k < count; k++ { + setLocalAnchors(&out[k]) + } + return count +} + +// setLocalAnchors stores the contact in the local spaces of the bodies, for the next step +func setLocalAnchors(m *constraint.Manifold) { + transformA, transformB := m.BodyA.Transform, m.BodyB.Transform + for j := 0; j < m.Count; j++ { + point := &m.Points[j] + // Position is halfway between both surfaces, the normal goes from A to B + halfSeparation := m.Normal.Mul(point.Separation / 2) + point.LocalAnchorA = transformA.ToLocal(point.Position.Sub(halfSeparation)) + point.LocalAnchorB = transformB.ToLocal(point.Position.Add(halfSeparation)) + } + m.LocalNormal = transformA.Rotation.Conjugate().Rotate(m.Normal) + m.RelativePosition = transformA.ToLocal(transformB.Position) + m.RelativeRotation = transformA.Rotation.Conjugate().Mul(transformB.Rotation) +} + +// reuseManifold: if B moved less than 1 mm and 2° relative to A since the contact points were computed, +// the previous contact points are moved with the bodies instead of running the collision detection again +// (the body pair cache of Jolt). The separation of each point is measured again. +func reuseManifold(previous *constraint.Manifold, margin float64, m *constraint.Manifold) bool { + transformA, transformB := &previous.BodyA.Transform, &previous.BodyB.Transform + rotationA := &transformA.Rotation + + relativePosition := actor.RotateInverse(rotationA, transformB.Position.Sub(transformA.Position)) + if relativePosition.Sub(previous.RelativePosition).LenSqr() > pairCacheMaxDeltaPosition*pairCacheMaxDeltaPosition { + return false + } + conjugateA := mgl64.Quat{W: rotationA.W, V: rotationA.V.Mul(-1)} + relativeRotation := actor.MulQuat(&conjugateA, &transformB.Rotation) + if math.Abs(relativeRotation.Dot(previous.RelativeRotation)) < pairCacheCosMaxDeltaRotationDiv2 { + return false + } + + m.Reset(previous.BodyA, previous.BodyB) + m.Normal = actor.Rotate(rotationA, previous.LocalNormal) + m.LocalNormal = previous.LocalNormal + m.RelativePosition = previous.RelativePosition + m.RelativeRotation = previous.RelativeRotation + for j := 0; j < previous.Count; j++ { + point := &previous.Points[j] + onA := transformA.Position.Add(actor.Rotate(rotationA, point.LocalAnchorA)) + onB := transformB.Position.Add(actor.Rotate(&transformB.Rotation, point.LocalAnchorB)) + separation := onB.Sub(onA).Dot(m.Normal) + if separation > margin { + continue } - }() - - // Path 2: analytic collisions with planes - wg.Add(1) - go func() { - defer wg.Done() - contactsChan := collidePlane(planePairs, workersCount) - for contact := range contactsChan { - allContacts <- contact + m.Points[m.Count] = constraint.ContactPoint{ + Position: onA.Add(onB).Mul(0.5), + Separation: separation, + LocalAnchorA: point.LocalAnchorA, + LocalAnchorB: point.LocalAnchorB, } - }() - - // Fermer le canal de sortie quand tout est fini - go func() { - wg.Wait() - close(allContacts) - }() + m.Count++ + } + return m.Count > 0 +} - // Collecter tous les contacts - contacts := make([]*constraint.ContactConstraint, 0) - for c := range allContacts { - contacts = append(contacts, c) +// compactManifolds keeps the manifolds found, in the same order: counts[i] manifolds at offsets[i] for the pair i +func compactManifolds(manifolds []constraint.Manifold, offsets, counts []int) []constraint.Manifold { + n := 0 + for i, count := range counts { + for k := 0; k < count; k++ { + manifolds[n] = manifolds[offsets[i]+k] + n++ + } } - //fmt.Println("COUNT PAIRS", len(contacts)) - return contacts + return manifolds[:n] } -func GJK(pairChan <-chan Pair, workersCount int) <-chan CollisionPair { - collisionChan := make(chan CollisionPair, workersCount) - - go func() { - var wg sync.WaitGroup - defer close(collisionChan) - - for range workersCount { - wg.Add(1) - go func() { - defer wg.Done() - - for p := range pairChan { - simplex := gjk.SimplexPool.Get().(*gjk.Simplex) - simplex.Reset() - - if collision := gjk.GJK(p.BodyA, p.BodyB, simplex); collision { - collisionChan <- CollisionPair{ - BodyA: p.BodyA, - BodyB: p.BodyB, - simplex: simplex, - } - } else { - gjk.SimplexPool.Put(simplex) - } - } - }() +// Collide computes the contact between a and b, including the points closer than margin. +// The normal points from a to b. +// - planes: CollideWithPlane of the shape +// - spheres & capsules: closest points of their segments (collision_capsule.go) +// - other shapes: GJK/EPA, then the contact points are clipped (epa/manifold.go) +// +// Against a heightfield, a body can touch the terrain with several normals: Collide keeps the deepest patch, +// CollideAll returns all of them +func Collide(a, b *actor.RigidBody, margin float64, m *constraint.Manifold) bool { + var manifolds [1]constraint.Manifold + found := CollideAll(a, b, margin, manifolds[:]) > 0 + *m = manifolds[0] + return found +} - } - wg.Wait() - }() +// CollideAll writes in manifolds the contacts between a and b (MaxManifoldsPerPair at most), and returns their count +func CollideAll(a, b *actor.RigidBody, margin float64, manifolds []constraint.Manifold) int { + if len(manifolds) == 0 { + return 0 + } + m := &manifolds[0] + m.Reset(a, b) - return collisionChan + if field, ok := a.Shape.(*actor.Heightfield); ok { + return collideHeightfield(a, field, b, margin, false, manifolds) + } + if field, ok := b.Shape.(*actor.Heightfield); ok { + return collideHeightfield(b, field, a, margin, true, manifolds) + } + if collide(a, b, margin, m) { + return 1 + } + return 0 } -func EPA(p <-chan CollisionPair, workersCount int) <-chan *constraint.ContactConstraint { - ch := make(chan *constraint.ContactConstraint, workersCount) - - go func() { - var wg sync.WaitGroup - defer close(ch) - - for range workersCount { - wg.Add(1) - go func() { - defer wg.Done() - for pair := range p { - contact, err := epa.EPA(pair.BodyA, pair.BodyB, pair.simplex) - gjk.SimplexPool.Put(pair.simplex) - if err != nil { - continue - } - ch <- &contact - } - }() - } +// collide the convex shapes a & b +func collide(a, b *actor.RigidBody, margin float64, m *constraint.Manifold) bool { + if plane, ok := a.Shape.(*actor.Plane); ok { + return collidePlane(plane, b, margin, false, m) + } + if plane, ok := b.Shape.(*actor.Plane); ok { + return collidePlane(plane, a, margin, true, m) + } + + if isAnalyticPair(a.Shape, b.Shape) { + return collideAnalyticPair(a, b, margin, m) + } - wg.Wait() - }() + simplex := gjk.SimplexPool.Get().(*gjk.Simplex) + defer gjk.SimplexPool.Put(simplex) - return ch + result, ok := penetration(a, b, margin, simplex) + if !ok { + return false + } + epa.Manifold(a, b, result, margin, m) + return m.Count > 0 } -func collidePlane(pairs <-chan Pair, workersCount int) <-chan *constraint.ContactConstraint { - ch := make(chan *constraint.ContactConstraint, workersCount) - - go func() { - var wg sync.WaitGroup - defer close(ch) - - for range workersCount { - wg.Add(1) - go func() { - defer wg.Done() - for pair := range pairs { - // Identifier quel body est le plan - var plane *actor.Plane - var object *actor.RigidBody - var planeBody *actor.RigidBody - var contactNormal mgl64.Vec3 - - if p, ok := pair.BodyA.Shape.(*actor.Plane); ok { - plane = p - planeBody = pair.BodyA - object = pair.BodyB - contactNormal = plane.Normal - } else if p, ok := pair.BodyB.Shape.(*actor.Plane); ok { - plane = p - planeBody = pair.BodyB - object = pair.BodyA - contactNormal = plane.Normal.Mul(-1) - } else { - continue // No plane (should not happen, the data is prefiltered in NarrowPhase) - } - - collision, result := object.Shape.CollideWithPlane(plane.Normal, plane.Distance, object.Transform) - - if !collision { - continue - } - - var points []constraint.ContactPoint - for _, point := range result { - points = append(points, constraint.ContactPoint{Position: point.Position, Penetration: point.Penetration}) - } - - // Créer la contrainte - contact := &constraint.ContactConstraint{ - BodyA: planeBody, - BodyB: object, - Normal: contactNormal, - Points: points, - } - - ch <- contact - } - }() +// penetration of a + margin into b (the convex shapes), false if they are further than the margin. +// A rounded shape (sphere, capsule) is its core with a radius: GJK gives the distance and the closest points of the +// cores, exact against a polytope (Bullet, Jolt), and the radii are added along their direction. EPA runs on the full +// shapes only if the cores overlap, or are too close for their direction to be a normal +func penetration(a, b *actor.RigidBody, margin float64, simplex *gjk.Simplex) (epa.Result, bool) { + coreA, radiusA := gjk.NewCoreProxy(a) + coreB, radiusB := gjk.NewCoreProxy(b) + if radiusA+radiusB > 0 { + if closest := gjk.Distance(&coreA, &coreB); !closest.Overlap && closest.Distance > normalEpsilon { + depth := radiusA + radiusB + margin - closest.Distance + if depth < 0 { + return epa.Result{}, false + } + return epa.Result{ + Normal: closest.Normal, + Depth: depth, + WitnessA: closest.PointA.Add(closest.Normal.Mul(radiusA + margin)), + WitnessB: closest.PointB.Sub(closest.Normal.Mul(radiusB)), + }, true } + } + + simplex.Reset() + proxyA, proxyB := gjk.NewProxy(a), gjk.NewProxy(b) + if !gjk.GJKProxies(&proxyA, &proxyB, margin, simplex) { + return epa.Result{}, false + } + result, err := epa.EPAProxies(&proxyA, &proxyB, simplex, margin) + if err != nil { + return epa.Result{}, false + } + return result, true +} + +// planeBuffers: the buffers of collidePlane, reused to avoid the allocations +type planeBuffers struct { + plane actor.PlaneContact + points []constraint.ContactPoint +} - wg.Wait() - }() +var planeContactsPool = sync.Pool{New: func() any { + return &planeBuffers{plane: make(actor.PlaneContact, 0, 8), points: make([]constraint.ContactPoint, 0, 8)} +}} + +// collidePlane keeps the order of the pair: if the plane is body B, the normal is reversed. +// The points of the shape are reduced to 4 like the other contacts: the deepest first +func collidePlane(plane *actor.Plane, object *actor.RigidBody, margin float64, planeIsB bool, m *constraint.Manifold) bool { + buffers := planeContactsPool.Get().(*planeBuffers) + defer planeContactsPool.Put(buffers) + buffers.plane = object.Shape.CollideWithPlane(plane.Normal, plane.Distance, object.Transform, margin, buffers.plane[:0]) + if len(buffers.plane) == 0 { + return false + } - return ch + m.Normal = plane.Normal + if planeIsB { + m.Normal = plane.Normal.Mul(-1) + } + buffers.points = buffers.points[:0] + for _, p := range buffers.plane { + buffers.points = append(buffers.points, constraint.ContactPoint{Position: p.Position, Separation: p.Separation}) + } + epa.Reduce(buffers.points, m.Normal, m) + return m.Count > 0 } diff --git a/collision_capsule.go b/collision_capsule.go new file mode 100644 index 0000000..7608cd3 --- /dev/null +++ b/collision_capsule.go @@ -0,0 +1,247 @@ +package feather + +import ( + "math" + + "github.com/akmonengine/feather/actor" + "github.com/akmonengine/feather/constraint" + "github.com/go-gl/mathgl/mgl64" +) + +const ( + // segmentEpsilon: under this squared length, a segment is a point (also used to detect parallel segments) + segmentEpsilon = 1e-12 + + // parallelSinSquared: two capsules closer to parallel than ~1.1° touch along a line (2 points) + parallelSinSquared = 4e-4 + + // contactMergeDistance: under this overlap length, both points of a parallel contact are merged + contactMergeDistance = 1e-6 + + // normalEpsilon: under this distance between the closest points, their direction is not reliable for the normal + normalEpsilon = 1e-9 +) + +// CollideCapsuleCapsule uses the closest points of both segments. +// Parallel capsules get 2 contact points (both ends of the overlap), so they don't roll +func CollideCapsuleCapsule(a, b *actor.RigidBody, margin float64, manifold *constraint.Manifold) bool { + capsuleA, okA := a.Shape.(*actor.Capsule) + capsuleB, okB := b.Shape.(*actor.Capsule) + if !okA || !okB { + return false + } + + a0, a1 := capsuleA.Segment(a.Transform) + b0, b1 := capsuleB.Segment(b.Transform) + + manifold.Reset(a, b) + return collideSegments(a0, a1, capsuleA.Radius, b0, b1, capsuleB.Radius, + b.Transform.Position.Sub(a.Transform.Position), margin, manifold) +} + +// CollideCapsuleSphere uses the closest point of the segment to the center of the sphere +func CollideCapsuleSphere(capsule, sphere *actor.RigidBody, margin float64, manifold *constraint.Manifold) bool { + if _, ok := capsule.Shape.(*actor.Capsule); !ok { + return false + } + if _, ok := sphere.Shape.(*actor.Sphere); !ok { + return false + } + return collideAnalyticPair(capsule, sphere, margin, manifold) +} + +// segmentOf returns the segment and radius of a capsule, or of a sphere (a segment of length 0) +func segmentOf(body *actor.RigidBody) (mgl64.Vec3, mgl64.Vec3, float64, bool) { + switch shape := body.Shape.(type) { + case *actor.Capsule: + p0, p1 := shape.Segment(body.Transform) + return p0, p1, shape.Radius, true + case *actor.Sphere: + return body.Transform.Position, body.Transform.Position, shape.Radius, true + } + return mgl64.Vec3{}, mgl64.Vec3{}, 0, false +} + +// isAnalyticPair: capsules and spheres have an exact solution, no need for GJK/EPA +func isAnalyticPair(a, b actor.ShapeInterface) bool { + _, aIsCapsule := a.(*actor.Capsule) + _, bIsCapsule := b.(*actor.Capsule) + _, aIsSphere := a.(*actor.Sphere) + _, bIsSphere := b.(*actor.Sphere) + + return (aIsCapsule || aIsSphere) && (bIsCapsule || bIsSphere) +} + +func collideAnalyticPair(a, b *actor.RigidBody, margin float64, manifold *constraint.Manifold) bool { + a0, a1, radiusA, okA := segmentOf(a) + b0, b1, radiusB, okB := segmentOf(b) + if !okA || !okB { + return false + } + manifold.Reset(a, b) + return collideSegments(a0, a1, radiusA, b0, b1, radiusB, b.Transform.Position.Sub(a.Transform.Position), margin, manifold) +} + +// collideSegments is used for capsules and spheres: 2 segments with a radius. +// centerOffset (B - A) gives the normal when the segments intersect +func collideSegments(a0, a1 mgl64.Vec3, radiusA float64, b0, b1 mgl64.Vec3, radiusB float64, + centerOffset mgl64.Vec3, margin float64, manifold *constraint.Manifold) bool { + directionA := a1.Sub(a0) + directionB := b1.Sub(b0) + radii := radiusA + radiusB + + s, t := closestSegmentParameters(a0, directionA, b0, directionB) + closestA := a0.Add(directionA.Mul(s)) + closestB := b0.Add(directionB.Mul(t)) + + delta := closestB.Sub(closestA) + distanceSquared := delta.LenSqr() + if reach := radii + margin; distanceSquared > reach*reach { + return false + } + + distance := math.Sqrt(distanceSquared) + var normal mgl64.Vec3 + if distance > normalEpsilon { + normal = delta.Mul(1 / distance) + } else { + normal = fallbackNormal(directionA, directionB, centerOffset) + } + + manifold.Normal = normal + manifold.Count = 0 + + if areParallel(directionA, directionB) { + addParallelContacts(a0, directionA, radiusA, b0, directionB, radiusB, margin, manifold) + if manifold.Count > 0 { + return true + } + } + + addContact(manifold, closestA, closestB, normal, radiusA, radiusB, distance-radii) + return true +} + +// addParallelContacts adds a point at each end of the overlap of 2 parallel segments. +// Nothing if the overlap is too short (end to end): the caller adds the closest point +func addParallelContacts(a0, directionA mgl64.Vec3, radiusA float64, b0, directionB mgl64.Vec3, radiusB float64, + margin float64, manifold *constraint.Manifold) { + lengthSquaredA := directionA.LenSqr() + start := b0.Sub(a0).Dot(directionA) / lengthSquaredA + end := b0.Add(directionB).Sub(a0).Dot(directionA) / lengthSquaredA + + low := math.Max(0, math.Min(start, end)) + high := math.Min(1, math.Max(start, end)) + if (high-low)*math.Sqrt(lengthSquaredA) <= contactMergeDistance { + return + } + + radii := radiusA + radiusB + for _, parameter := range [2]float64{low, high} { + onA := a0.Add(directionA.Mul(parameter)) + onB := b0.Add(directionB.Mul(closestPointParameter(b0, directionB, onA))) + + separation := onB.Sub(onA).Dot(manifold.Normal) - radii + if separation <= margin { + addContact(manifold, onA, onB, manifold.Normal, radiusA, radiusB, separation) + } + } +} + +// addContact adds a point halfway between the surface of A and the surface of B +func addContact(manifold *constraint.Manifold, onA, onB, normal mgl64.Vec3, radiusA, radiusB, separation float64) { + position := onA.Add(onB).Mul(0.5).Add(normal.Mul((radiusA - radiusB) / 2)) + manifold.Add(position, separation) +} + +func areParallel(directionA, directionB mgl64.Vec3) bool { + lengthsSquared := directionA.LenSqr() * directionB.LenSqr() + if lengthsSquared <= segmentEpsilon { + return false + } + return directionA.Cross(directionB).LenSqr() <= parallelSinSquared*lengthsSquared +} + +// fallbackNormal when the segments touch or intersect: perpendicular to both axes if they cross, +// otherwise perpendicular to the axis of A, towards B +func fallbackNormal(directionA, directionB, centerOffset mgl64.Vec3) mgl64.Vec3 { + normal := directionA.Cross(directionB) + if normal.LenSqr() <= segmentEpsilon*directionA.LenSqr()*directionB.LenSqr() { + // Parallel axes, or a point: remove the part of the offset along the axis + normal = centerOffset + if lengthSquaredA := directionA.LenSqr(); lengthSquaredA > segmentEpsilon { + normal = normal.Sub(directionA.Mul(normal.Dot(directionA) / lengthSquaredA)) + } + if normal.LenSqr() <= segmentEpsilon { + normal = anyPerpendicular(directionA) + } + } + if normal.Dot(centerOffset) < 0 { + normal = normal.Mul(-1) + } + return normal.Normalize() +} + +// anyPerpendicular returns a unit vector orthogonal to v (+Y if v is null) +func anyPerpendicular(v mgl64.Vec3) mgl64.Vec3 { + if v.LenSqr() <= segmentEpsilon { + return mgl64.Vec3{0, 1, 0} + } + axis := mgl64.Vec3{1, 0, 0} + if math.Abs(v.X()) > math.Abs(v.Z()) { + axis = mgl64.Vec3{0, 0, 1} + } + return v.Cross(axis).Normalize() +} + +// closestPointParameter returns t in [0,1] of the point origin + t*direction closest to p +func closestPointParameter(origin, direction, p mgl64.Vec3) float64 { + lengthSquared := direction.LenSqr() + if lengthSquared <= segmentEpsilon { + return 0 + } + return clamp01(p.Sub(origin).Dot(direction) / lengthSquared) +} + +// closestSegmentParameters returns s & t in [0,1] of the closest points of p1 + s*d1 and p2 + t*d2 +// See Ericson, Real-Time Collision Detection, 5.1.9 +func closestSegmentParameters(p1, d1, p2, d2 mgl64.Vec3) (float64, float64) { + r := p1.Sub(p2) + a := d1.Dot(d1) + e := d2.Dot(d2) + f := d2.Dot(r) + + if a <= segmentEpsilon && e <= segmentEpsilon { + return 0, 0 + } + if a <= segmentEpsilon { + return 0, clamp01(f / e) + } + + c := d1.Dot(r) + if e <= segmentEpsilon { + return clamp01(-c / a), 0 + } + + b := d1.Dot(d2) + denominator := a*e - b*b + + // Parallel segments: any s is valid, we take the first end of A + s := 0.0 + if denominator > segmentEpsilon*a*e { + s = clamp01((b*f - c*e) / denominator) + } + + t := (b*s + f) / e + if t < 0 { + return clamp01(-c / a), 0 + } + if t > 1 { + return clamp01((b - c) / a), 1 + } + return s, t +} + +func clamp01(value float64) float64 { + return math.Max(0, math.Min(1, value)) +} diff --git a/collision_capsule_test.go b/collision_capsule_test.go new file mode 100644 index 0000000..196785b --- /dev/null +++ b/collision_capsule_test.go @@ -0,0 +1,569 @@ +package feather + +import ( + "math" + "sort" + "testing" + + "github.com/akmonengine/feather/actor" + "github.com/akmonengine/feather/constraint" + "github.com/akmonengine/feather/epa" + "github.com/akmonengine/feather/gjk" + "github.com/go-gl/mathgl/mgl64" +) + +// Rotations mapping the capsule's local Y axis onto a world axis. +var ( + capsuleAlongX = mgl64.QuatRotate(-math.Pi/2, mgl64.Vec3{0, 0, 1}) + capsuleAlongZ = mgl64.QuatRotate(math.Pi/2, mgl64.Vec3{1, 0, 0}) +) + +func createCapsule(position mgl64.Vec3, rotation mgl64.Quat, halfHeight, radius float64, bodyType actor.BodyType) *actor.RigidBody { + return actor.NewRigidBody( + actor.Transform{Position: position, Rotation: rotation}, + &actor.Capsule{HalfHeight: halfHeight, Radius: radius}, + bodyType, + 1.0, + ) +} + +func nearlyEqualVec(a, b mgl64.Vec3, tolerance float64) bool { + return a.Sub(b).Len() <= tolerance +} + +// sortedPoints orders contact points along an axis so tests do not depend on emission order. +func sortedPoints(points []constraint.ContactPoint, axis mgl64.Vec3) []constraint.ContactPoint { + sorted := append([]constraint.ContactPoint(nil), points...) + sort.Slice(sorted, func(i, j int) bool { + return sorted[i].Position.Dot(axis) < sorted[j].Position.Dot(axis) + }) + return sorted +} + +type expectedContact struct { + normal mgl64.Vec3 + points []constraint.ContactPoint // ordered along sortAxis +} + +func checkManifold(t *testing.T, m *constraint.Manifold, want expectedContact, sortAxis mgl64.Vec3, tolerance float64) { + t.Helper() + checkContact(t, m.Normal, m.Points[:m.Count], want, sortAxis, tolerance) +} + +func checkContact(t *testing.T, normal mgl64.Vec3, points []constraint.ContactPoint, want expectedContact, sortAxis mgl64.Vec3, tolerance float64) { + t.Helper() + if !nearlyEqualVec(normal, want.normal, tolerance) { + t.Errorf("normal = %v, want %v", normal, want.normal) + } + if len(points) != len(want.points) { + t.Fatalf("got %d points %v, want %d %v", len(points), points, len(want.points), want.points) + } + got := sortedPoints(points, sortAxis) + for i := range got { + if !nearlyEqualVec(got[i].Position, want.points[i].Position, tolerance) { + t.Errorf("point[%d] = %v, want %v", i, got[i].Position, want.points[i].Position) + } + if math.Abs(got[i].Separation-want.points[i].Separation) > tolerance { + t.Errorf("point[%d] separation = %.9f, want %.9f", i, got[i].Separation, want.points[i].Separation) + } + } +} + +func point(x, y, z, depth float64) constraint.ContactPoint { + return constraint.ContactPoint{Position: mgl64.Vec3{x, y, z}, Separation: -depth} +} + +// Contact points of the analytic paths sit halfway between the two surfaces. +func TestCollideCapsuleCapsule(t *testing.T) { + const tol = 1e-12 + yAxis := mgl64.Vec3{0, 1, 0} + + t.Run("side by side, parallel", func(t *testing.T) { + a := createCapsule(mgl64.Vec3{0, 0, 0}, mgl64.QuatIdent(), 1, 0.5, actor.BodyTypeDynamic) + b := createCapsule(mgl64.Vec3{0.9, 0, 0}, mgl64.QuatIdent(), 1, 0.5, actor.BodyTypeDynamic) + var m constraint.Manifold + if !CollideCapsuleCapsule(a, b, 0, &m) { + t.Fatal("no collision") + } + checkManifold(t, &m, expectedContact{ + normal: mgl64.Vec3{1, 0, 0}, + points: []constraint.ContactPoint{point(0.45, -1, 0, 0.1), point(0.45, 1, 0, 0.1)}, + }, yAxis, tol) + }) + + t.Run("parallel, partial overlap and different radii", func(t *testing.T) { + a := createCapsule(mgl64.Vec3{0, 0, 0}, mgl64.QuatIdent(), 1, 0.5, actor.BodyTypeDynamic) + b := createCapsule(mgl64.Vec3{0, 1.5, -0.7}, mgl64.QuatIdent(), 1, 0.3, actor.BodyTypeDynamic) + var m constraint.Manifold + if !CollideCapsuleCapsule(a, b, 0, &m) { + t.Fatal("no collision") + } + // Surfaces at z=-0.5 (A) and z=-0.4 (B): midpoint z=-0.45. Axial overlap y ∈ [0.5, 1]. + checkManifold(t, &m, expectedContact{ + normal: mgl64.Vec3{0, 0, -1}, + points: []constraint.ContactPoint{point(0, 0.5, -0.45, 0.1), point(0, 1, -0.45, 0.1)}, + }, yAxis, tol) + }) + + t.Run("crossed", func(t *testing.T) { + a := createCapsule(mgl64.Vec3{0, 0, 0}, mgl64.QuatIdent(), 1, 0.5, actor.BodyTypeDynamic) + b := createCapsule(mgl64.Vec3{0.9, 0.3, 0}, capsuleAlongZ, 1, 0.5, actor.BodyTypeDynamic) + var m constraint.Manifold + if !CollideCapsuleCapsule(a, b, 0, &m) { + t.Fatal("no collision") + } + checkManifold(t, &m, expectedContact{ + normal: mgl64.Vec3{1, 0, 0}, + points: []constraint.ContactPoint{point(0.45, 0.3, 0, 0.1)}, + }, yAxis, tol) + }) + + t.Run("end to end", func(t *testing.T) { + a := createCapsule(mgl64.Vec3{0, 0, 0}, mgl64.QuatIdent(), 1, 0.5, actor.BodyTypeDynamic) + b := createCapsule(mgl64.Vec3{0, 2.9, 0}, mgl64.QuatIdent(), 1, 0.5, actor.BodyTypeDynamic) + var m constraint.Manifold + if !CollideCapsuleCapsule(a, b, 0, &m) { + t.Fatal("no collision") + } + checkManifold(t, &m, expectedContact{ + normal: mgl64.Vec3{0, 1, 0}, + points: []constraint.ContactPoint{point(0, 1.45, 0, 0.1)}, + }, yAxis, tol) + }) + + t.Run("end against side (T)", func(t *testing.T) { + a := createCapsule(mgl64.Vec3{0, 0, 0}, capsuleAlongX, 1, 0.5, actor.BodyTypeDynamic) + b := createCapsule(mgl64.Vec3{0.2, 1.3, 0}, mgl64.QuatIdent(), 0.5, 0.4, actor.BodyTypeDynamic) + var m constraint.Manifold + if !CollideCapsuleCapsule(a, b, 0, &m) { + t.Fatal("no collision") + } + // B's lower end (0.2,0.8,0) is 0.8 above A's axis; radii sum 0.9. Surfaces at y=0.5 and y=0.4. + checkManifold(t, &m, expectedContact{ + normal: mgl64.Vec3{0, 1, 0}, + points: []constraint.ContactPoint{point(0.2, 0.45, 0, 0.1)}, + }, yAxis, tol) + }) + + t.Run("arbitrary pose, parallel", func(t *testing.T) { + rotation := mgl64.QuatRotate(0.7, mgl64.Vec3{1, 2, 3}.Normalize()) + offset := mgl64.Vec3{3, -2, 5} + a := createCapsule(offset, rotation, 1, 0.5, actor.BodyTypeDynamic) + b := createCapsule(offset.Add(rotation.Rotate(mgl64.Vec3{0.9, 0, 0})), rotation, 1, 0.5, actor.BodyTypeDynamic) + var m constraint.Manifold + if !CollideCapsuleCapsule(a, b, 0, &m) { + t.Fatal("no collision") + } + world := func(x, y, z, depth float64) constraint.ContactPoint { + return constraint.ContactPoint{Position: offset.Add(rotation.Rotate(mgl64.Vec3{x, y, z})), Separation: -depth} + } + checkManifold(t, &m, expectedContact{ + normal: rotation.Rotate(mgl64.Vec3{1, 0, 0}), + points: []constraint.ContactPoint{world(0.45, -1, 0, 0.1), world(0.45, 1, 0, 0.1)}, + }, rotation.Rotate(yAxis), 1e-9) + }) + + t.Run("axes intersect", func(t *testing.T) { + a := createCapsule(mgl64.Vec3{0, 0, 0}, mgl64.QuatIdent(), 1, 0.5, actor.BodyTypeDynamic) + b := createCapsule(mgl64.Vec3{0, 0, 0}, capsuleAlongZ, 1, 0.5, actor.BodyTypeDynamic) + var m constraint.Manifold + if !CollideCapsuleCapsule(a, b, 0, &m) { + t.Fatal("no collision") + } + if m.Count != 1 || math.Abs(-m.Points[0].Separation-1) > tol { + t.Errorf("manifold = %+v, want one point of depth 1", m) + } + if math.Abs(m.Normal.Len()-1) > tol || math.Abs(m.Normal.Y()) > tol || math.Abs(m.Normal.Z()) > tol { + t.Errorf("normal = %v, want a unit vector orthogonal to both axes", m.Normal) + } + }) + + t.Run("separated", func(t *testing.T) { + a := createCapsule(mgl64.Vec3{0, 0, 0}, mgl64.QuatIdent(), 1, 0.5, actor.BodyTypeDynamic) + for _, b := range []*actor.RigidBody{ + createCapsule(mgl64.Vec3{1.001, 0, 0}, mgl64.QuatIdent(), 1, 0.5, actor.BodyTypeDynamic), // 1 mm apart + createCapsule(mgl64.Vec3{0, 3.1, 0}, mgl64.QuatIdent(), 1, 0.5, actor.BodyTypeDynamic), + createCapsule(mgl64.Vec3{1.2, 0, 0}, capsuleAlongZ, 1, 0.5, actor.BodyTypeDynamic), + } { + var m constraint.Manifold + if CollideCapsuleCapsule(a, b, 0, &m) { + t.Errorf("capsule at %v: unexpected collision %+v", b.Transform.Position, m) + } + } + }) + + t.Run("touching: zero separation", func(t *testing.T) { + a := createCapsule(mgl64.Vec3{0, 0, 0}, mgl64.QuatIdent(), 1, 0.5, actor.BodyTypeDynamic) + b := createCapsule(mgl64.Vec3{1.0, 0, 0}, mgl64.QuatIdent(), 1, 0.5, actor.BodyTypeDynamic) + var m constraint.Manifold + if !CollideCapsuleCapsule(a, b, 0, &m) || m.Count != 2 || m.MinSeparation() != 0 { + t.Errorf("touching capsules: %+v, want 2 points at separation 0", m) + } + }) + + t.Run("speculative: within the margin only", func(t *testing.T) { + a := createCapsule(mgl64.Vec3{0, 0, 0}, mgl64.QuatIdent(), 1, 0.5, actor.BodyTypeDynamic) + b := createCapsule(mgl64.Vec3{1.01, 0, 0}, mgl64.QuatIdent(), 1, 0.5, actor.BodyTypeDynamic) + var m constraint.Manifold + if !CollideCapsuleCapsule(a, b, 0.02, &m) || math.Abs(m.MinSeparation()-0.01) > 1e-12 { + t.Errorf("capsules 1 cm apart with a 2 cm margin: %+v, want separation 0.01", m) + } + if CollideCapsuleCapsule(a, b, 0.005, &m) { + t.Errorf("capsules 1 cm apart with a 5 mm margin: unexpected contact %+v", m) + } + }) +} + +func TestCollideCapsuleSphere(t *testing.T) { + const tol = 1e-12 + yAxis := mgl64.Vec3{0, 1, 0} + capsule := createCapsule(mgl64.Vec3{0, 0, 0}, mgl64.QuatIdent(), 1, 0.5, actor.BodyTypeDynamic) + + t.Run("against the side", func(t *testing.T) { + sphere := createSphere(mgl64.Vec3{0.9, 0.3, 0}, 0.5, actor.BodyTypeDynamic) + var m constraint.Manifold + if !CollideCapsuleSphere(capsule, sphere, 0, &m) { + t.Fatal("no collision") + } + checkManifold(t, &m, expectedContact{ + normal: mgl64.Vec3{1, 0, 0}, + points: []constraint.ContactPoint{point(0.45, 0.3, 0, 0.1)}, + }, yAxis, tol) + }) + + t.Run("against the cap", func(t *testing.T) { + sphere := createSphere(mgl64.Vec3{0, -1.7, 0}, 0.3, actor.BodyTypeDynamic) + var m constraint.Manifold + if !CollideCapsuleSphere(capsule, sphere, 0, &m) { + t.Fatal("no collision") + } + // Surfaces at y=-1.5 (capsule) and y=-1.4 (sphere). + checkManifold(t, &m, expectedContact{ + normal: mgl64.Vec3{0, -1, 0}, + points: []constraint.ContactPoint{point(0, -1.45, 0, 0.1)}, + }, yAxis, tol) + }) + + t.Run("oblique on the cap", func(t *testing.T) { + direction := mgl64.Vec3{1, 1, 1}.Normalize() + sphere := createSphere(mgl64.Vec3{0, 1, 0}.Add(direction.Mul(0.8)), 0.4, actor.BodyTypeDynamic) + var m constraint.Manifold + if !CollideCapsuleSphere(capsule, sphere, 0, &m) { + t.Fatal("no collision") + } + mid := mgl64.Vec3{0, 1, 0}.Add(direction.Mul(0.45)) + checkManifold(t, &m, expectedContact{ + normal: direction, + points: []constraint.ContactPoint{{Position: mid, Separation: -0.1}}, + }, yAxis, 1e-12) + }) + + t.Run("centre on the axis", func(t *testing.T) { + sphere := createSphere(mgl64.Vec3{0, 0.2, 0}, 0.3, actor.BodyTypeDynamic) + var m constraint.Manifold + if !CollideCapsuleSphere(capsule, sphere, 0, &m) { + t.Fatal("no collision") + } + if m.Count != 1 || math.Abs(-m.Points[0].Separation-0.8) > tol { + t.Errorf("manifold = %+v, want one point of depth 0.8", m) + } + if math.Abs(m.Normal.Len()-1) > tol || math.Abs(m.Normal.Y()) > tol { + t.Errorf("normal = %v, want a unit vector orthogonal to the axis", m.Normal) + } + }) + + t.Run("separated", func(t *testing.T) { + sphere := createSphere(mgl64.Vec3{0, 2.001, 0}, 0.5, actor.BodyTypeDynamic) // 1 mm apart + var m constraint.Manifold + if CollideCapsuleSphere(capsule, sphere, 0, &m) { + t.Errorf("unexpected collision %+v", m) + } + }) +} + +// narrowPhaseOne runs the public narrow phase on a single pair. +func narrowPhaseOne(a, b *actor.RigidBody) []constraint.Manifold { + return NarrowPhase([]Pair{{BodyA: a, BodyB: b}}, 2) +} + +func TestNarrowPhaseCapsulePairs(t *testing.T) { + yAxis := mgl64.Vec3{0, 1, 0} + + t.Run("capsule-capsule uses the analytic path", func(t *testing.T) { + a := createCapsule(mgl64.Vec3{0, 0, 0}, mgl64.QuatIdent(), 1, 0.5, actor.BodyTypeDynamic) + b := createCapsule(mgl64.Vec3{0.9, 0, 0}, mgl64.QuatIdent(), 1, 0.5, actor.BodyTypeDynamic) + contacts := narrowPhaseOne(a, b) + if len(contacts) != 1 { + t.Fatalf("got %d contacts, want 1", len(contacts)) + } + c := contacts[0] + if c.BodyA != a || c.BodyB != b { + t.Errorf("bodies not preserved") + } + checkContact(t, c.Normal, c.Points[:c.Count], expectedContact{ + normal: mgl64.Vec3{1, 0, 0}, + points: []constraint.ContactPoint{point(0.45, -1, 0, 0.1), point(0.45, 1, 0, 0.1)}, + }, yAxis, 1e-12) + }) + + t.Run("sphere-capsule keeps the A to B normal", func(t *testing.T) { + sphere := createSphere(mgl64.Vec3{0.9, 0.3, 0}, 0.5, actor.BodyTypeDynamic) + capsule := createCapsule(mgl64.Vec3{0, 0, 0}, mgl64.QuatIdent(), 1, 0.5, actor.BodyTypeDynamic) + contacts := narrowPhaseOne(sphere, capsule) + if len(contacts) != 1 { + t.Fatalf("got %d contacts, want 1", len(contacts)) + } + c := contacts[0] + if c.BodyA != sphere || c.BodyB != capsule { + t.Errorf("bodies not preserved") + } + checkContact(t, c.Normal, c.Points[:c.Count], expectedContact{ + normal: mgl64.Vec3{-1, 0, 0}, + points: []constraint.ContactPoint{point(0.45, 0.3, 0, 0.1)}, + }, yAxis, 1e-12) + }) + + t.Run("capsule lying on a plane", func(t *testing.T) { + // The spatial grid always emits the plane as BodyA. + plane := createPlane(mgl64.Vec3{0, 1, 0}, 0) + capsule := createCapsule(mgl64.Vec3{0, 0.45, 0}, capsuleAlongX, 1, 0.5, actor.BodyTypeDynamic) + contacts := narrowPhaseOne(plane, capsule) + if len(contacts) != 1 { + t.Fatalf("got %d contacts, want 1", len(contacts)) + } + checkContact(t, contacts[0].Normal, contacts[0].Points[:contacts[0].Count], expectedContact{ + normal: mgl64.Vec3{0, 1, 0}, // from the plane (A) to the capsule (B) + // Halfway between the capsule surface (y=-0.05) and the plane. + points: []constraint.ContactPoint{point(-1, -0.025, 0, 0.05), point(1, -0.025, 0, 0.05)}, + }, mgl64.Vec3{1, 0, 0}, 1e-12) + }) +} + +// Capsule against box goes through GJK/EPA: EPA converges to EPAConvergenceTolerance. +func TestNarrowPhaseCapsuleBox(t *testing.T) { + const tol = 1e-6 + xAxis := mgl64.Vec3{1, 0, 0} + + t.Run("lying on the top face", func(t *testing.T) { + box := createBox(mgl64.Vec3{0, 0, 0}, mgl64.Vec3{2, 0.5, 2}, actor.BodyTypeStatic) + capsule := createCapsule(mgl64.Vec3{0.3, 0.9, 0.2}, capsuleAlongX, 1, 0.5, actor.BodyTypeDynamic) + contacts := narrowPhaseOne(box, capsule) + if len(contacts) != 1 { + t.Fatalf("got %d contacts, want 1", len(contacts)) + } + // Deepest line of the capsule: y=0.4, x ∈ [-0.7, 1.3]; the points lie halfway to the + // box face (y=0.5). + checkContact(t, contacts[0].Normal, contacts[0].Points[:contacts[0].Count], expectedContact{ + normal: mgl64.Vec3{0, 1, 0}, + points: []constraint.ContactPoint{point(-0.7, 0.45, 0.2, 0.1), point(1.3, 0.45, 0.2, 0.1)}, + }, xAxis, tol) + }) + + t.Run("standing on the top face, capsule first", func(t *testing.T) { + capsule := createCapsule(mgl64.Vec3{0.5, 1.9, -0.5}, mgl64.QuatIdent(), 1, 0.5, actor.BodyTypeDynamic) + box := createBox(mgl64.Vec3{0, 0, 0}, mgl64.Vec3{2, 0.5, 2}, actor.BodyTypeStatic) + contacts := narrowPhaseOne(capsule, box) + if len(contacts) != 1 { + t.Fatalf("got %d contacts, want 1", len(contacts)) + } + checkContact(t, contacts[0].Normal, contacts[0].Points[:contacts[0].Count], expectedContact{ + normal: mgl64.Vec3{0, -1, 0}, + points: []constraint.ContactPoint{point(0.5, 0.45, -0.5, 0.1)}, + }, xAxis, tol) + }) + + t.Run("end against a side face", func(t *testing.T) { + box := createBox(mgl64.Vec3{0, 0, 0}, mgl64.Vec3{1, 1, 1}, actor.BodyTypeStatic) + capsule := createCapsule(mgl64.Vec3{2.4, 0.2, 0.1}, capsuleAlongX, 1, 0.5, actor.BodyTypeDynamic) + contacts := narrowPhaseOne(box, capsule) + if len(contacts) != 1 { + t.Fatalf("got %d contacts, want 1", len(contacts)) + } + checkContact(t, contacts[0].Normal, contacts[0].Points[:contacts[0].Count], expectedContact{ + normal: mgl64.Vec3{1, 0, 0}, + points: []constraint.ContactPoint{point(0.95, 0.2, 0.1, 0.1)}, + }, xAxis, tol) + }) + + t.Run("crossed over an edge", func(t *testing.T) { + // Capsule along Z resting across the top-right edge of a box, at 45°. + box := createBox(mgl64.Vec3{0, 0, 0}, mgl64.Vec3{1, 1, 1}, actor.BodyTypeStatic) + direction := mgl64.Vec3{1, 1, 0}.Normalize() + center := mgl64.Vec3{1, 1, 0}.Add(direction.Mul(0.4)) + capsule := createCapsule(center, capsuleAlongZ, 2, 0.5, actor.BodyTypeDynamic) + contacts := narrowPhaseOne(box, capsule) + if len(contacts) != 1 { + t.Fatalf("got %d contacts, want 1", len(contacts)) + } + c := contacts[0] + // On a rounded surface, EPA's distance tolerance bounds the normal error to + // acos(1 - tol/radius), well under 0.1° here. + maxAngle := math.Acos(1 - epa.EPAConvergenceTolerance/0.5) + if mgl64.RadToDeg(maxAngle) > 0.1 { + t.Fatalf("EPA tolerance allows %.3f°, want < 0.1°", mgl64.RadToDeg(maxAngle)) + } + if angle := math.Acos(math.Min(1, c.Normal.Dot(direction))); angle > maxAngle { + t.Errorf("normal = %v, %.2f° from %v (max %.2f°)", c.Normal, mgl64.RadToDeg(angle), direction, mgl64.RadToDeg(maxAngle)) + } + if c.Count != 2 { + t.Fatalf("got %d points %v, want the capsule line clipped to the box (2 points)", c.Count, c.Points) + } + zs := []float64{c.Points[0].Position.Z(), c.Points[1].Position.Z()} + sort.Float64s(zs) + if math.Abs(zs[0]+1) > 1e-6 || math.Abs(zs[1]-1) > 1e-6 { + t.Errorf("points z = %v, want the capsule line clipped to the box, [-1 1]", zs) + } + for _, p := range c.Points[:c.Count] { + if math.Abs(p.Separation+0.1) > tol { + t.Errorf("separation = %f, want -0.1", p.Separation) + } + // The points lie on the capsule's deepest line, within the depth of the edge x=y=1. + edgeDistance := mgl64.Vec3{p.Position.X() - 1, p.Position.Y() - 1, 0}.Len() + if edgeDistance > 0.1+tol { + t.Errorf("point %v is %.4f from the box edge, want <= 0.1", p.Position, edgeDistance) + } + } + }) +} + +// The GJK/EPA general path must agree with the analytic kernels. +func TestCapsuleCapsuleAnalyticMatchesGJK(t *testing.T) { + poses := capsuleBenchPoses() + for i, pose := range poses { + var m constraint.Manifold + analytic := CollideCapsuleCapsule(pose.a, pose.b, 0, &m) + + simplex := &gjk.Simplex{} + general := gjk.GJK(pose.a, pose.b, simplex) + if analytic != general { + t.Errorf("pose %d: analytic collision %v, GJK %v", i, analytic, general) + continue + } + if !general { + continue + } + result, err := epa.EPA(pose.a, pose.b, simplex, 0) + if err != nil { + t.Errorf("pose %d: EPA error %v", i, err) + continue + } + if !nearlyEqualVec(result.Normal, m.Normal, 1e-3) { + t.Errorf("pose %d: EPA normal %v, analytic %v", i, result.Normal, m.Normal) + } + if math.Abs(result.Depth+m.MinSeparation()) > 2*epa.EPAConvergenceTolerance { + t.Errorf("pose %d: EPA depth %f, analytic %f", i, result.Depth, -m.MinSeparation()) + } + } +} + +func TestCapsuleAnalyticDoesNotAllocate(t *testing.T) { + poses := capsuleBenchPoses() + sphere := createSphere(mgl64.Vec3{0.9, 0.3, 0}, 0.5, actor.BodyTypeDynamic) + var m constraint.Manifold + + allocs := testing.AllocsPerRun(100, func() { + for _, pose := range poses { + CollideCapsuleCapsule(pose.a, pose.b, 0, &m) + CollideCapsuleSphere(pose.a, sphere, 0, &m) + } + }) + if allocs != 0 { + t.Errorf("analytic capsule kernels allocate %.1f times per run, want 0", allocs) + } +} + +// simulateCapsuleOnPlane drops nothing: the capsule starts exactly resting on the ground +// and the maximum displacement from that pose over the duration is returned. +func simulateCapsuleOnPlane(t *testing.T, rotation mgl64.Quat, restingHeight float64, seconds float64) (maxDrift float64, finalAxis mgl64.Vec3) { + t.Helper() + world := World{ + Gravity: mgl64.Vec3{0, -9.81, 0}, + Substeps: 10, + Workers: 1, + Events: NewEvents(), + } + world.AddBody(createPlane(mgl64.Vec3{0, 1, 0}, 0)) + start := mgl64.Vec3{0.25, restingHeight, -0.5} + capsule := createCapsule(start, rotation, 0.6, 0.3, actor.BodyTypeDynamic) + world.AddBody(capsule) + + const dt = 1.0 / 60.0 + for step := 0; step < int(seconds/dt); step++ { + world.Step(dt) + maxDrift = math.Max(maxDrift, capsule.Transform.Position.Sub(start).Len()) + } + return maxDrift, capsule.Transform.Rotation.Rotate(mgl64.Vec3{0, 1, 0}) +} + +func TestCapsuleRestsOnPlane(t *testing.T) { + const maxDrift = 1e-4 // 0.1 mm + + t.Run("upright", func(t *testing.T) { + drift, axis := simulateCapsuleOnPlane(t, mgl64.QuatIdent(), 0.6+0.3, 10) + if !(drift < maxDrift) { // also rejects NaN + t.Errorf("upright capsule drifted %.3e m over 10 s, want < %.0e", drift, maxDrift) + } + if !(axis.Y() >= 1-1e-9) { + t.Errorf("upright capsule tilted: axis %v", axis) + } + }) + + t.Run("lying", func(t *testing.T) { + drift, axis := simulateCapsuleOnPlane(t, capsuleAlongX, 0.3, 10) + if !(drift < maxDrift) { // also rejects NaN + t.Errorf("lying capsule drifted %.3e m over 10 s, want < %.0e", drift, maxDrift) + } + if !(math.Abs(axis.Y()) <= 1e-6) { + t.Errorf("lying capsule tilted: axis %v", axis) + } + }) +} + +type capsulePose struct { + name string + a, b *actor.RigidBody +} + +// capsuleBenchPoses covers the contact configurations of the acceptance criteria. +func capsuleBenchPoses() []capsulePose { + tilted := mgl64.QuatRotate(0.4, mgl64.Vec3{1, 0, 1}.Normalize()) + return []capsulePose{ + {"parallel", + createCapsule(mgl64.Vec3{0, 0, 0}, mgl64.QuatIdent(), 1, 0.5, actor.BodyTypeDynamic), + createCapsule(mgl64.Vec3{0.9, 0.2, 0}, mgl64.QuatIdent(), 1, 0.5, actor.BodyTypeDynamic)}, + {"crossed", + createCapsule(mgl64.Vec3{0, 0, 0}, mgl64.QuatIdent(), 1, 0.5, actor.BodyTypeDynamic), + createCapsule(mgl64.Vec3{0.9, 0.3, 0}, capsuleAlongZ, 1, 0.5, actor.BodyTypeDynamic)}, + {"end to end", + createCapsule(mgl64.Vec3{0, 0, 0}, mgl64.QuatIdent(), 1, 0.5, actor.BodyTypeDynamic), + createCapsule(mgl64.Vec3{0.05, 2.9, 0}, mgl64.QuatIdent(), 1, 0.5, actor.BodyTypeDynamic)}, + {"tilted", + createCapsule(mgl64.Vec3{0, 0, 0}, capsuleAlongX, 1, 0.4, actor.BodyTypeDynamic), + createCapsule(mgl64.Vec3{0.3, 0.7, 0.1}, tilted, 0.8, 0.4, actor.BodyTypeDynamic)}, + {"separated", + createCapsule(mgl64.Vec3{0, 0, 0}, mgl64.QuatIdent(), 1, 0.5, actor.BodyTypeDynamic), + createCapsule(mgl64.Vec3{1.5, 0, 0}, capsuleAlongZ, 1, 0.5, actor.BodyTypeDynamic)}, + } +} + +func BenchmarkCapsuleCapsule(b *testing.B) { + for _, pose := range capsuleBenchPoses() { + b.Run("analytic/"+pose.name, func(b *testing.B) { + var m constraint.Manifold + b.ReportAllocs() + for i := 0; i < b.N; i++ { + CollideCapsuleCapsule(pose.a, pose.b, 0, &m) + } + }) + b.Run("gjk-epa/"+pose.name, func(b *testing.B) { + simplex := &gjk.Simplex{} + b.ReportAllocs() + for i := 0; i < b.N; i++ { + simplex.Reset() + if gjk.GJK(pose.a, pose.b, simplex) { + _, _ = epa.EPA(pose.a, pose.b, simplex, 0) + } + } + }) + } +} diff --git a/collision_core_test.go b/collision_core_test.go new file mode 100644 index 0000000..49d2e87 --- /dev/null +++ b/collision_core_test.go @@ -0,0 +1,86 @@ +package feather + +import ( + "math" + "math/rand" + "testing" + + "github.com/akmonengine/feather/actor" + "github.com/akmonengine/feather/constraint" + "github.com/go-gl/mathgl/mgl64" +) + +// closestSphereBox: the exact contact of a sphere with a box whose center is outside the box, the separation and the +// normal from the sphere towards the box +func closestSphereBox(sphere, box *actor.RigidBody) (float64, mgl64.Vec3, bool) { + radius := sphere.Shape.(*actor.Sphere).Radius + h := box.Shape.(*actor.Box).HalfExtents + c := box.Transform.ToLocal(sphere.Transform.Position) + q := mgl64.Vec3{math.Max(-h[0], math.Min(h[0], c[0])), math.Max(-h[1], math.Min(h[1], c[1])), math.Max(-h[2], math.Min(h[2], c[2]))} + if q == c { + return 0, mgl64.Vec3{}, false + } + return c.Sub(q).Len() - radius, box.Transform.Rotation.Rotate(q.Sub(c).Normalize()), true +} + +// A sphere against a box is its center (a core) with a radius: the separation and the normal are those of the closest +// point of the box to the center, exact to the rounding, on the faces, the edges and the corners, within the margin +func TestSphereBoxIsExact(t *testing.T) { + r := rand.New(rand.NewSource(12)) + var m constraint.Manifold + tested := 0 + for i := 0; i < 400; i++ { + box := actor.NewRigidBody(actor.Transform{Rotation: mgl64.QuatRotate(r.Float64()*math.Pi, mgl64.Vec3{r.NormFloat64(), r.NormFloat64(), r.NormFloat64()}.Normalize())}, + &actor.Box{HalfExtents: mgl64.Vec3{0.1 + 0.5*r.Float64(), 0.1 + 0.5*r.Float64(), 0.1 + 0.5*r.Float64()}}, actor.BodyTypeDynamic, 1) + direction := mgl64.Vec3{r.NormFloat64(), r.NormFloat64(), r.NormFloat64()}.Normalize() + sphere := createSphere(direction.Mul(0.5+0.6*r.Float64()), 0.1+0.3*r.Float64(), actor.BodyTypeDynamic) + want, wantNormal, outside := closestSphereBox(sphere, box) + if !outside || want > SpeculativeDistance { + continue + } + tested++ + if !Collide(sphere, box, SpeculativeDistance, &m) || m.Count != 1 { + t.Fatalf("pair %d: no contact, separation %.4f", i, want) + } + if math.Abs(m.Points[0].Separation-want) > 1e-9 { + t.Fatalf("pair %d: separation %.12f, want %.12f", i, m.Points[0].Separation, want) + } + if m.Normal.Sub(wantNormal).Len() > 1e-9 { + t.Fatalf("pair %d: normal %v, want %v", i, m.Normal, wantNormal) + } + } + if tested < 100 { + t.Fatalf("only %d pairs tested", tested) + } +} + +// The center of the sphere inside the box: the cores overlap, EPA gives the shallowest way out +func TestSphereInsideBox(t *testing.T) { + box := createBox(mgl64.Vec3{}, mgl64.Vec3{1, 0.5, 1}, actor.BodyTypeStatic) + sphere := createSphere(mgl64.Vec3{0.2, 0.3, -0.1}, 0.25, actor.BodyTypeDynamic) + var m constraint.Manifold + if !Collide(sphere, box, 0, &m) { + t.Fatal("no contact") + } + if want := -(0.5 - 0.3 + 0.25); math.Abs(m.Points[0].Separation-want) > 1e-6 || m.Normal.Sub(mgl64.Vec3{0, -1, 0}).Len() > 1e-6 { + t.Errorf("separation %.6f normal %v, want %.6f and (0 -1 0)", m.Points[0].Separation, m.Normal, want) + } +} + +// The cores of the rounded shapes don't allocate: the core is the shape itself, seen through another type +func TestCoresDoNotAllocate(t *testing.T) { + box := createBox(mgl64.Vec3{}, mgl64.Vec3{0.5, 0.5, 0.5}, actor.BodyTypeStatic) + sphere := createSphere(mgl64.Vec3{0.3, 0.74, 0.2}, 0.25, actor.BodyTypeDynamic) + capsule := createCapsule(mgl64.Vec3{0, 0.8, 0}, mgl64.QuatRotate(math.Pi/2, mgl64.Vec3{0, 0, 1}), 0.4, 0.3, actor.BodyTypeDynamic) + var m constraint.Manifold + allocs := testing.AllocsPerRun(100, func() { + Collide(sphere, box, SpeculativeDistance, &m) + Collide(box, capsule, SpeculativeDistance, &m) + }) + if allocs != 0 { + t.Errorf("the rounded shapes against a box allocate %.1f times per run, want 0", allocs) + } + if !Collide(box, capsule, SpeculativeDistance, &m) || m.Count != 2 { + t.Errorf("the capsule lying on the box has %d points, want 2", m.Count) + } +} diff --git a/collision_heightfield.go b/collision_heightfield.go new file mode 100644 index 0000000..49e6dbf --- /dev/null +++ b/collision_heightfield.go @@ -0,0 +1,330 @@ +package feather + +import ( + "math" + "slices" + "sync" + + "github.com/akmonengine/feather/actor" + "github.com/akmonengine/feather/constraint" + "github.com/akmonengine/feather/epa" + "github.com/akmonengine/feather/gjk" + "github.com/go-gl/mathgl/mgl64" +) + +const ( + // MaxManifoldsPerPair: a body on a terrain touches it with 8 normals at most (8 patches): + // on a rough terrain, both ends of a capsule can touch 4 triangles each + MaxManifoldsPerPair = 8 + + // patchCos: the contacts of 2 triangles whose normals differ by less than 5° are in the same patch: cos(5°) + patchCos = 0.99619469809174553229501040247389 + + // triangleFaceCos: the contact of a triangle is a face contact if its normal is the normal of the triangle (0.5°) + triangleFaceCos = 0.99996 + + // edgeBarycentric: a point on a triangle with a barycentric coordinate under this value is on an edge + edgeBarycentric = 1e-3 + + // weldDistance: 2 points of a patch closer than this distance are the same point (m) + weldDistance = 1e-4 + + // insideTriangle: a point on a side of a triangle is inside (barycentric coordinate) + insideTriangle = -1e-9 +) + +// triangleShape is a triangle of a heightfield, in world space: its body has the identity transform +type triangleShape struct { + vertices [3]mgl64.Vec3 + aabb actor.AABB +} + +// ComputeAABB: the vertices are in world space +func (t *triangleShape) ComputeAABB(transform actor.Transform) actor.AABB { + return t.aabb +} + +func (t *triangleShape) ComputeMass(density float64) float64 { + return math.Inf(1) +} + +func (t *triangleShape) ComputeInertia(mass float64) mgl64.Mat3 { + return mgl64.Mat3{} +} + +func (t *triangleShape) Support(direction mgl64.Vec3) mgl64.Vec3 { + best := 0 + for i := 1; i < 3; i++ { + if t.vertices[i].Dot(direction) > t.vertices[best].Dot(direction) { + best = i + } + } + return t.vertices[best] +} + +// GetContactFeature: the face, the manifold keeps its deepest edge or vertex if it is not aligned with the normal +func (t *triangleShape) GetContactFeature(direction mgl64.Vec3, output *[8]mgl64.Vec3, count *int) { + copy(output[:3], t.vertices[:]) + *count = 3 +} + +func (t *triangleShape) CollideWithPlane(planeNormal mgl64.Vec3, planeDistance float64, myTransform actor.Transform, margin float64, contacts actor.PlaneContact) actor.PlaneContact { + return contacts +} + +// triangleContact: the points of a triangle, in heightfieldScratch.points, with their normal (from the terrain to the body). +// A witness contact is only used if its patch has no other contact +type triangleContact struct { + normal mgl64.Vec3 + first int + count int + separation float64 + witness bool +} + +// heightfieldScratch: the buffers of a collision with a heightfield, reused to avoid the allocations +type heightfieldScratch struct { + shape triangleShape + triangle actor.RigidBody + simplex gjk.Simplex + manifold constraint.Manifold + cells []int32 + plane actor.PlaneContact + contacts []triangleContact + points []constraint.ContactPoint + patches [MaxManifoldsPerPair][]constraint.ContactPoint +} + +var heightfieldPool = sync.Pool{New: func() any { + s := &heightfieldScratch{} + s.triangle = actor.RigidBody{Transform: actor.NewTransform(), BodyType: actor.BodyTypeStatic, Shape: &s.shape} + return s +}} + +// collideHeightfield writes in out the patches of contact between the terrain and the body, and returns their count. +// Each triangle under the body is tested with GJK/EPA. A contact on an inactive edge (between 2 triangles almost flat, +// or bent inwards) takes the normal of its triangle: a body sliding on the terrain doesn't hit the inner edges. +// A contact with the face of a triangle is the contact with its plane (CollideWithPlane of the shape), limited to +// the triangle: on a flat terrain, the bodies behave exactly as on a plane. +// The contacts are then grouped by normal: one patch (a manifold of 4 points) per normal. +// The order of the pair is kept: if the terrain is B, the normals point from the body to the terrain +func collideHeightfield(terrain *actor.RigidBody, field *actor.Heightfield, object *actor.RigidBody, margin float64, terrainIsB bool, out []constraint.Manifold) int { + s := heightfieldPool.Get().(*heightfieldScratch) + defer heightfieldPool.Put(s) + + bounds := object.AABB() + bounds = actor.AABB{Min: bounds.Min.Sub(mgl64.Vec3{margin, margin, margin}), Max: bounds.Max.Add(mgl64.Vec3{margin, margin, margin})} + s.cells = field.OverlapCells(localBounds(terrain.Transform, bounds), s.cells[:0]) + s.contacts, s.points = s.contacts[:0], s.points[:0] + + cellsZ := field.ZSamples - 1 + for _, cell := range s.cells { + x, z := int(cell)/cellsZ, int(cell)%cellsZ + for t := 0; t < 2; t++ { + local, edges := field.Triangle(x, z, t) + for i := range local { + s.shape.vertices[i] = terrain.Transform.ToWorld(local[i]) + } + s.shape.aabb = triangleAABB(s.shape.vertices) + if s.shape.aabb.Overlaps(bounds) { + s.collideTriangle(object, edges, margin) + } + } + } + if len(s.contacts) == 0 { + return 0 + } + + // ========== PATCHES ========== + // the deepest contacts first: they give the normal of their patch, the shallowest are dropped (as in Jolt) + slices.SortStableFunc(s.contacts, func(a, b triangleContact) int { + switch { + case a.separation < b.separation: + return -1 + case a.separation > b.separation: + return 1 + } + return 0 + }) + var normals [MaxManifoldsPerPair]mgl64.Vec3 + var witnesses [MaxManifoldsPerPair]int + patches := 0 + for c, contact := range s.contacts { + best, bestCos := -1, math.Inf(-1) + for k := 0; k < patches; k++ { + if cos := normals[k].Dot(contact.normal); cos > bestCos { + best, bestCos = k, cos + } + } + if bestCos < patchCos { + // a new normal: a new patch, unless the deepest patches are already found + if patches == min(len(out), MaxManifoldsPerPair) { + continue + } + best = patches + normals[best] = contact.normal + s.patches[best] = s.patches[best][:0] + witnesses[best] = -1 + patches++ + } + if contact.witness { + // the deepest witness contact of the patch, in case it has no other contact + if witnesses[best] < 0 { + witnesses[best] = c + } + continue + } + for _, point := range s.points[contact.first : contact.first+contact.count] { + s.patches[best] = weld(s.patches[best], point) + } + } + for k := 0; k < patches; k++ { + if len(s.patches[k]) == 0 { + witness := s.contacts[witnesses[k]] + s.patches[k] = append(s.patches[k], s.points[witness.first]) + } + } + + for k := 0; k < patches; k++ { + m := &out[k] + m.Reset(terrain, object) + m.Normal = normals[k] + if terrainIsB { + m.Reset(object, terrain) + m.Normal = normals[k].Mul(-1) + } + epa.Reduce(s.patches[k], normals[k], m) + } + return patches +} + +// collideTriangle adds the contact of the body with the triangle of s.shape +func (s *heightfieldScratch) collideTriangle(object *actor.RigidBody, edges uint8, margin float64) { + result, ok := penetration(&s.triangle, object, margin, &s.simplex) + if !ok { + return + } + + vertices := s.shape.vertices + faceNormal := vertices[1].Sub(vertices[0]).Cross(vertices[2].Sub(vertices[0])).Normalize() + + // ========== FACE ========== + // the points of the body above the triangle, closer to its plane than the margin + first := len(s.points) + s.plane = object.Shape.CollideWithPlane(faceNormal, -faceNormal.Dot(vertices[0]), object.Transform, margin, s.plane[:0]) + for _, point := range s.plane { + if u, v, w := barycentric(point.Position, vertices[0], vertices[1], vertices[2]); u >= insideTriangle && v >= insideTriangle && w >= insideTriangle { + s.points = append(s.points, constraint.ContactPoint{Position: point.Position, Separation: point.Separation}) + } + } + faceFound := s.addContact(faceNormal, first, false) + + // ========== EDGE ========== + // the body touches an active edge or vertex from above: the contact of EPA, with its normal + onTriangle := result.WitnessA.Sub(result.Normal.Mul(margin)) + cos := result.Normal.Dot(faceNormal) + if cos < triangleFaceCos && cos > 0 && touchesEdge(vertices, onTriangle, edges&0b111) { + first = len(s.points) + epa.Manifold(&s.triangle, object, result, margin, &s.manifold) + s.points = append(s.points, s.manifold.Points[:s.manifold.Count]...) + s.addContact(result.Normal, first, false) + return + } + + if !faceFound { + // the body is beside the triangle, above an inactive edge: the witness point, with the normal of the face, + // only used if no other triangle has a contact with this normal + first = len(s.points) + onB := result.WitnessB + s.points = append(s.points, constraint.ContactPoint{Position: onTriangle.Add(onB).Mul(0.5), Separation: margin - result.Depth}) + s.addContact(faceNormal, first, true) + } +} + +// addContact: the points from first to the end of s.points, with their normal. Returns false if there is no point +func (s *heightfieldScratch) addContact(normal mgl64.Vec3, first int, witness bool) bool { + count := len(s.points) - first + if count == 0 { + return false + } + deepest := math.Inf(1) + for _, point := range s.points[first:] { + deepest = math.Min(deepest, point.Separation) + } + s.contacts = append(s.contacts, triangleContact{normal: normal, first: first, count: count, separation: deepest, witness: witness}) + return true +} + +// touchesEdge: the point is on one of the edges of the triangle (bit e for the edge from the vertex e to the vertex e+1), +// or on a vertex of these edges +func touchesEdge(vertices [3]mgl64.Vec3, p mgl64.Vec3, edges uint8) bool { + u, v, w := barycentric(p, vertices[0], vertices[1], vertices[2]) + weights := [3]float64{u, v, w} + for e := 0; e < 3; e++ { + // the edge e goes from the vertex e to the vertex e+1: the weight of the opposite vertex is 0 + if weights[(e+2)%3] <= edgeBarycentric && edges&(1< 1e-9 { + t.Fatalf("rotation %v: deepest point %.4f, the contact has %.4f", rotation, lowest, m.MinSeparation()) + } + } } diff --git a/constraint/constraint.go b/constraint/constraint.go index 53edcfe..d39e379 100644 --- a/constraint/constraint.go +++ b/constraint/constraint.go @@ -4,41 +4,27 @@ import ( "math" "github.com/akmonengine/feather/actor" - "github.com/go-gl/mathgl/mgl64" ) -type Constraint interface { - SolvePosition(dt float64) - SolveVelocity(dt float64) -} +var posInf = math.Inf(1) func ComputeRestitution(matA, matB actor.Material) float64 { - // Option 1: Average (more realistic) + // Average return (matA.Restitution + matB.Restitution) / 2.0 - - // Option 2: Maximum (if one bounces, it bounces) - //return math.Max(matA.Restitution, matB.Restitution) - - // Option 3: Geometric mean (Box2D approach) - // return math.Sqrt(matA.Restitution * matB.Restitution) } func ComputeStaticFriction(matA, matB actor.Material) float64 { - // Moyenne géométrique (standard en physique) + // Geometric mean return math.Sqrt(matA.StaticFriction * matB.StaticFriction) } func ComputeDynamicFriction(matA, matB actor.Material) float64 { + // Geometric mean return math.Sqrt(matA.DynamicFriction * matB.DynamicFriction) } -func clampSmallVelocities(rb *actor.RigidBody) { - const velocityThreshold = 1e-5 - - if rb.Velocity.Len() < velocityThreshold { - rb.Velocity = mgl64.Vec3{0, 0, 0} - } - if rb.AngularVelocity.Len() < velocityThreshold { - rb.AngularVelocity = mgl64.Vec3{0, 0, 0} - } +// ComputeRollingResistance is the largest rolling resistance of both materials, times the largest radius +// of both shapes (0 for a box): it limits the torque that stops the rolling (as in Box2D) +func ComputeRollingResistance(matA, matB actor.Material, radiusA, radiusB float64) float64 { + return math.Max(matA.RollingResistance, matB.RollingResistance) * math.Max(radiusA, radiusB) } diff --git a/constraint/constraint_test.go b/constraint/constraint_test.go index 96989b1..44870ce 100644 --- a/constraint/constraint_test.go +++ b/constraint/constraint_test.go @@ -5,7 +5,6 @@ import ( "testing" "github.com/akmonengine/feather/actor" - "github.com/go-gl/mathgl/mgl64" ) func TestComputeRestitution(t *testing.T) { @@ -76,80 +75,3 @@ func TestComputeRestitution(t *testing.T) { }) } } - -func TestClampSmallVelocities(t *testing.T) { - tests := []struct { - name string - initialVelocity mgl64.Vec3 - expectedVelocity mgl64.Vec3 - shouldClamp bool - }{ - { - name: "zero velocity stays zero", - initialVelocity: mgl64.Vec3{0, 0, 0}, - expectedVelocity: mgl64.Vec3{0, 0, 0}, - shouldClamp: true, - }, - { - name: "very small velocity gets clamped", - initialVelocity: mgl64.Vec3{1e-9, 1e-9, 1e-9}, - expectedVelocity: mgl64.Vec3{0, 0, 0}, - shouldClamp: true, - }, - { - name: "velocity at threshold gets clamped", - initialVelocity: mgl64.Vec3{5e-9, 5e-9, 0}, - expectedVelocity: mgl64.Vec3{0, 0, 0}, - shouldClamp: true, - }, - { - name: "normal velocity is not clamped", - initialVelocity: mgl64.Vec3{1.0, 2.0, 3.0}, - expectedVelocity: mgl64.Vec3{1.0, 2.0, 3.0}, - shouldClamp: false, - }, - { - name: "small but above threshold velocity is not clamped", - initialVelocity: mgl64.Vec3{2e-5, 0, 0}, - expectedVelocity: mgl64.Vec3{2e-5, 0, 0}, - shouldClamp: false, - }, - { - name: "negative velocity gets clamped if small enough", - initialVelocity: mgl64.Vec3{-1e-9, -1e-9, -1e-9}, - expectedVelocity: mgl64.Vec3{0, 0, 0}, - shouldClamp: true, - }, - { - name: "large negative velocity is not clamped", - initialVelocity: mgl64.Vec3{-5.0, -2.0, -1.0}, - expectedVelocity: mgl64.Vec3{-5.0, -2.0, -1.0}, - shouldClamp: false, - }, - } - - for _, tt := range tests { - t.Run(tt.name, func(t *testing.T) { - rb := &actor.RigidBody{ - Velocity: tt.initialVelocity, - } - - clampSmallVelocities(rb) - - // Check if velocity was clamped to zero - if tt.shouldClamp { - if rb.Velocity != tt.expectedVelocity { - t.Errorf("clampSmallVelocities() velocity = %v, want %v", rb.Velocity, tt.expectedVelocity) - } - } else { - // Check if velocity remained unchanged - epsilon := 1e-10 - if math.Abs(rb.Velocity.X()-tt.expectedVelocity.X()) > epsilon || - math.Abs(rb.Velocity.Y()-tt.expectedVelocity.Y()) > epsilon || - math.Abs(rb.Velocity.Z()-tt.expectedVelocity.Z()) > epsilon { - t.Errorf("clampSmallVelocities() velocity = %v, want %v", rb.Velocity, tt.expectedVelocity) - } - } - }) - } -} diff --git a/constraint/contact.go b/constraint/contact.go index 78aa30e..3d6a249 100644 --- a/constraint/contact.go +++ b/constraint/contact.go @@ -1,284 +1,78 @@ package constraint import ( - "math" - "github.com/akmonengine/feather/actor" "github.com/go-gl/mathgl/mgl64" ) -const ( - // DefaultCompliance controls soft constraint stiffness for contact resolution. - // Lower values = stiffer contacts (less penetration, potential jitter) - // Higher values = softer contacts (more penetration, smoother) - // Typical range: 1e-10 (very stiff) to 1e-6 (soft) - // See PHYSICS_GUIDE.md for tuning guidelines. - DefaultCompliance = 1e-7 -) +// MaxContactPoints: 4 points are enough to keep a box flat (Erin Catto, GDC 2007) +const MaxContactPoints = 4 type ContactPoint struct { - Position mgl64.Vec3 - Penetration float64 + // Position is halfway between both surfaces + Position mgl64.Vec3 + // Separation < 0 when the bodies overlap, > 0 for a speculative contact (not touching yet) + Separation float64 + + // NormalImpulse applied by the solver during the last step (N·s), to warm start the next step + NormalImpulse float64 + + // LocalAnchorA is the point on the surface of A, in the local space of A, LocalAnchorB the point on the + // surface of B, in the local space of B. They find the same point in the next step (warm starting), + // and move the contact with the bodies (pair cache) + LocalAnchorA mgl64.Vec3 + LocalAnchorB mgl64.Vec3 } -type ContactConstraint struct { - BodyA *actor.RigidBody - BodyB *actor.RigidBody - Points []ContactPoint - Normal mgl64.Vec3 +// Manifold is the contact between 2 bodies. Normal points from A to B +type Manifold struct { + BodyA *actor.RigidBody + BodyB *actor.RigidBody + // IndexA & IndexB: the indices of the bodies in the World, set by the World for its solver + IndexA, IndexB int32 + Normal mgl64.Vec3 + Points [MaxContactPoints]ContactPoint + Count int + + // Impulses of the friction applied by the solver during the last step, to warm start the next step: along the + // tangents at the friction center of the points (N·s), and around the normal (N·m·s) + FrictionImpulse mgl64.Vec3 + TwistImpulse float64 + + // RollingImpulse applied by the solver during the last step (N·m·s), to warm start the next step + RollingImpulse mgl64.Vec3 + + // When the contact points were computed: the normal in the local space of A, + // and the position & rotation of B in the local space of A (pair cache) + LocalNormal mgl64.Vec3 + RelativePosition mgl64.Vec3 + RelativeRotation mgl64.Quat } -// SolvePosition resolves penetration (PBD style, no lambda accumulation) -func (c *ContactConstraint) SolvePosition(dt float64) { - if len(c.Points) == 0 { - return - } - if c.BodyA.IsSleeping && c.BodyB.IsSleeping { - return - } - - bodyA := c.BodyA - bodyB := c.BodyB - - bodyA.Mutex.Lock() - bodyB.Mutex.Lock() - defer bodyA.Mutex.Unlock() - defer bodyB.Mutex.Unlock() - - // ========== 1. Calculate total effective weight ========== - invMassA := 1.0 / bodyA.Material.GetMass() - invMassB := 1.0 / bodyB.Material.GetMass() - IA_inv := bodyA.GetInverseInertiaWorld() - IB_inv := bodyB.GetInverseInertiaWorld() - - var totalWeight float64 - var totalPenetration float64 - - for _, point := range c.Points { - penetration := point.Penetration - if penetration <= 1e-8 { - continue - } - - rA := point.Position.Sub(bodyA.Transform.Position) - rB := point.Position.Sub(bodyB.Transform.Position) - - // Calculate effective inertia for this point - rA_cross_n := rA.Cross(c.Normal) - rB_cross_n := rB.Cross(c.Normal) - - angularInertiaA := IA_inv.Mul3x1(rA_cross_n).Dot(rA_cross_n) - angularInertiaB := IB_inv.Mul3x1(rB_cross_n).Dot(rB_cross_n) - - wA := invMassA + angularInertiaA - wB := invMassB + angularInertiaB - totalWeight += wA + wB - - totalPenetration += penetration - } - - // ========== 2. Calculate deltaLambda (global correction) ========== - if totalWeight <= 1e-8 { - return - } - - compliance := DefaultCompliance - alphaTilde := compliance / (dt * dt) - deltaLambda := -totalPenetration / (totalWeight + alphaTilde) - - // ========== 3. Apply linear corrections ========== - totalImpulse := c.Normal.Mul(deltaLambda) - - if bodyA.BodyType != actor.BodyTypeStatic { - bodyA.Transform.Position = bodyA.Transform.Position.Add(totalImpulse.Mul(invMassA)) - } - if bodyB.BodyType != actor.BodyTypeStatic { - bodyB.Transform.Position = bodyB.Transform.Position.Sub(totalImpulse.Mul(invMassB)) - } - - // ========== 4. Apply angular corrections ========== - // Accumulate torques from all points, then apply ONE SINGLE correction - var totalTorqueA, totalTorqueB mgl64.Vec3 - - for _, point := range c.Points { - if point.Penetration <= 1e-8 { - continue - } - - rA := point.Position.Sub(bodyA.Transform.Position) - rB := point.Position.Sub(bodyB.Transform.Position) - - // Accumulate angular moments - // Body A receives +totalImpulse → torque_A = rA × (+totalImpulse) - // Body B receives -totalImpulse → torque_B = rB × (-totalImpulse) - totalTorqueA = totalTorqueA.Add(rA.Cross(totalImpulse)) - totalTorqueB = totalTorqueB.Add(rB.Cross(totalImpulse.Mul(-1))) - } - - // Calculate total angular correction - // In XPBD: Δθ = I_inv * (Σ torque) - deltaRotA := IA_inv.Mul3x1(totalTorqueA) - deltaRotB := IB_inv.Mul3x1(totalTorqueB) - - // Apply ONE SINGLE rotation correction via quaternions - // For a small angle δθ, the rotation quaternion is q_delta ≈ [1, δθ/2] - if bodyA.BodyType != actor.BodyTypeStatic && deltaRotA.Len() > 1e-10 { - qDelta := mgl64.Quat{W: 1.0, V: deltaRotA.Mul(0.5)} - qDelta = qDelta.Normalize() - bodyA.Transform.Rotation = qDelta.Mul(bodyA.Transform.Rotation).Normalize() - bodyA.Transform.InverseRotation = bodyA.Transform.Rotation.Inverse() - } - - if bodyB.BodyType != actor.BodyTypeStatic && deltaRotB.Len() > 1e-10 { - qDelta := mgl64.Quat{W: 1.0, V: deltaRotB.Mul(0.5)} - qDelta = qDelta.Normalize() - bodyB.Transform.Rotation = qDelta.Mul(bodyB.Transform.Rotation).Normalize() - bodyB.Transform.InverseRotation = bodyB.Transform.Rotation.Inverse() - } +func (m *Manifold) Reset(a, b *actor.RigidBody) { + *m = Manifold{BodyA: a, BodyB: b} } -// SolveVelocity applies restitution -func (c *ContactConstraint) SolveVelocity(dt float64) { - if len(c.Points) == 0 { +// Add a point, ignored after MaxContactPoints +func (m *Manifold) Add(position mgl64.Vec3, separation float64) { + if m.Count == MaxContactPoints { return } - if c.BodyA.IsSleeping && c.BodyB.IsSleeping { - return - } - - bodyA := c.BodyA - bodyB := c.BodyB - - bodyA.Mutex.Lock() - bodyB.Mutex.Lock() - defer bodyA.Mutex.Unlock() - defer bodyB.Mutex.Unlock() - - invMassA := 1.0 / bodyA.Material.GetMass() - invMassB := 1.0 / bodyB.Material.GetMass() - IA_inv := bodyA.GetInverseInertiaWorld() - IB_inv := bodyB.GetInverseInertiaWorld() - - restitution := ComputeRestitution(bodyA.Material, bodyB.Material) - staticFriction := ComputeStaticFriction(bodyA.Material, bodyB.Material) - dynamicFriction := ComputeDynamicFriction(bodyA.Material, bodyB.Material) - - // ========== ACCUMULATE all impulses ========== - var totalLinearImpulseA mgl64.Vec3 - var totalLinearImpulseB mgl64.Vec3 - var totalAngularImpulseA mgl64.Vec3 - var totalAngularImpulseB mgl64.Vec3 - - for _, point := range c.Points { - rA := point.Position.Sub(bodyA.Transform.Position) - rB := point.Position.Sub(bodyB.Transform.Position) - - // ========== Velocities ========== - vA := bodyA.Velocity.Add(bodyA.AngularVelocity.Cross(rA)) - vB := bodyB.Velocity.Add(bodyB.AngularVelocity.Cross(rB)) - relativeVel := vB.Sub(vA) - normalVel := relativeVel.Dot(c.Normal) - - // ========== Pre-resolution velocity ========== - vA_prev := bodyA.PresolveVelocity.Add(bodyA.PresolveAngularVelocity.Cross(rA)) - vB_prev := bodyB.PresolveVelocity.Add(bodyB.PresolveAngularVelocity.Cross(rB)) - relativeVelPrev := vB_prev.Sub(vA_prev) - normalVelPrev := relativeVelPrev.Dot(c.Normal) - - // ========== NORMAL IMPULSE (restitution) ========== - rA_cross_n := rA.Cross(c.Normal) - rB_cross_n := rB.Cross(c.Normal) - - angularInertiaA := IA_inv.Mul3x1(rA_cross_n).Dot(rA_cross_n) - angularInertiaB := IB_inv.Mul3x1(rB_cross_n).Dot(rB_cross_n) - - effectiveMassNormal := invMassA + invMassB + angularInertiaA + angularInertiaB - - if effectiveMassNormal < 1e-10 { - continue - } - - // ========== Impulse for this point ========== - targetVel := -restitution * normalVelPrev - deltaV := targetVel - normalVel - lambdaNormal := deltaV / effectiveMassNormal - - // ========== CRITICAL: Prevent attractive impulses ========== - if lambdaNormal < 0 { - lambdaNormal = 0 - } - - normalImpulse := c.Normal.Mul(lambdaNormal) - - // Accumulate normal impulse - totalLinearImpulseA = totalLinearImpulseA.Sub(normalImpulse.Mul(invMassA)) - totalLinearImpulseB = totalLinearImpulseB.Add(normalImpulse.Mul(invMassB)) - - torqueA := rA.Cross(normalImpulse.Mul(-1)) - torqueB := rB.Cross(normalImpulse) - - totalAngularImpulseA = totalAngularImpulseA.Add(IA_inv.Mul3x1(torqueA)) - totalAngularImpulseB = totalAngularImpulseB.Add(IB_inv.Mul3x1(torqueB)) - - // ========== TANGENTIAL IMPULSE (friction) ========== - // Only if there is a normal force - if lambdaNormal > 0 { - // Tangential velocity (component perpendicular to normal) - tangentVel := relativeVel.Sub(c.Normal.Mul(normalVel)) - tangentSpeed := tangentVel.Len() - - if tangentSpeed > 1e-6 { - // Tangential direction - tangentDir := tangentVel.Mul(1.0 / tangentSpeed) - - // Effective mass in tangential direction - rA_cross_t := rA.Cross(tangentDir) - rB_cross_t := rB.Cross(tangentDir) - angularInertiaA_t := IA_inv.Mul3x1(rA_cross_t).Dot(rA_cross_t) - angularInertiaB_t := IB_inv.Mul3x1(rB_cross_t).Dot(rB_cross_t) - - effectiveMassTangent := invMassA + invMassB + angularInertiaA_t + angularInertiaB_t - - if effectiveMassTangent < 1e-10 { - continue - } - - // Impulse to cancel tangential velocity - lambdaTangent := -tangentSpeed / effectiveMassTangent - - // Coulomb's law: |F_friction| ≤ μ * |F_normal| - maxStaticFriction := staticFriction * math.Abs(lambdaNormal) - - var frictionImpulse mgl64.Vec3 - - if math.Abs(lambdaTangent) <= maxStaticFriction { - // Static friction: completely cancels tangential velocity - frictionImpulse = tangentDir.Mul(lambdaTangent) - } else { - // Dynamic friction: limited by μ_dynamic - maxDynamicFriction := dynamicFriction * math.Abs(lambdaNormal) - frictionImpulse = tangentDir.Mul(-math.Copysign(maxDynamicFriction, tangentSpeed)) - } - - // Accumulate friction impulse - totalLinearImpulseA = totalLinearImpulseA.Sub(frictionImpulse.Mul(invMassA)) - totalLinearImpulseB = totalLinearImpulseB.Add(frictionImpulse.Mul(invMassB)) + m.Points[m.Count] = ContactPoint{Position: position, Separation: separation} + m.Count++ +} - torqueA_friction := rA.Cross(frictionImpulse.Mul(-1)) - torqueB_friction := rB.Cross(frictionImpulse) +func (m *Manifold) Flip() { + m.BodyA, m.BodyB = m.BodyB, m.BodyA + m.Normal = m.Normal.Mul(-1) +} - totalAngularImpulseA = totalAngularImpulseA.Add(IA_inv.Mul3x1(torqueA_friction)) - totalAngularImpulseB = totalAngularImpulseB.Add(IB_inv.Mul3x1(torqueB_friction)) - } +func (m *Manifold) MinSeparation() float64 { + min := posInf + for i := 0; i < m.Count; i++ { + if m.Points[i].Separation < min { + min = m.Points[i].Separation } } - - // ========== APPLY all impulses ========== - bodyA.Velocity = bodyA.Velocity.Add(totalLinearImpulseA) - bodyB.Velocity = bodyB.Velocity.Add(totalLinearImpulseB) - bodyA.AngularVelocity = bodyA.AngularVelocity.Add(totalAngularImpulseA) - bodyB.AngularVelocity = bodyB.AngularVelocity.Add(totalAngularImpulseB) - - clampSmallVelocities(bodyA) - clampSmallVelocities(bodyB) + return min } diff --git a/constraint/contact_test.go b/constraint/contact_test.go deleted file mode 100644 index 621abb3..0000000 --- a/constraint/contact_test.go +++ /dev/null @@ -1,404 +0,0 @@ -package constraint - -import ( - "math" - "testing" - - "github.com/akmonengine/feather/actor" - "github.com/go-gl/mathgl/mgl64" -) - -// Helper function to create a dynamic rigid body for testing -// Mass is calculated from density and shape (unit sphere) -func createDynamicBody(position mgl64.Vec3, velocity mgl64.Vec3, density float64) *actor.RigidBody { - // Create a unit sphere shape - shape := &actor.Sphere{Radius: 1.0} - - rb := actor.NewRigidBody( - actor.Transform{Position: position}, - shape, - actor.BodyTypeDynamic, - density, - ) - - rb.Velocity = velocity - rb.PresolveVelocity = velocity - rb.Material.Restitution = 0.5 - - return rb -} - -// Helper function to create a static rigid body -func createStaticBody(position mgl64.Vec3) *actor.RigidBody { - // Create a unit box shape for static body - shape := &actor.Box{HalfExtents: mgl64.Vec3{1, 1, 1}} - - rb := actor.NewRigidBody( - actor.Transform{Position: position}, - shape, - actor.BodyTypeStatic, - 0.0, - ) - - return rb -} - -func TestContactConstraint_SolvePosition_NoPenetration(t *testing.T) { - bodyA := createDynamicBody(mgl64.Vec3{0, 0, 0}, mgl64.Vec3{0, 0, 0}, 1.0) - bodyB := createDynamicBody(mgl64.Vec3{2, 0, 0}, mgl64.Vec3{0, 0, 0}, 1.0) - - constraint := &ContactConstraint{ - BodyA: bodyA, - BodyB: bodyB, - Normal: mgl64.Vec3{1, 0, 0}, - Points: []ContactPoint{ - { - Position: mgl64.Vec3{1, 0, 0}, - Penetration: 0.0, // No penetration - }, - }, - } - - originalPosA := bodyA.Transform.Position - originalPosB := bodyB.Transform.Position - - constraint.SolvePosition(0.016) // 60 FPS timestep - - // Positions should not change when there's no penetration - if bodyA.Transform.Position != originalPosA { - t.Errorf("BodyA position changed when there was no penetration: %v -> %v", originalPosA, bodyA.Transform.Position) - } - if bodyB.Transform.Position != originalPosB { - t.Errorf("BodyB position changed when there was no penetration: %v -> %v", originalPosB, bodyB.Transform.Position) - } -} - -func TestContactConstraint_SolvePosition_WithPenetration(t *testing.T) { - bodyA := createDynamicBody(mgl64.Vec3{0, 0, 0}, mgl64.Vec3{0, 0, 0}, 1.0) - bodyB := createDynamicBody(mgl64.Vec3{1.5, 0, 0}, mgl64.Vec3{0, 0, 0}, 1.0) - - penetrationDepth := 0.5 - - constraint := &ContactConstraint{ - BodyA: bodyA, - BodyB: bodyB, - Normal: mgl64.Vec3{1, 0, 0}, // Normal points from A to B - Points: []ContactPoint{ - { - Position: mgl64.Vec3{0.75, 0, 0}, - Penetration: penetrationDepth, - }, - }, - } - - originalPosA := bodyA.Transform.Position - originalPosB := bodyB.Transform.Position - - constraint.SolvePosition(0.016) - - // Bodies should move apart - // BodyA should move in -normal direction (left) - if bodyA.Transform.Position.X() >= originalPosA.X() { - t.Errorf("BodyA should move left (negative X), but moved from %v to %v", originalPosA, bodyA.Transform.Position) - } - - // BodyB should move in +normal direction (right) - if bodyB.Transform.Position.X() <= originalPosB.X() { - t.Errorf("BodyB should move right (positive X), but moved from %v to %v", originalPosB, bodyB.Transform.Position) - } - - // The separation distance should increase - newSeparation := bodyB.Transform.Position.Sub(bodyA.Transform.Position).Len() - oldSeparation := originalPosB.Sub(originalPosA).Len() - - if newSeparation <= oldSeparation { - t.Errorf("Bodies did not separate: old distance=%v, new distance=%v", oldSeparation, newSeparation) - } -} - -func TestContactConstraint_SolvePosition_EqualMasses(t *testing.T) { - mass := 2.0 - bodyA := createDynamicBody(mgl64.Vec3{0, 0, 0}, mgl64.Vec3{0, 0, 0}, mass) - bodyB := createDynamicBody(mgl64.Vec3{1, 0, 0}, mgl64.Vec3{0, 0, 0}, mass) - - penetration := 0.2 - - constraint := &ContactConstraint{ - BodyA: bodyA, - BodyB: bodyB, - Normal: mgl64.Vec3{1, 0, 0}, - Points: []ContactPoint{ - { - Position: mgl64.Vec3{0.5, 0, 0}, - Penetration: penetration, - }, - }, - } - - originalPosA := bodyA.Transform.Position - originalPosB := bodyB.Transform.Position - - constraint.SolvePosition(0.016) - - // With equal masses, both should move equal distances - deltaA := bodyA.Transform.Position.Sub(originalPosA).Len() - deltaB := bodyB.Transform.Position.Sub(originalPosB).Len() - - if math.Abs(deltaA-deltaB) > 1e-6 { - t.Errorf("Equal mass bodies should move equal distances: deltaA=%v, deltaB=%v", deltaA, deltaB) - } -} - -func TestContactConstraint_SolvePosition_StaticBody(t *testing.T) { - bodyA := createStaticBody(mgl64.Vec3{0, 0, 0}) - bodyB := createDynamicBody(mgl64.Vec3{1, 0, 0}, mgl64.Vec3{0, 0, 0}, 1.0) - - constraint := &ContactConstraint{ - BodyA: bodyA, - BodyB: bodyB, - Normal: mgl64.Vec3{1, 0, 0}, - Points: []ContactPoint{ - { - Position: mgl64.Vec3{0.5, 0, 0}, - Penetration: 0.3, - }, - }, - } - - originalPosA := bodyA.Transform.Position - originalPosB := bodyB.Transform.Position - - constraint.SolvePosition(0.016) - - // Static body should not move - if bodyA.Transform.Position != originalPosA { - t.Errorf("Static body moved: %v -> %v", originalPosA, bodyA.Transform.Position) - } - - // Dynamic body should move away - if bodyB.Transform.Position.X() <= originalPosB.X() { - t.Errorf("Dynamic body should move away from static body: %v -> %v", originalPosB, bodyB.Transform.Position) - } -} - -func TestContactConstraint_SolvePosition_BothStatic(t *testing.T) { - bodyA := createStaticBody(mgl64.Vec3{0, 0, 0}) - bodyB := createStaticBody(mgl64.Vec3{1, 0, 0}) - - constraint := &ContactConstraint{ - BodyA: bodyA, - BodyB: bodyB, - Normal: mgl64.Vec3{1, 0, 0}, - Points: []ContactPoint{ - { - Position: mgl64.Vec3{0.5, 0, 0}, - Penetration: 0.5, - }, - }, - } - - originalPosA := bodyA.Transform.Position - originalPosB := bodyB.Transform.Position - - constraint.SolvePosition(0.016) - - // Both static bodies should not move - if bodyA.Transform.Position != originalPosA { - t.Errorf("Static bodyA moved: %v -> %v", originalPosA, bodyA.Transform.Position) - } - if bodyB.Transform.Position != originalPosB { - t.Errorf("Static bodyB moved: %v -> %v", originalPosB, bodyB.Transform.Position) - } -} - -func TestContactConstraint_SolveVelocity_NoRelativeVelocity(t *testing.T) { - bodyA := createDynamicBody(mgl64.Vec3{0, 0, 0}, mgl64.Vec3{1, 0, 0}, 1.0) - bodyB := createDynamicBody(mgl64.Vec3{2, 0, 0}, mgl64.Vec3{1, 0, 0}, 1.0) - - constraint := &ContactConstraint{ - BodyA: bodyA, - BodyB: bodyB, - Normal: mgl64.Vec3{1, 0, 0}, - Points: []ContactPoint{ - { - Position: mgl64.Vec3{1, 0, 0}, - Penetration: 0.1, - }, - }, - } - - originalVelA := bodyA.Velocity - originalVelB := bodyB.Velocity - - constraint.SolveVelocity(0.016) - - // Since both bodies have the same velocity, there's no relative motion to correct - // Velocities should remain relatively unchanged - epsilon := 0.1 - if bodyA.Velocity.Sub(originalVelA).Len() > epsilon { - t.Logf("BodyA velocity changed from %v to %v (expected minimal change)", originalVelA, bodyA.Velocity) - } - if bodyB.Velocity.Sub(originalVelB).Len() > epsilon { - t.Logf("BodyB velocity changed from %v to %v (expected minimal change)", originalVelB, bodyB.Velocity) - } -} - -func TestContactConstraint_SolveVelocity_Approaching(t *testing.T) { - // Body A moving right, Body B stationary - bodyA := createDynamicBody(mgl64.Vec3{0, 0, 0}, mgl64.Vec3{5, 0, 0}, 1.0) - bodyB := createDynamicBody(mgl64.Vec3{2, 0, 0}, mgl64.Vec3{0, 0, 0}, 1.0) - - bodyA.Material.Restitution = 0.8 - bodyB.Material.Restitution = 0.8 - - constraint := &ContactConstraint{ - BodyA: bodyA, - BodyB: bodyB, - Normal: mgl64.Vec3{1, 0, 0}, // Normal points from A to B - Points: []ContactPoint{ - { - Position: mgl64.Vec3{1, 0, 0}, - Penetration: 0.1, - }, - }, - } - - constraint.SolveVelocity(0.016) - - // After collision, bodyA should slow down (or reverse) - if bodyA.Velocity.X() >= 5.0 { - t.Errorf("BodyA should slow down after collision: velocity=%v", bodyA.Velocity) - } - - // BodyB should gain velocity in the positive direction - if bodyB.Velocity.X() <= 0.0 { - t.Errorf("BodyB should gain velocity after collision: velocity=%v", bodyB.Velocity) - } -} - -func TestContactConstraint_SolveVelocity_Restitution(t *testing.T) { - // Test with high restitution (bouncy collision) - bodyA := createDynamicBody(mgl64.Vec3{0, 0, 0}, mgl64.Vec3{10, 0, 0}, 1.0) - bodyB := createDynamicBody(mgl64.Vec3{2, 0, 0}, mgl64.Vec3{0, 0, 0}, 1.0) - - bodyA.Material.Restitution = 1.0 - bodyB.Material.Restitution = 1.0 - bodyA.PresolveVelocity = mgl64.Vec3{10, 0, 0} - bodyB.PresolveVelocity = mgl64.Vec3{0, 0, 0} - - constraint := &ContactConstraint{ - BodyA: bodyA, - BodyB: bodyB, - Normal: mgl64.Vec3{1, 0, 0}, - Points: []ContactPoint{ - { - Position: mgl64.Vec3{1, 0, 0}, - Penetration: 0.1, - }, - }, - } - - constraint.SolveVelocity(0.5) // Larger timestep to avoid restitution threshold - - // With perfect restitution and equal masses, velocities should exchange - // (bodyA should slow down significantly, bodyB should speed up) - totalMomentumBefore := 10.0 // bodyA momentum - totalMomentumAfter := bodyA.Velocity.X() + bodyB.Velocity.X() - - // Momentum should be conserved (approximately) - if math.Abs(totalMomentumBefore-totalMomentumAfter) > 1.0 { - t.Logf("Momentum conservation: before=%v, after=%v", totalMomentumBefore, totalMomentumAfter) - } -} - -func TestContactConstraint_SolveVelocity_LowSpeedNoRestitution(t *testing.T) { - // Test restitution threshold - low velocity collisions should not bounce - dt := 0.016 - bodyA := createDynamicBody(mgl64.Vec3{0, 0, 0}, mgl64.Vec3{0.1, 0, 0}, 1.0) - bodyB := createDynamicBody(mgl64.Vec3{2, 0, 0}, mgl64.Vec3{0, 0, 0}, 1.0) - - bodyA.Material.Restitution = 0.9 - bodyB.Material.Restitution = 0.9 - bodyA.PresolveVelocity = mgl64.Vec3{0.1, 0, 0} - bodyB.PresolveVelocity = mgl64.Vec3{0, 0, 0} - - constraint := &ContactConstraint{ - BodyA: bodyA, - BodyB: bodyB, - Normal: mgl64.Vec3{1, 0, 0}, - Points: []ContactPoint{ - { - Position: mgl64.Vec3{1, 0, 0}, - Penetration: 0.05, - }, - }, - } - - constraint.SolveVelocity(dt) - - // Due to restitution threshold (2 * 9.81 * dt), low velocities should not bounce much - restitutionThreshold := 2.0 * 9.81 * dt - - if math.Abs(bodyA.PresolveVelocity.X()) < restitutionThreshold { - // Low velocity - should not bounce with full restitution - relativeVel := bodyB.Velocity.Sub(bodyA.Velocity).Dot(constraint.Normal) - if relativeVel < 0 { - t.Logf("Low velocity collision correctly avoided bouncing") - } - } -} - -func TestContactConstraint_SolveVelocity_MultiplePoints(t *testing.T) { - bodyA := createDynamicBody(mgl64.Vec3{0, 0, 0}, mgl64.Vec3{2, 0, 0}, 1.0) - bodyB := createDynamicBody(mgl64.Vec3{2, 0, 0}, mgl64.Vec3{0, 0, 0}, 1.0) - - constraint := &ContactConstraint{ - BodyA: bodyA, - BodyB: bodyB, - Normal: mgl64.Vec3{1, 0, 0}, - Points: []ContactPoint{ - { - Position: mgl64.Vec3{1, 0.5, 0}, - Penetration: 0.1, - }, - { - Position: mgl64.Vec3{1, -0.5, 0}, - Penetration: 0.1, - }, - }, - } - - originalVelA := bodyA.Velocity - constraint.SolveVelocity(0.016) - - // Velocity should change due to contact resolution - if bodyA.Velocity == originalVelA { - t.Errorf("Expected velocity to change with multiple contact points") - } -} - -func TestContactConstraint_SolveVelocity_SmallVelocityClamping(t *testing.T) { - // Test that very small velocities get clamped to zero - bodyA := createDynamicBody(mgl64.Vec3{0, 0, 0}, mgl64.Vec3{1e-9, 1e-9, 1e-9}, 1.0) - bodyB := createDynamicBody(mgl64.Vec3{2, 0, 0}, mgl64.Vec3{0, 0, 0}, 1.0) - - constraint := &ContactConstraint{ - BodyA: bodyA, - BodyB: bodyB, - Normal: mgl64.Vec3{1, 0, 0}, - Points: []ContactPoint{ - { - Position: mgl64.Vec3{1, 0, 0}, - Penetration: 0.1, - }, - }, - } - - constraint.SolveVelocity(0.016) - - // Small velocities should be clamped to zero - zeroVec := mgl64.Vec3{0, 0, 0} - if bodyA.Velocity != zeroVec && bodyA.Velocity.Len() < 1e-7 { - t.Logf("Very small velocity was appropriately handled: %v", bodyA.Velocity) - } -} diff --git a/epa/epa.go b/epa/epa.go index ad06782..73d16a1 100644 --- a/epa/epa.go +++ b/epa/epa.go @@ -1,265 +1,291 @@ -// Package epa implements the Expanding Polytope Algorithm for computing penetration depth. +// Package epa implements the Expanding Polytope Algorithm, to compute the penetration depth. // -// EPA is run after GJK detects a collision to determine: -// - Penetration depth (how far shapes overlap) -// - Contact normal (direction to separate shapes) -// - Contact points (where shapes touch) +// EPA runs after GJK when there is a collision. It returns the penetration depth, the contact normal, +// and the witness points of the contact on both shapes. +// The polytope starts from the tetrahedron of GJK and grows towards the Minkowski difference, +// until the closest face to the origin is on its surface. // -// The algorithm expands a polytope (starting from GJK's final simplex) toward the origin -// in the Minkowski difference space, finding the closest face which gives us the -// Minimum Translation Vector (MTV) to separate the shapes. -// -// For detailed algorithm explanation with pseudocode and visual examples, see: +// For detailed algorithm explanation, see: // ALGORITHMS.md - "EPA Algorithm" section // // References: // - Van den Bergen: "Proximity Queries and Penetration Depth Computation on 3D Game Objects" (2001) +// - Ericson: "Real-Time Collision Detection" (2004) package epa import ( - "fmt" + "errors" "math" + "sync" "github.com/akmonengine/feather/actor" - "github.com/akmonengine/feather/constraint" "github.com/akmonengine/feather/gjk" "github.com/go-gl/mathgl/mgl64" ) const ( - // EPAMaxIterations limits polytope expansion to prevent infinite loops. - // Typical convergence: 5-15 iterations for simple shapes. - // If this limit is reached, EPA returns an error. - EPAMaxIterations = 32 - - // EPAConvergenceTolerance defines when EPA has converged. - // If the distance to a new support point improves by less than this threshold, - // we've found the closest face to the origin. - // Lower values = more precision but slower convergence. - EPAConvergenceTolerance = 0.001 - - // EPAMinFaceDistance is the minimum face distance before we skip it. - // Faces very close to or behind the origin are likely degenerate. - EPAMinFaceDistance = 0.0001 - - // NormalSnapThreshold is used to clamp nearly-zero normal components to exactly zero. - // This helps with numerical stability and axis-aligned collisions. - NormalSnapThreshold = 1e-8 - - // DegeneratePenetrationEstimate is a fallback penetration depth for degenerate cases - // where we have insufficient simplex points to compute accurate depth. - DegeneratePenetrationEstimate = 0.01 - - // Small initial capacity for PolytopeBuilder - grows dynamically as needed - // Using very small initial capacity (4) for memory efficiency - // No maximum limits - buffers grow to accommodate any reasonable polytope size - polytopeInitialCapacity = 4 + // EPAMaxIterations: flat faces converge in a few iterations, rounded shapes need more + EPAMaxIterations = 128 + + // EPAConvergenceTolerance (m): EPA stops when the new support point improves the distance by less than this value. + // It is the error on the penetration depth + EPAConvergenceTolerance = 1e-7 + + // EPATieTolerance (m): the faces of the Minkowski difference less deep than the closest one by less than this are + // as deep. EPA takes the first of them in a fixed order in the local space of A, not the one the rounding found + // first: moved by 1 µm, a scene keeps the same normals (Box3D keeps its choices with a pecking order, the bias of + // 0.95 of its manifolds, and a cache of the features). The rounding is ~1e-11 m far from the origin: 1 µm keeps + // the depth exact to 1 µm + EPATieTolerance = 1e-6 ) -// EPA computes penetration depth and contact information for overlapping convex shapes. -// -// Algorithm overview: -// 1. Start with simplex from GJK (tetrahedron containing origin) -// 2. Build initial polytope faces from simplex -// 3. Find face closest to origin -// 4. Get support point in face normal direction -// 5. If converged (new point doesn't improve distance) → done -// 6. Otherwise, expand polytope by adding support point -// 7. Repeat from step 3 -// -// Parameters: -// - a, b: The two colliding rigid bodies -// - simplex: Final simplex from GJK (typically 4 points forming tetrahedron) -// -// Returns: -// - ContactConstraint: Contains contact normal, penetration depth, contact points -// - error: Non-nil if EPA failed to converge or encountered degenerate case -// -// The contact normal points from body A toward body B (separation direction). -// Penetration depth is always positive (how far to move B away from A). -func EPA(a, b *actor.RigidBody, simplex *gjk.Simplex) (constraint.ContactConstraint, error) { - // If simplex is too small (degenerate case), create a minimal contact - if simplex.Count < 4 { - return handleDegenerateSimplex(a, b, simplex), nil - } +// tieOrder: the direction, in the local space of A, which orders the normals as deep. Its components are different and +// not zero, so that no 2 axes of a box (nor their opposites) have the same rank +var tieOrder = mgl64.Vec3{1, math.Sqrt2, math.Sqrt(3)}.Normalize() + +var ErrNoConvergence = errors.New("epa: no convergence") + +// Result is the penetration of A (+ margin) into B +type Result struct { + // Normal from A to B: moving B by Depth along Normal separates the shapes + Normal mgl64.Vec3 + Depth float64 + // Witness points: the deepest points of the contact, on A (+ margin) and on B. + // WitnessA - WitnessB = Depth * Normal + WitnessA mgl64.Vec3 + WitnessB mgl64.Vec3 +} - // Get builder from pool - single allocation replacing multiple pools - builder := polytopeBuilderPool.Get().(*PolytopeBuilder) - defer polytopeBuilderPool.Put(builder) - builder.Reset() +type face struct { + v [3]int // counter-clockwise, seen from outside + normal mgl64.Vec3 + distance float64 + // converged: the face is on the surface of the Minkowski difference + converged bool +} - // Step 1: Build initial polytope faces from the tetrahedron simplex - if err := builder.BuildInitialFaces(simplex); err != nil { - return constraint.ContactConstraint{}, err +type edge struct{ a, b int } + +type polytope struct { + vertices []gjk.Vertex + faces []face + horizon []edge +} + +var polytopePool = sync.Pool{New: func() any { return &polytope{} }} + +// EPA computes the penetration of A (+ margin) into B, from the tetrahedron of GJK +func EPA(a, b *actor.RigidBody, simplex *gjk.Simplex, margin float64) (Result, error) { + proxyA, proxyB := gjk.NewProxy(a), gjk.NewProxy(b) + return EPAProxies(&proxyA, &proxyB, simplex, margin) +} + +// EPAProxies is EPA for prepared bodies +func EPAProxies(a, b *gjk.Proxy, simplex *gjk.Simplex, margin float64) (Result, error) { + if simplex.Count != 4 { + return Result{}, ErrNoConvergence } - var closestFaceIndex int - var closestFace *Face - var support mgl64.Vec3 - var distance float64 + p := polytopePool.Get().(*polytope) + defer polytopePool.Put(p) + p.vertices = p.vertices[:0] + p.faces = p.faces[:0] - // Step 2: Iteratively expand polytope toward origin - for i := 0; i < EPAMaxIterations; i++ { - if len(builder.faces) == 0 { - // All faces removed (degenerate polytope) - should not happen - break + for i := 0; i < 4; i++ { + p.vertices = append(p.vertices, simplex.Vertex(i)) + } + for _, f := range [4][3]int{{0, 1, 2}, {0, 3, 1}, {0, 2, 3}, {1, 3, 2}} { + if !p.addFace(f[0], f[1], f[2]) { + return Result{}, ErrNoConvergence } + } + // The normals must point outwards: if the first face points to the 4th vertex, flip all faces + if p.faces[0].normal.Dot(p.vertices[3].W.Sub(p.vertices[0].W)) > 0 { + for i := range p.faces { + f := &p.faces[i] + f.v[1], f.v[2] = f.v[2], f.v[1] + f.normal = f.normal.Mul(-1) + f.distance = -f.distance + } + } - // Step 3: Find the face closest to the origin - // This face's normal and distance give us the current best MTV estimate - closestFaceIndex = builder.FindClosestFaceIndex() - closestFace = &builder.faces[closestFaceIndex] - - // Skip faces that are too close to or behind the origin (degenerate) - if closestFace.Distance < EPAMinFaceDistance { - // Remove this face and try the next one using swap-with-last - builder.faces[closestFaceIndex] = builder.faces[len(builder.faces)-1] - builder.faces = builder.faces[:len(builder.faces)-1] - continue + // the closest face converges, then the faces as deep as it (EPATieTolerance): the deepest normals are all known + for iteration := 0; iteration < EPAMaxIterations; iteration++ { + closest := p.faces[p.closestFace()] + target := p.unconvergedTie(closest.distance) + if target < 0 { + return p.result(p.faces[p.firstTie(closest.distance, a)]), nil } - // Step 4: Get support point in the direction of the closest face's normal - support = gjk.MinkowskiSupport(a, b, closestFace.Normal) - distance = support.Dot(closestFace.Normal) - - // Step 5: Check for convergence - // If the new support point doesn't significantly improve the distance, - // we've found the face of the Minkowski difference closest to the origin - if distance-closestFace.Distance < EPAConvergenceTolerance { - // Generate contact manifold (multiple contact points for stability) - manifoldPoints := GenerateManifold(a, b, closestFace.Normal, closestFace.Distance) - - return constraint.ContactConstraint{ - BodyA: a, - BodyB: b, - Points: manifoldPoints, - Normal: closestFace.Normal, - }, nil + f := p.faces[target] + v := gjk.SupportProxies(a, b, f.normal, margin) + if v.W.Dot(f.normal)-f.distance < EPAConvergenceTolerance { + p.faces[target].converged = true + continue } - // Step 6: Expand polytope by adding the new support point - // This removes faces that "see" the new point and adds new faces connecting to it - // Zero allocations - all operations use fixed buffers - if err := builder.AddPointAndRebuildFaces(support, closestFaceIndex); err != nil { - // Buffer overflow - return current best estimate instead of failing - manifoldPoints := GenerateManifold(a, b, closestFace.Normal, closestFace.Distance) - return constraint.ContactConstraint{ - BodyA: a, - BodyB: b, - Points: manifoldPoints, - Normal: closestFace.Normal, - }, nil + if !p.expand(v) { + return p.result(closest), nil } } - // EPA failed to converge within max iterations (rare, indicates numerical issues) - return constraint.ContactConstraint{}, fmt.Errorf("EPA failed to converge after %d iterations", EPAMaxIterations) + return p.result(p.faces[p.closestFace()]), nil } -// handleDegenerateSimplex creates a contact constraint when GJK returns an incomplete simplex. -// -// This happens in rare edge cases where shapes are touching but GJK couldn't build a full -// tetrahedron. We estimate the contact normal and penetration depth from available points. -// -// Cases: -// - 2+ points: Use closest point to origin as penetration estimate -// - 1 point: Estimate from body center separation (very approximate) -// -// Returns a valid ContactConstraint with estimated values. -func handleDegenerateSimplex(bodyA, bodyB *actor.RigidBody, simplex *gjk.Simplex) constraint.ContactConstraint { - if simplex.Count >= 2 { - // Use first two points to estimate - a := simplex.Points[0] - b := simplex.Points[1] - - // Find which point is closer to origin - distA := math.Sqrt(a.Dot(a)) - distB := math.Sqrt(b.Dot(b)) - - var penetration float64 - var normal mgl64.Vec3 - - if distA < distB { - penetration = distA - normal = a.Normalize() - } else { - penetration = distB - normal = b.Normalize() +// unconvergedTie: a face as deep as the closest one (EPATieTolerance) not on the surface yet, the closest first; -1 if +// all are on the surface +func (p *polytope) unconvergedTie(closest float64) int { + best := -1 + for i := range p.faces { + f := &p.faces[i] + if f.converged || f.distance > closest+EPATieTolerance { + continue } - - manifoldPoints := GenerateManifold(bodyA, bodyB, normal, penetration) - - return constraint.ContactConstraint{ - BodyA: bodyA, - BodyB: bodyB, - Points: manifoldPoints, - Normal: normal, + if best < 0 || f.distance < p.faces[best].distance { + best = i } } + return best +} - // Single point simplex - most degenerate case - // Estimate contact normal from body centers - normal := bodyB.Transform.Position.Sub(bodyA.Transform.Position) - normalLen := normal.Len() - - if normalLen < NormalSnapThreshold { - // Centers are at same location, use default upward direction - normal = mgl64.Vec3{0, 1, 0} - } else { - normal = normal.Mul(1.0 / normalLen) +// firstTie: among the faces as deep as the closest one, the first feature in the order of tieOrder (local space of A). +// The triangles of a same feature (normals within sameFeatureCos: a face of a box, or a rounded surface) are not tied: +// the closest one is kept, and among the triangles of a flat face, the one containing the projection of the origin +func (p *polytope) firstTie(closest float64, a *gjk.Proxy) int { + best := -1 + for i := range p.faces { + f := &p.faces[i] + if f.distance > closest+EPATieTolerance { + continue + } + if best < 0 { + best = i + continue + } + b := &p.faces[best] + if f.normal.Dot(b.normal) > sameFeatureCos { + if f.distance < b.distance-sameDistance || + (math.Abs(f.distance-b.distance) <= sameDistance && p.containsProjection(f) && !p.containsProjection(b)) { + best = i + } + continue + } + if a.Inverse.Mul3x1(f.normal).Dot(tieOrder) > a.Inverse.Mul3x1(b.normal).Dot(tieOrder) { + best = i + } } + return best +} - // Estimate penetration depth (highly approximate for degenerate case) - penetration := DegeneratePenetrationEstimate +const ( + // sameFeatureCos: 2 triangles of the polytope with normals closer than 1° belong to the same feature. On a rounded + // shape of 10 cm, the triangles within EPATieTolerance of the closest one are within 0.3° of it + sameFeatureCos = 0.9998476951563913 // cos(1°) + // sameDistance (m): 2 triangles of a flat face are at the same distance, to the rounding + sameDistance = 1e-12 +) - // Generate manifold with estimated normal - manifoldPoints := GenerateManifold(bodyA, bodyB, normal, penetration) +// containsProjection: the projection of the origin on the face is inside its triangle +func (p *polytope) containsProjection(f *face) bool { + a, b, c := p.vertices[f.v[0]].W, p.vertices[f.v[1]].W, p.vertices[f.v[2]].W + point := f.normal.Mul(f.distance) + n := b.Sub(a).Cross(c.Sub(a)) + return b.Sub(a).Cross(point.Sub(a)).Dot(n) >= 0 && c.Sub(b).Cross(point.Sub(b)).Dot(n) >= 0 && + a.Sub(c).Cross(point.Sub(c)).Dot(n) >= 0 +} - // Return fallback contact constraint - return constraint.ContactConstraint{ - BodyA: bodyA, - BodyB: bodyB, - Points: manifoldPoints, - Normal: normal, +// addFace returns false if the triangle is degenerate +func (p *polytope) addFace(i, j, k int) bool { + a, b, c := p.vertices[i].W, p.vertices[j].W, p.vertices[k].W + n := b.Sub(a).Cross(c.Sub(a)) + length := n.Len() + if length < 1e-14 { + return false } + n = n.Mul(1 / length) + p.faces = append(p.faces, face{v: [3]int{i, j, k}, normal: n, distance: n.Dot(a)}) + return true } -// snapNormalToAxis clamps nearly-zero components of a normal vector to exactly zero. -// -// This improves numerical stability for axis-aligned collisions (box on ground) -// by preventing tiny floating-point errors from causing jitter in tangent directions. -// -// Components with absolute value < NormalSnapThreshold are set to 0, then the -// vector is renormalized. -func snapNormalToAxis(normal mgl64.Vec3) mgl64.Vec3 { - const threshold = NormalSnapThreshold - - x := normal[0] - y := normal[1] - z := normal[2] - - // Clamp tiny components to zero - if math.Abs(x) < threshold { - x = 0 +func (p *polytope) closestFace() int { + best := 0 + for i := 1; i < len(p.faces); i++ { + if p.faces[i].distance < p.faces[best].distance { + best = i + } } - if math.Abs(y) < threshold { - y = 0 + return best +} + +// expand adds the vertex: the faces it can see are removed, and the hole is closed with new faces +// from the horizon to the vertex +func (p *polytope) expand(v gjk.Vertex) bool { + p.vertices = append(p.vertices, v) + index := len(p.vertices) - 1 + p.horizon = p.horizon[:0] + + kept := p.faces[:0] + for _, f := range p.faces { + if f.normal.Dot(v.W.Sub(p.vertices[f.v[0]].W)) > 0 { + for e := 0; e < 3; e++ { + p.toggleEdge(edge{f.v[e], f.v[(e+1)%3]}) + } + continue + } + kept = append(kept, f) } - if math.Abs(z) < threshold { - z = 0 + p.faces = kept + + if len(p.horizon) == 0 { + return false + } + for _, e := range p.horizon { + if !p.addFace(e.a, e.b, index) { + return false + } } + return true +} - // Reconstruct the normal - clamped := mgl64.Vec3{x, y, z} +// toggleEdge: an edge shared by 2 removed faces is inside and disappears, the others are the horizon +func (p *polytope) toggleEdge(e edge) { + for i, h := range p.horizon { + if h.a == e.b && h.b == e.a { + p.horizon = append(p.horizon[:i], p.horizon[i+1:]...) + return + } + } + p.horizon = append(p.horizon, e) +} - // Renormalize (important!) - length := math.Sqrt(clamped.Dot(clamped)) - if length > 1e-8 { - clamped = clamped.Mul(1.0 / length) - } else { - // If all components were clamped to zero, return default - return mgl64.Vec3{0, 1, 0} +// result projects the origin on the face, the barycentric coordinates give the witness points +func (p *polytope) result(f face) Result { + a, b, c := p.vertices[f.v[0]], p.vertices[f.v[1]], p.vertices[f.v[2]] + point := f.normal.Mul(f.distance) + u, v, w := barycentric(point, a.W, b.W, c.W) + + return Result{ + Normal: f.normal, + Depth: math.Max(f.distance, 0), + WitnessA: a.A.Mul(u).Add(b.A.Mul(v)).Add(c.A.Mul(w)), + WitnessB: a.B.Mul(u).Add(b.B.Mul(v)).Add(c.B.Mul(w)), } +} - return clamped +// barycentric coordinates of p in the triangle, clamped to it (Ericson 3.4) +func barycentric(p, a, b, c mgl64.Vec3) (float64, float64, float64) { + v0, v1, v2 := b.Sub(a), c.Sub(a), p.Sub(a) + d00, d01, d11 := v0.Dot(v0), v0.Dot(v1), v1.Dot(v1) + d20, d21 := v2.Dot(v0), v2.Dot(v1) + denominator := d00*d11 - d01*d01 + if denominator == 0 { + return 1, 0, 0 + } + v := (d11*d20 - d01*d21) / denominator + w := (d00*d21 - d01*d20) / denominator + v, w = math.Max(v, 0), math.Max(w, 0) + if sum := v + w; sum > 1 { + v, w = v/sum, w/sum + } + return 1 - v - w, v, w } diff --git a/epa/epa_test.go b/epa/epa_test.go index f99b564..ff4eaef 100644 --- a/epa/epa_test.go +++ b/epa/epa_test.go @@ -2,6 +2,7 @@ package epa import ( "math" + "math/rand" "testing" "github.com/akmonengine/feather/actor" @@ -9,471 +10,260 @@ import ( "github.com/go-gl/mathgl/mgl64" ) -// TestSnapNormalToAxis tests the normal snapping function for numerical stability -func TestSnapNormalToAxis(t *testing.T) { - tests := []struct { - name string - input mgl64.Vec3 - expected mgl64.Vec3 - }{ - { - name: "small_x_component", - input: mgl64.Vec3{1e-9, 1.0, 0.0}, - expected: mgl64.Vec3{0.0, 1.0, 0.0}, - }, - { - name: "small_y_component", - input: mgl64.Vec3{1.0, 1e-9, 0.0}, - expected: mgl64.Vec3{1.0, 0.0, 0.0}, - }, - { - name: "small_z_component", - input: mgl64.Vec3{0.0, 1.0, 1e-9}, - expected: mgl64.Vec3{0.0, 1.0, 0.0}, - }, - { - name: "already_axis_aligned_x", - input: mgl64.Vec3{1.0, 0.0, 0.0}, - expected: mgl64.Vec3{1.0, 0.0, 0.0}, - }, - { - name: "diagonal_normal", - input: mgl64.Vec3{1.0, 1.0, 1.0}.Normalize(), - expected: mgl64.Vec3{1.0, 1.0, 1.0}.Normalize(), - }, - { - name: "near_zero_vector", - input: mgl64.Vec3{1e-9, 1e-9, 1e-9}, - expected: mgl64.Vec3{0.0, 1.0, 0.0}, // Default fallback - }, - { - name: "multiple_small_components", - input: mgl64.Vec3{1e-8, 1e-8, 1.0}, - expected: mgl64.Vec3{0.0, 0.0, 1.0}, - }, - } - - for _, tt := range tests { - t.Run(tt.name, func(t *testing.T) { - result := snapNormalToAxis(tt.input) - - if !vec3ApproxEqual(result, tt.expected, 1e-6) { - t.Errorf("snapNormalToAxis(%v) = %v, want %v", tt.input, result, tt.expected) - } +func body(position mgl64.Vec3, rotation mgl64.Quat, shape actor.ShapeInterface) *actor.RigidBody { + return actor.NewRigidBody(actor.Transform{Position: position, Rotation: rotation}, shape, actor.BodyTypeDynamic, 1) +} - // Verify result is normalized - if !isNormalized(result, 1e-6) { - t.Errorf("result is not normalized: length = %v", result.Len()) - } - }) - } +func randomRotation(r *rand.Rand) mgl64.Quat { + u1, u2, u3 := r.Float64(), r.Float64(), r.Float64() + return mgl64.Quat{W: math.Sqrt(1-u1) * math.Sin(2*math.Pi*u2), V: mgl64.Vec3{ + math.Sqrt(1-u1) * math.Cos(2*math.Pi*u2), math.Sqrt(u1) * math.Sin(2*math.Pi*u3), math.Sqrt(u1) * math.Cos(2*math.Pi*u3), + }}.Normalize() } -// TestHandleDegenerateSimplex tests the handling of degenerate GJK simplex cases -func TestHandleDegenerateSimplex(t *testing.T) { - // Create mock rigid bodies with shapes - bodyA := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{1, 1, 1}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 0, 0}, - Rotation: mgl64.QuatIdent(), - }, - } +// depthAlong is how far B must move along n to leave A: h_A(n) + h_B(-n). +func depthAlong(a, b *actor.RigidBody, n mgl64.Vec3) float64 { + return a.SupportWorld(n).Dot(n) - b.SupportWorld(n.Mul(-1)).Dot(n) +} - bodyB := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{1, 1, 1}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 1.0, 0}, - Rotation: mgl64.QuatIdent(), - }, +// satBoxBox is the exact penetration of two boxes: the minimum over the 15 separating axes +// (the face normals and edge cross products span every face of their Minkowski difference). +func satBoxBox(a, b *actor.RigidBody) (float64, mgl64.Vec3) { + var axes []mgl64.Vec3 + var edgesA, edgesB [3]mgl64.Vec3 + for i := 0; i < 3; i++ { + var e mgl64.Vec3 + e[i] = 1 + edgesA[i] = a.Transform.Rotation.Rotate(e) + edgesB[i] = b.Transform.Rotation.Rotate(e) + axes = append(axes, edgesA[i], edgesB[i]) } - - t.Run("two_points_simplex", func(t *testing.T) { - simplex := &gjk.Simplex{} - simplex.Points[0] = mgl64.Vec3{0, 0.5, 0} - simplex.Points[1] = mgl64.Vec3{0, 0.6, 0} - simplex.Count = 2 - - result := handleDegenerateSimplex(bodyA, bodyB, simplex) - - // Should return a valid contact constraint - if result.Normal.Len() == 0 { - t.Error("normal should not be zero vector") - } - - // Check that we have contact points (the actual penetration is in the points) - if len(result.Points) == 0 { - t.Errorf("should have at least one contact point") - } - - // Normal should be approximately in the direction from A to B - expectedDir := mgl64.Vec3{0, 1, 0} - if result.Normal.Dot(expectedDir) <= 0 { - t.Errorf("normal should point upward, got %v", result.Normal) - } - }) - - t.Run("one_point_simplex", func(t *testing.T) { - simplex := &gjk.Simplex{} - simplex.Points[0] = mgl64.Vec3{0, 0.5, 0} - simplex.Count = 1 - - result := handleDegenerateSimplex(bodyA, bodyB, simplex) - - // Should use center-based estimation - if result.Normal.Len() == 0 { - t.Error("normal should not be zero vector") - } - - // For degenerate cases, we just check that we have a valid result - if len(result.Points) == 0 { - t.Errorf("should have at least one contact point even in degenerate case") - } - }) - - t.Run("aligned_centers", func(t *testing.T) { - // Same position bodies - bodyA.Transform.Position = mgl64.Vec3{0, 0, 0} - bodyB.Transform.Position = mgl64.Vec3{0, 0, 0} - - simplex := &gjk.Simplex{} - simplex.Count = 1 - - result := handleDegenerateSimplex(bodyA, bodyB, simplex) - - // Should use default upward normal - expectedNormal := mgl64.Vec3{0, 1, 0} - if !vec3ApproxEqual(result.Normal, expectedNormal, 1e-6) { - t.Errorf("normal = %v, want %v for aligned centers", result.Normal, expectedNormal) + for _, x := range edgesA { + for _, y := range edgesB { + if c := x.Cross(y); c.Len() > 1e-9 { + axes = append(axes, c.Normalize()) + } } - }) - - t.Run("close_centers", func(t *testing.T) { - // Very close but not identical centers - bodyA.Transform.Position = mgl64.Vec3{0, 0, 0} - bodyB.Transform.Position = mgl64.Vec3{1e-8, 1e-8, 1e-8} - - simplex := &gjk.Simplex{} - simplex.Count = 1 - - result := handleDegenerateSimplex(bodyA, bodyB, simplex) - - // Should still work and return a valid normal - if result.Normal.Len() == 0 { - t.Error("normal should not be zero vector") + } + best, normal := math.Inf(1), mgl64.Vec3{} + for _, axis := range axes { + for _, n := range [2]mgl64.Vec3{axis, axis.Mul(-1)} { + if d := depthAlong(a, b, n); d < best { + best, normal = d, n + } } - }) + } + return best, normal } -// TestEPA tests the main EPA function -func TestEPA(t *testing.T) { - t.Run("convergence_success", func(t *testing.T) { - // Create two overlapping boxes - bodyA := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{1, 1, 1}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 0, 0}, - Rotation: mgl64.QuatIdent(), - }, - } - - bodyB := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{1, 1, 1}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 1.5, 0}, - Rotation: mgl64.QuatIdent(), - }, - } - - // Create a valid tetrahedron simplex - simplex := &gjk.Simplex{} - simplex.Points[0] = mgl64.Vec3{0.5, 0.5, 0.5} - simplex.Points[1] = mgl64.Vec3{-0.5, 0.5, 0.5} - simplex.Points[2] = mgl64.Vec3{0.5, -0.5, 0.5} - simplex.Points[3] = mgl64.Vec3{0.5, 0.5, -0.5} - simplex.Count = 4 - - result, err := EPA(bodyA, bodyB, simplex) - - if err != nil { - t.Fatalf("EPA failed: %v", err) - } - - // Verify result - if result.Normal.Len() == 0 { - t.Error("normal should not be zero vector") - } - - if len(result.Points) == 0 { - t.Error("should have at least one contact point") - } - - // Normal should point from A to B (upward) - if result.Normal.Y() <= 0 { - t.Errorf("normal should point upward, got %v", result.Normal) - } - - // Check that we have reasonable contact points - if len(result.Points) == 0 { - t.Errorf("should have at least one contact point") - } - }) - - t.Run("degenerate_simplex", func(t *testing.T) { - bodyA := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{1, 1, 1}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 0, 0}, - Rotation: mgl64.QuatIdent(), - }, - } - - bodyB := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{1, 1, 1}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 1.0, 0}, - Rotation: mgl64.QuatIdent(), - }, - } - - // Degenerate simplex with only 2 points - simplex := &gjk.Simplex{} - simplex.Points[0] = mgl64.Vec3{0, 0.5, 0} - simplex.Points[1] = mgl64.Vec3{0, 0.6, 0} - simplex.Count = 2 - - result, err := EPA(bodyA, bodyB, simplex) - - if err != nil { - t.Fatalf("EPA failed: %v", err) - } - - // Should handle degenerate case gracefully - if result.Normal.Len() == 0 { - t.Error("normal should not be zero vector") - } - - if len(result.Points) == 0 { - t.Error("should have at least one contact point even with degenerate simplex") - } - }) - - t.Run("single_point_simplex", func(t *testing.T) { - bodyA := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{1, 1, 1}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 0, 0}, - Rotation: mgl64.QuatIdent(), - }, - } - - bodyB := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{1, 1, 1}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 1.0, 0}, - Rotation: mgl64.QuatIdent(), - }, - } - - // Single point simplex - simplex := &gjk.Simplex{} - simplex.Points[0] = mgl64.Vec3{0, 0.5, 0} - simplex.Count = 1 - - result, err := EPA(bodyA, bodyB, simplex) - - if err != nil { - t.Fatalf("EPA failed: %v", err) +// closestSphereBox is the exact penetration of a sphere (A) into a box (B). +func closestSphereBox(sphere, box *actor.RigidBody) (float64, mgl64.Vec3) { + radius := sphere.Shape.(*actor.Sphere).Radius + h := box.Shape.(*actor.Box).HalfExtents + c := box.Transform.ToLocal(sphere.Transform.Position) + q := mgl64.Vec3{math.Max(-h[0], math.Min(h[0], c[0])), math.Max(-h[1], math.Min(h[1], c[1])), math.Max(-h[2], math.Min(h[2], c[2]))} + var depth float64 + var outward mgl64.Vec3 // from the box towards the sphere, local + if q != c { + depth, outward = radius-c.Sub(q).Len(), c.Sub(q).Normalize() + } else { + best := math.Inf(1) + for i := 0; i < 3; i++ { + for _, s := range [2]float64{1, -1} { + if d := h[i] - s*c[i]; d < best { + best = d + outward = mgl64.Vec3{} + outward[i] = s + } + } } + depth = radius + best + } + return depth, box.Transform.Rotation.Rotate(outward).Mul(-1) +} - // Should handle single point case - if result.Normal.Len() == 0 { - t.Error("normal should not be zero vector") +// place moves b along a random direction until the exact penetration is target. +func place(a, b *actor.RigidBody, direction mgl64.Vec3, target float64, exact func(a, b *actor.RigidBody) (float64, mgl64.Vec3)) bool { + lo, hi := 0.0, 5.0 + for i := 0; i < 80; i++ { + mid := (lo + hi) / 2 + b.Transform.Position = direction.Mul(mid) + if d, _ := exact(a, b); d > target { + lo = mid + } else { + hi = mid } - }) + } + b.Transform.Position = direction.Mul(hi) + d, _ := exact(a, b) + return d > 0 +} - t.Run("convergence_with_rotation", func(t *testing.T) { - // Test with rotated boxes - bodyA := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{1, 1, 1}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 0, 0}, - Rotation: mgl64.QuatRotate(math.Pi/4, mgl64.Vec3{0, 1, 0}), - }, - } +func runEPA(t *testing.T, a, b *actor.RigidBody, margin float64) Result { + t.Helper() + simplex := &gjk.Simplex{} + if !gjk.GJKMargin(a, b, margin, simplex) { + t.Fatalf("GJK found no overlap: a=%v b=%v", a.Transform, b.Transform) + } + result, err := EPA(a, b, simplex, margin) + if err != nil { + t.Fatalf("EPA: %v", err) + } + return result +} - bodyB := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{1, 1, 1}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 1.5, 0}, - Rotation: mgl64.QuatRotate(math.Pi/6, mgl64.Vec3{0, 1, 0}), - }, +// EPA is exact against SAT on random box pairs, shallow and deep. v0.2.0 was off by up to +// 0.3 mm on the depth. +func TestEPABoxBoxMatchesSAT(t *testing.T) { + r := rand.New(rand.NewSource(11)) + for i := 0; i < 400; i++ { + a := body(mgl64.Vec3{}, randomRotation(r), &actor.Box{HalfExtents: mgl64.Vec3{0.1 + 0.5*r.Float64(), 0.1 + 0.5*r.Float64(), 0.1 + 0.5*r.Float64()}}) + b := body(mgl64.Vec3{}, randomRotation(r), &actor.Box{HalfExtents: mgl64.Vec3{0.1 + 0.5*r.Float64(), 0.1 + 0.5*r.Float64(), 0.1 + 0.5*r.Float64()}}) + direction := mgl64.Vec3{r.NormFloat64(), r.NormFloat64(), r.NormFloat64()}.Normalize() + target := 1e-4 + 0.2*r.Float64()*r.Float64() + if !place(a, b, direction, target, satBoxBox) { + continue + } + want, wantNormal := satBoxBox(a, b) + got := runEPA(t, a, b, 0) + if math.Abs(got.Depth-want) > 1e-6 { + t.Fatalf("pair %d: depth %.9f, SAT %.9f", i, got.Depth, want) + } + // Depth along EPA's normal is the depth itself: the normal separates the boxes. + if d := depthAlong(a, b, got.Normal); math.Abs(d-want) > 1e-6 { + t.Fatalf("pair %d: moving B by %.9f along %v leaves %.2e of overlap (SAT normal %v)", i, got.Depth, got.Normal, d-want, wantNormal) } + } +} - // Create a valid simplex - simplex := &gjk.Simplex{} - simplex.Points[0] = mgl64.Vec3{0.5, 0.5, 0.5} - simplex.Points[1] = mgl64.Vec3{-0.5, 0.5, 0.5} - simplex.Points[2] = mgl64.Vec3{0.5, -0.5, 0.5} - simplex.Points[3] = mgl64.Vec3{0.5, 0.5, -0.5} - simplex.Count = 4 - - result, err := EPA(bodyA, bodyB, simplex) - - if err != nil { - t.Fatalf("EPA failed with rotation: %v", err) +// Sphere against box is exact too (v0.2.0: up to 2.9° and 0.9 mm off). +func TestEPASphereBoxMatchesClosestPoint(t *testing.T) { + r := rand.New(rand.NewSource(12)) + for i := 0; i < 400; i++ { + sphere := body(mgl64.Vec3{}, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.1 + 0.5*r.Float64()}) + box := body(mgl64.Vec3{}, randomRotation(r), &actor.Box{HalfExtents: mgl64.Vec3{0.1 + 0.5*r.Float64(), 0.1 + 0.5*r.Float64(), 0.1 + 0.5*r.Float64()}}) + direction := mgl64.Vec3{r.NormFloat64(), r.NormFloat64(), r.NormFloat64()}.Normalize() + if !place(sphere, box, direction, 1e-4+0.02*r.Float64(), closestSphereBox) { + continue } - - // Should still converge - if result.Normal.Len() == 0 { - t.Error("normal should not be zero vector with rotation") + want, wantNormal := closestSphereBox(sphere, box) + got := runEPA(t, sphere, box, 0) + if math.Abs(got.Depth-want) > 1e-6 { + t.Fatalf("pair %d: depth %.9f, want %.9f", i, got.Depth, want) } - - if len(result.Points) == 0 { - t.Error("should have contact points with rotation") + if angle := math.Acos(math.Min(1, got.Normal.Dot(wantNormal))) * 180 / math.Pi; angle > 0.1 { + t.Fatalf("pair %d: normal %.4f° off", i, angle) } - }) + } } -// TestEPAIntegration tests the integration between GJK and EPA -func TestEPAIntegration(t *testing.T) { - t.Run("box_box_collision", func(t *testing.T) { - // Create two boxes that are clearly overlapping - bodyA := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{1, 1, 1}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 0, 0}, - Rotation: mgl64.QuatIdent(), - }, - } - - bodyB := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{1, 1, 1}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 1.5, 0}, - Rotation: mgl64.QuatIdent(), - }, - } - - // First run GJK to get simplex +// Capsule against box (a segment against a box, rounded by the radius) goes through EPA: +// the depth along its normal is the penetration, within the convergence tolerance. +func TestEPACapsuleBoxIsMinimal(t *testing.T) { + r := rand.New(rand.NewSource(13)) + for i := 0; i < 300; i++ { + capsule := body(mgl64.Vec3{}, randomRotation(r), &actor.Capsule{HalfHeight: 0.05 + 0.5*r.Float64(), Radius: 0.05 + 0.3*r.Float64()}) + box := body(mgl64.Vec3{r.NormFloat64(), r.NormFloat64(), r.NormFloat64()}.Mul(0.3), randomRotation(r), &actor.Box{HalfExtents: mgl64.Vec3{0.1 + 0.5*r.Float64(), 0.1 + 0.5*r.Float64(), 0.1 + 0.5*r.Float64()}}) simplex := &gjk.Simplex{} - if !gjk.GJK(bodyA, bodyB, simplex) { - t.Skip("GJK did not detect collision, skipping EPA test") - } - - if simplex.Count < 4 { - t.Skip("GJK returned degenerate simplex, skipping") + if !gjk.GJK(capsule, box, simplex) { + continue } - - // Then run EPA - epaResult, err := EPA(bodyA, bodyB, simplex) - + got, err := EPA(capsule, box, simplex, 0) if err != nil { - t.Fatalf("EPA failed: %v", err) + t.Fatal(err) } - - // Verify integration results - if epaResult.Normal.Len() == 0 { - t.Error("EPA result normal should not be zero") - } - - if len(epaResult.Points) == 0 { - t.Error("EPA should return at least one contact point") + if d := depthAlong(capsule, box, got.Normal); math.Abs(d-got.Depth) > 1e-6 { + t.Fatalf("pair %d: depth %.9f but %.9f along its normal", i, got.Depth, d) } - - // The normal should be consistent with collision direction - expectedNormal := mgl64.Vec3{0, 1, 0} - if epaResult.Normal.Dot(expectedNormal) <= 0 { - t.Errorf("EPA normal %v should be in same direction as expected %v", - epaResult.Normal, expectedNormal) - } - - // Check penetration in contact points - if len(epaResult.Points) > 0 { - for _, point := range epaResult.Points { - if point.Penetration <= 0 || point.Penetration > 2.0 { - t.Errorf("penetration should be reasonable, got %v", point.Penetration) - } + // No direction separates them with less: sample around the normal. + for k := 0; k < 64; k++ { + n := got.Normal.Add(mgl64.Vec3{r.NormFloat64(), r.NormFloat64(), r.NormFloat64()}.Mul(0.2)).Normalize() + if d := depthAlong(capsule, box, n); d < got.Depth-1e-6 { + t.Fatalf("pair %d: direction %v separates with %.9f < EPA %.9f", i, n, d, got.Depth) } } - }) - - t.Run("sphere_sphere_collision", func(t *testing.T) { - bodyA := &actor.RigidBody{ - Shape: &actor.Sphere{Radius: 1.0}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 0, 0}, - Rotation: mgl64.QuatIdent(), - }, - } - - bodyB := &actor.RigidBody{ - Shape: &actor.Sphere{Radius: 1.0}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 1.9, 0}, - Rotation: mgl64.QuatIdent(), - }, - } - - // Run GJK - simplex := &gjk.Simplex{} - if !gjk.GJK(bodyA, bodyB, simplex) { - t.Skip("GJK did not detect collision") - } - - // Run EPA - epaResult, err := EPA(bodyA, bodyB, simplex) - - if err != nil { - t.Fatalf("EPA failed: %v", err) - } - - // Spheres should have single contact point - if len(epaResult.Points) != 1 { - t.Errorf("Expected 1 contact point for spheres, got %d", len(epaResult.Points)) - } - - // Normal should be in the correct direction - expectedNormal := mgl64.Vec3{0, 1, 0} - if epaResult.Normal.Dot(expectedNormal) <= 0 { - t.Errorf("EPA normal %v should be in same direction as expected %v", - epaResult.Normal, expectedNormal) - } - }) + } +} - t.Run("rotated_boxes_collision", func(t *testing.T) { - bodyA := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{1, 1, 1}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 0, 0}, - Rotation: mgl64.QuatRotate(math.Pi/6, mgl64.Vec3{0, 1, 0}), - }, - } +// The witness points are the deepest points of each shape: WitnessA - WitnessB = depth·n, +// WitnessA on A's surface along n, WitnessB on B's surface along -n. +func TestEPAWitnessPoints(t *testing.T) { + a := body(mgl64.Vec3{}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{1, 1, 1}}) + b := body(mgl64.Vec3{0.3, 1.9, -0.2}, mgl64.QuatIdent(), &actor.Sphere{Radius: 1}) + got := runEPA(t, a, b, 0) + if !near(got.Normal, mgl64.Vec3{0, 1, 0}, 1e-6) || math.Abs(got.Depth-0.1) > 1e-6 { + t.Fatalf("normal %v depth %f, want +Y 0.1", got.Normal, got.Depth) + } + // On a curved surface the witness converges like sqrt(tolerance): a few micrometres. + if !near(got.WitnessB, mgl64.Vec3{0.3, 0.9, -0.2}, 1e-5) { + t.Errorf("witness on the sphere %v, want its lowest point (0.3, 0.9, -0.2)", got.WitnessB) + } + if d := got.WitnessA.Sub(got.WitnessB).Sub(got.Normal.Mul(got.Depth)).Len(); d > 1e-6 { + t.Errorf("WitnessA - WitnessB is %.2e from depth·normal", d) + } +} - bodyB := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{1, 1, 1}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 1.8, 0}, - Rotation: mgl64.QuatRotate(math.Pi/4, mgl64.Vec3{0, 1, 0}), - }, - } +// With a margin, shapes up to margin apart overlap: depth = margin - distance. +func TestEPAMargin(t *testing.T) { + a := body(mgl64.Vec3{}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{1, 1, 1}}) + b := body(mgl64.Vec3{0, 2.005, 0}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{1, 1, 1}}) + simplex := &gjk.Simplex{} + if gjk.GJK(a, b, simplex) { + t.Fatal("boxes 5 mm apart overlap") + } + got := runEPA(t, a, b, 0.02) + if math.Abs(got.Depth-0.015) > 1e-6 || !near(got.Normal, mgl64.Vec3{0, 1, 0}, 1e-6) { + t.Errorf("depth %f normal %v, want 0.015 along +Y", got.Depth, got.Normal) + } +} - // Run GJK - simplex := &gjk.Simplex{} - if !gjk.GJK(bodyA, bodyB, simplex) { - t.Skip("GJK did not detect collision") - } +// Exactly touching shapes (the origin on the Minkowski boundary) still produce a result. +func TestEPATouching(t *testing.T) { + a := body(mgl64.Vec3{}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{1, 1, 1}}) + b := body(mgl64.Vec3{0.5, 2, 0.5}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{1, 1, 1}}) + got := runEPA(t, a, b, 0.01) + if math.Abs(got.Depth-0.01) > 1e-6 { + t.Errorf("touching boxes with a 1 cm margin: depth %f, want 0.01", got.Depth) + } +} - // Run EPA - epaResult, err := EPA(bodyA, bodyB, simplex) +func TestEPARejectsIncompleteSimplex(t *testing.T) { + a := body(mgl64.Vec3{}, mgl64.QuatIdent(), &actor.Sphere{Radius: 1}) + simplex := &gjk.Simplex{Count: 2} + if _, err := EPA(a, a, simplex, 0); err == nil { + t.Error("EPA accepted a 2-point simplex") + } +} - if err != nil { - t.Fatalf("EPA failed: %v", err) +func TestBarycentric(t *testing.T) { + a, b, c := mgl64.Vec3{0, 0, 0}, mgl64.Vec3{1, 0, 0}, mgl64.Vec3{0, 1, 0} + for _, tc := range []struct { + p mgl64.Vec3 + u, v, w float64 + }{ + {mgl64.Vec3{0, 0, 0}, 1, 0, 0}, + {mgl64.Vec3{0.25, 0.25, 0}, 0.5, 0.25, 0.25}, + {mgl64.Vec3{2, 0, 0}, 0, 1, 0}, // clamped outside + {mgl64.Vec3{-1, -1, 0}, 1, 0, 0}, // clamped outside + } { + u, v, w := barycentric(tc.p, a, b, c) + if math.Abs(u-tc.u) > 1e-12 || math.Abs(v-tc.v) > 1e-12 || math.Abs(w-tc.w) > 1e-12 { + t.Errorf("barycentric(%v) = %v %v %v, want %v %v %v", tc.p, u, v, w, tc.u, tc.v, tc.w) } + } +} - // Should work with rotation - if len(epaResult.Points) == 0 { - t.Error("should have contact points with rotation") +func BenchmarkEPABoxBox(b *testing.B) { + boxA := body(mgl64.Vec3{}, mgl64.QuatRotate(0.3, mgl64.Vec3{1, 1, 0}.Normalize()), &actor.Box{HalfExtents: mgl64.Vec3{0.5, 0.5, 0.5}}) + boxB := body(mgl64.Vec3{0.2, 0.9, 0.1}, mgl64.QuatRotate(0.7, mgl64.Vec3{0, 1, 1}.Normalize()), &actor.Box{HalfExtents: mgl64.Vec3{0.5, 0.5, 0.5}}) + simplex := &gjk.Simplex{} + b.ReportAllocs() + for i := 0; i < b.N; i++ { + simplex.Reset() + if gjk.GJKMargin(boxA, boxB, 0.02, simplex) { + _, _ = EPA(boxA, boxB, simplex, 0.02) } - }) + } } + +func near(a, b mgl64.Vec3, tolerance float64) bool { return a.Sub(b).Len() <= tolerance } diff --git a/epa/face.go b/epa/face.go deleted file mode 100644 index f4286cc..0000000 --- a/epa/face.go +++ /dev/null @@ -1,53 +0,0 @@ -package epa - -import ( - "github.com/go-gl/mathgl/mgl64" -) - -// Face represents a triangular face of the polytope in EPA. -// Each face has 3 vertices, an outward-pointing normal, and distance to origin. -// -// This struct is now used by PolytopeBuilder for zero-allocation EPA. -// The old pointer-based approach with facePool has been replaced. -type Face struct { - Points [3]mgl64.Vec3 // The 3 vertices of the triangle - Normal mgl64.Vec3 // Outward-pointing normal - Distance float64 // Distance from origin to the face plane -} - -// Edge represents an edge between two vertices. -// Kept for backward compatibility, but no longer used with PolytopeBuilder. -// PolytopeBuilder uses EdgeEntry instead with occurrence counting. -type Edge struct { - A, B mgl64.Vec3 -} - -// compareVec3 compares two vectors lexicographically (x, then y, then z). -// Returns: -// -// -1 if a < b -// 0 if a == b -// +1 if a > b -// -// Used by PolytopeBuilder for edge normalization and point deduplication. -func compareVec3(a, b mgl64.Vec3) int { - if a[0] != b[0] { - if a[0] < b[0] { - return -1 - } - return 1 - } - if a[1] != b[1] { - if a[1] < b[1] { - return -1 - } - return 1 - } - if a[2] != b[2] { - if a[2] < b[2] { - return -1 - } - return 1 - } - return 0 -} diff --git a/epa/face_test.go b/epa/face_test.go deleted file mode 100644 index cdae002..0000000 --- a/epa/face_test.go +++ /dev/null @@ -1,682 +0,0 @@ -package epa - -import ( - "fmt" - "math" - "testing" - - "github.com/akmonengine/feather/gjk" - "github.com/go-gl/mathgl/mgl64" -) - -// Helper functions for testing -func vec3ApproxEqual(a, b mgl64.Vec3, tolerance float64) bool { - return math.Abs(a.X()-b.X()) < tolerance && - math.Abs(a.Y()-b.Y()) < tolerance && - math.Abs(a.Z()-b.Z()) < tolerance -} - -func isNormalized(v mgl64.Vec3, tolerance float64) bool { - length := v.Len() - return math.Abs(length-1.0) < tolerance -} - -// normalizeEdge normalizes an edge so that A < B lexicographically -// This is the same logic used in PolytopeBuilder.findBoundaryEdges -func normalizeEdge(edge Edge) Edge { - a, b := edge.A, edge.B - if compareVec3(a, b) > 0 { - return Edge{A: b, B: a} - } - return Edge{A: a, B: b} -} - -// TestCompareVec3 tests lexicographic comparison of vectors -func TestCompareVec3(t *testing.T) { - tests := []struct { - name string - a mgl64.Vec3 - b mgl64.Vec3 - expected int - }{ - { - name: "equal vectors", - a: mgl64.Vec3{1, 2, 3}, - b: mgl64.Vec3{1, 2, 3}, - expected: 0, - }, - { - name: "a < b on x", - a: mgl64.Vec3{1, 2, 3}, - b: mgl64.Vec3{2, 2, 3}, - expected: -1, - }, - { - name: "a > b on x", - a: mgl64.Vec3{2, 2, 3}, - b: mgl64.Vec3{1, 2, 3}, - expected: 1, - }, - { - name: "a < b on y (x equal)", - a: mgl64.Vec3{1, 1, 3}, - b: mgl64.Vec3{1, 2, 3}, - expected: -1, - }, - { - name: "a > b on y (x equal)", - a: mgl64.Vec3{1, 3, 3}, - b: mgl64.Vec3{1, 2, 3}, - expected: 1, - }, - { - name: "a < b on z (x,y equal)", - a: mgl64.Vec3{1, 2, 2}, - b: mgl64.Vec3{1, 2, 3}, - expected: -1, - }, - { - name: "a > b on z (x,y equal)", - a: mgl64.Vec3{1, 2, 4}, - b: mgl64.Vec3{1, 2, 3}, - expected: 1, - }, - } - - for _, tt := range tests { - t.Run(tt.name, func(t *testing.T) { - result := compareVec3(tt.a, tt.b) - if result != tt.expected { - t.Errorf("compareVec3(%v, %v) = %d, want %d", tt.a, tt.b, result, tt.expected) - } - }) - } -} - -// TestNormalizeEdge tests edge normalization -func TestNormalizeEdge(t *testing.T) { - tests := []struct { - name string - edge Edge - expected Edge - }{ - { - name: "already normalized (A < B)", - edge: Edge{A: mgl64.Vec3{0, 0, 0}, B: mgl64.Vec3{1, 0, 0}}, - expected: Edge{A: mgl64.Vec3{0, 0, 0}, B: mgl64.Vec3{1, 0, 0}}, - }, - { - name: "needs swap (A > B)", - edge: Edge{A: mgl64.Vec3{1, 0, 0}, B: mgl64.Vec3{0, 0, 0}}, - expected: Edge{A: mgl64.Vec3{0, 0, 0}, B: mgl64.Vec3{1, 0, 0}}, - }, - { - name: "same point", - edge: Edge{A: mgl64.Vec3{1, 1, 1}, B: mgl64.Vec3{1, 1, 1}}, - expected: Edge{A: mgl64.Vec3{1, 1, 1}, B: mgl64.Vec3{1, 1, 1}}, - }, - } - - for _, tt := range tests { - t.Run(tt.name, func(t *testing.T) { - result := normalizeEdge(tt.edge) - if !vec3ApproxEqual(result.A, tt.expected.A, 1e-9) || !vec3ApproxEqual(result.B, tt.expected.B, 1e-9) { - t.Errorf("normalizeEdge(%v) = %v, want %v", tt.edge, result, tt.expected) - } - }) - } -} - -// TestCreateFaceOutward tests face creation with outward normal -func TestCreateFaceOutward(t *testing.T) { - tests := []struct { - name string - a, b, c mgl64.Vec3 - oppositePoint mgl64.Vec3 - checkNormal bool // whether to check normal direction - }{ - { - name: "triangle on xy plane, opposite below", - a: mgl64.Vec3{1, 0, 0}, - b: mgl64.Vec3{0, 1, 0}, - c: mgl64.Vec3{0, 0, 0}, - oppositePoint: mgl64.Vec3{0, 0, -1}, - checkNormal: true, - }, - { - name: "triangle on xz plane", - a: mgl64.Vec3{1, 0, 0}, - b: mgl64.Vec3{0, 0, 1}, - c: mgl64.Vec3{0, 0, 0}, - oppositePoint: mgl64.Vec3{0, -1, 0}, - checkNormal: true, - }, - { - name: "degenerate triangle (collinear points)", - a: mgl64.Vec3{0, 0, 0}, - b: mgl64.Vec3{1, 0, 0}, - c: mgl64.Vec3{2, 0, 0}, - oppositePoint: mgl64.Vec3{0, 1, 0}, - checkNormal: false, // degenerate case - }, - } - - for _, tt := range tests { - t.Run(tt.name, func(t *testing.T) { - // Use PolytopeBuilder to create face - builder := &PolytopeBuilder{} - face := builder.createFaceOutward(tt.a, tt.b, tt.c, tt.oppositePoint) - - // Check that points are stored correctly - if !vec3ApproxEqual(face.Points[0], tt.a, 1e-9) { - t.Errorf("face.Points[0] = %v, want %v", face.Points[0], tt.a) - } - if !vec3ApproxEqual(face.Points[1], tt.b, 1e-9) { - t.Errorf("face.Points[1] = %v, want %v", face.Points[1], tt.b) - } - if !vec3ApproxEqual(face.Points[2], tt.c, 1e-9) { - t.Errorf("face.Points[2] = %v, want %v", face.Points[2], tt.c) - } - - if tt.checkNormal { - // Check that normal is normalized - if !isNormalized(face.Normal, 1e-6) { - t.Errorf("normal is not normalized: length = %v", face.Normal.Len()) - } - - // Check that normal points away from opposite point - toOpposite := tt.oppositePoint.Sub(tt.a) - dotProduct := face.Normal.Dot(toOpposite) - if dotProduct > 0 { - t.Errorf("normal points toward opposite point: dot = %v (should be <= 0)", dotProduct) - } - - // Check that distance is positive - if face.Distance < 0 { - t.Errorf("distance is negative: %v", face.Distance) - } - - // Distance should be at least the minimum threshold - if face.Distance < 0.0001 { - t.Logf("distance clamped to minimum: %v", face.Distance) - } - } else { - // Degenerate case should have default values - if face.Distance < 0.0001 { - t.Logf("degenerate triangle detected, distance set to minimum") - } - } - }) - } -} - -// TestBuildInitialFaces tests initial tetrahedron face creation -func TestBuildInitialFaces(t *testing.T) { - tests := []struct { - name string - simplex []mgl64.Vec3 - minFaces int - maxFaces int - expectFilter bool // whether we expect filtering of degenerate faces - }{ - { - name: "regular tetrahedron", - simplex: []mgl64.Vec3{ - {1, 0, 0}, - {0, 1, 0}, - {0, 0, 1}, - {0, 0, 0}, - }, - minFaces: 3, - maxFaces: 4, - expectFilter: false, - }, - { - name: "flat tetrahedron (4 coplanar points)", - simplex: []mgl64.Vec3{ - {0, 0, 0}, - {1, 0, 0}, - {0, 1, 0}, - {0.5, 0.5, 0}, - }, - minFaces: 3, // Safety returns all 4 if < 3 after filtering - maxFaces: 4, - expectFilter: true, - }, - { - name: "origin-centered tetrahedron", - simplex: []mgl64.Vec3{ - {1, 1, 1}, - {-1, -1, 1}, - {-1, 1, -1}, - {1, -1, -1}, - }, - minFaces: 3, - maxFaces: 4, - expectFilter: false, - }, - } - - for _, tt := range tests { - t.Run(tt.name, func(t *testing.T) { - // Create a simplex for testing - simplex := &gjk.Simplex{} - // Copy points to simplex - for i, point := range tt.simplex { - if i < len(simplex.Points) { - simplex.Points[i] = point - } - } - simplex.Count = len(tt.simplex) - - // Use PolytopeBuilder to build initial faces - builder := &PolytopeBuilder{} - err := builder.BuildInitialFaces(simplex) - if err != nil { - t.Fatalf("BuildInitialFaces failed: %v", err) - } - - // Get the faces from the builder - faces := builder.faces[:len(builder.faces)] - - // Check number of faces - if len(faces) < tt.minFaces || len(faces) > tt.maxFaces { - t.Errorf("BuildInitialFaces() returned %d faces, want between %d and %d", - len(faces), tt.minFaces, tt.maxFaces) - } - - // All faces should have valid distance - for i, face := range faces { - if face.Distance < 0 { - t.Errorf("face %d has negative distance: %v", i, face.Distance) - } - - // Check that normal is normalized (unless degenerate) - if face.Distance >= 0.0001 && !isNormalized(face.Normal, 1e-6) { - t.Errorf("face %d has non-normalized normal: length = %v", i, face.Normal.Len()) - } - } - }) - } -} - -// TestFindClosestFaceIndex tests finding the face closest to origin -func TestFindClosestFaceIndex(t *testing.T) { - tests := []struct { - name string - faces []Face - expectedIndex int - }{ - { - name: "single face", - faces: []Face{ - {Distance: 1.0}, - }, - expectedIndex: 0, - }, - { - name: "closest is first", - faces: []Face{ - {Distance: 0.5}, - {Distance: 1.0}, - {Distance: 2.0}, - }, - expectedIndex: 0, - }, - { - name: "closest is middle", - faces: []Face{ - {Distance: 2.0}, - {Distance: 0.3}, - {Distance: 1.0}, - }, - expectedIndex: 1, - }, - { - name: "closest is last", - faces: []Face{ - {Distance: 2.0}, - {Distance: 1.0}, - {Distance: 0.1}, - }, - expectedIndex: 2, - }, - } - - for _, tt := range tests { - t.Run(tt.name, func(t *testing.T) { - // Use PolytopeBuilder to find closest face - builder := &PolytopeBuilder{} - // Copy faces to builder using append - builder.faces = append(builder.faces, tt.faces...) - - result := builder.FindClosestFaceIndex() - if result != tt.expectedIndex { - t.Errorf("FindClosestFaceIndex() = %d, want %d", result, tt.expectedIndex) - } - }) - } -} - -// TestFindBoundaryEdges tests boundary edge detection -func TestFindBoundaryEdges(t *testing.T) { - tests := []struct { - name string - faces []Face - visibleIndices []int - minEdges int - maxEdges int - }{ - { - name: "single visible triangle", - faces: []Face{ - { - Points: [3]mgl64.Vec3{ - {0, 0, 0}, - {1, 0, 0}, - {0, 1, 0}, - }, - }, - }, - visibleIndices: []int{0}, - minEdges: 3, // All 3 edges are boundary - maxEdges: 3, - }, - { - name: "two adjacent triangles, one visible", - faces: []Face{ - { - Points: [3]mgl64.Vec3{ - {0, 0, 0}, - {1, 0, 0}, - {0, 1, 0}, - }, - }, - { - Points: [3]mgl64.Vec3{ - {0, 0, 0}, - {0, 1, 0}, - {0, 0, 1}, - }, - }, - }, - visibleIndices: []int{0}, - minEdges: 2, // Two edges are unique to face 0 - maxEdges: 3, - }, - { - name: "tetrahedron, two opposite faces visible", - faces: []Face{ - { - Points: [3]mgl64.Vec3{ - {0, 0, 0}, - {1, 0, 0}, - {0, 1, 0}, - }, - }, - { - Points: [3]mgl64.Vec3{ - {0, 0, 1}, - {1, 0, 1}, - {0, 1, 1}, - }, - }, - }, - visibleIndices: []int{0, 1}, - minEdges: 6, // All edges are boundary (no shared edges) - maxEdges: 6, - }, - } - - for _, tt := range tests { - t.Run(tt.name, func(t *testing.T) { - // Use PolytopeBuilder to find boundary edges - builder := &PolytopeBuilder{} - - // Copy faces to builder using append - builder.faces = append(builder.faces, tt.faces...) - // Copy visible indices to builder using append - builder.visibleIndices = append(builder.visibleIndices, tt.visibleIndices...) - - // Find boundary edges - err := builder.findBoundaryEdges() - if err != nil { - t.Fatalf("findBoundaryEdges failed: %v", err) - } - - // Get edges from builder - edges := builder.edges[:len(builder.edges)] - - if len(edges) < tt.minEdges || len(edges) > tt.maxEdges { - t.Errorf("findBoundaryEdges() returned %d edges, want between %d and %d", - len(edges), tt.minEdges, tt.maxEdges) - } - - // All boundary edges should be normalized - for _, edge := range edges { - normalized := normalizeEdge(Edge{A: edge.A, B: edge.B}) - if !vec3ApproxEqual(edge.A, normalized.A, 1e-9) || !vec3ApproxEqual(edge.B, normalized.B, 1e-9) { - t.Logf("edge not in normalized form: %v (normalized: %v)", Edge{A: edge.A, B: edge.B}, normalized) - } - } - }) - } -} - -// TestAddPointAndRebuildFaces tests polytope expansion -func TestAddPointAndRebuildFaces(t *testing.T) { - t.Run("add point to tetrahedron", func(t *testing.T) { - // Start with a simple tetrahedron - initialFaces := []Face{ - { - Points: [3]mgl64.Vec3{{1, 0, 0}, {0, 1, 0}, {0, 0, 0}}, - Normal: mgl64.Vec3{0, 0, 1}, - Distance: 0.1, - }, - { - Points: [3]mgl64.Vec3{{0, 0, 1}, {1, 0, 1}, {0, 1, 1}}, - Normal: mgl64.Vec3{0, 0, -1}, - Distance: 0.1, - }, - { - Points: [3]mgl64.Vec3{{0, 0, 0}, {0, 0, 1}, {1, 0, 0}}, - Normal: mgl64.Vec3{0, 1, 0}, - Distance: 0.1, - }, - { - Points: [3]mgl64.Vec3{{0, 1, 0}, {0, 1, 1}, {1, 1, 0}}, - Normal: mgl64.Vec3{0, -1, 0}, - Distance: 0.1, - }, - } - - // Use PolytopeBuilder for polytope expansion - builder := &PolytopeBuilder{} - - // Copy initial faces to builder using append - builder.faces = append(builder.faces, initialFaces...) - - support := mgl64.Vec3{2, 0.5, 0.5} - closestIndex := 0 - - err := builder.AddPointAndRebuildFaces(support, closestIndex) - if err != nil { - t.Fatalf("AddPointAndRebuildFaces failed: %v", err) - } - - // Get faces from builder - faces := builder.faces[:len(builder.faces)] - - // Should still have faces after rebuild - if len(faces) == 0 { - t.Error("AddPointAndRebuildFaces() resulted in no faces (safety check failed)") - } - - // Check that all faces have valid normals and distances - for i, face := range faces { - if face.Distance < 0 { - t.Errorf("face %d has negative distance after rebuild: %v", i, face.Distance) - } - } - }) - - t.Run("remove all faces safety check", func(t *testing.T) { - // Single face that would be removed - initialFaces := []Face{ - { - Points: [3]mgl64.Vec3{{1, 0, 0}, {0, 1, 0}, {0, 0, 0}}, - Normal: mgl64.Vec3{0, 0, 1}, - Distance: 0.5, - }, - } - - // Use PolytopeBuilder for polytope expansion - builder := &PolytopeBuilder{} - - // Copy initial faces to builder using append - builder.faces = append(builder.faces, initialFaces...) - - // Point that would make all faces visible - support := mgl64.Vec3{0, 0, 2} - closestIndex := 0 - - err := builder.AddPointAndRebuildFaces(support, closestIndex) - if err != nil { - t.Fatalf("AddPointAndRebuildFaces failed: %v", err) - } - - // Get faces from builder - faces := builder.faces[:len(builder.faces)] - - // Safety check should ensure at least one face remains - if len(faces) == 0 { - t.Error("safety check failed: no faces remain after rebuild") - } - }) - - t.Run("no visible faces case", func(t *testing.T) { - initialFaces := []Face{ - { - Points: [3]mgl64.Vec3{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}, - Normal: mgl64.Vec3{1, 1, 1}.Normalize(), - Distance: 1.0, - }, - } - - // Use PolytopeBuilder for polytope expansion - builder := &PolytopeBuilder{} - - // Copy initial faces to builder using append - builder.faces = append(builder.faces, initialFaces...) - // Point behind the face (not visible) - support := mgl64.Vec3{-1, -1, -1} - closestIndex := 0 - - initialLen := len(builder.faces) - err := builder.AddPointAndRebuildFaces(support, closestIndex) - if err != nil { - t.Fatalf("AddPointAndRebuildFaces failed: %v", err) - } - - // Get faces from builder - faces := builder.faces[:len(builder.faces)] - - // Should have modified the polytope - if len(faces) == 0 { - t.Error("no faces after rebuild") - } - - // Verify we have at least as many faces as before (or the safety fallback) - if len(faces) < initialLen && len(faces) != 1 { - t.Logf("face count changed from %d to %d", initialLen, len(faces)) - } - }) -} - -// Benchmark tests -func BenchmarkCreateFaceOutward(b *testing.B) { - a := mgl64.Vec3{1, 0, 0} - c := mgl64.Vec3{0, 1, 0} - d := mgl64.Vec3{0, 0, 0} - opposite := mgl64.Vec3{0, 0, 1} - builder := &PolytopeBuilder{} - - b.ResetTimer() - for i := 0; i < b.N; i++ { - builder.createFaceOutward(a, c, d, opposite) - } -} - -func BenchmarkBuildInitialFaces(b *testing.B) { - simplex := &gjk.Simplex{} - simplex.Points[0] = mgl64.Vec3{1, 0, 0} - simplex.Points[1] = mgl64.Vec3{0, 1, 0} - simplex.Points[2] = mgl64.Vec3{0, 0, 1} - simplex.Points[3] = mgl64.Vec3{0, 0, 0} - simplex.Count = 4 - builder := &PolytopeBuilder{} - - b.ResetTimer() - for i := 0; i < b.N; i++ { - builder.Reset() - err := builder.BuildInitialFaces(simplex) - - if err != nil { - fmt.Printf("error building initial faces: %v", err) - } - } -} - -func BenchmarkFindBoundaryEdges(b *testing.B) { - faces := []Face{ - {Points: [3]mgl64.Vec3{{0, 0, 0}, {1, 0, 0}, {0, 1, 0}}}, - {Points: [3]mgl64.Vec3{{0, 0, 0}, {0, 1, 0}, {0, 0, 1}}}, - {Points: [3]mgl64.Vec3{{0, 0, 0}, {0, 0, 1}, {1, 0, 0}}}, - {Points: [3]mgl64.Vec3{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}}, - } - visibleIndices := []int{0, 1} - builder := &PolytopeBuilder{} - - // Setup builder - for i, face := range faces { - if i < len(builder.faces) { - builder.faces[i] = face - } - } - for i, idx := range visibleIndices { - if i < len(builder.visibleIndices) { - builder.visibleIndices[i] = idx - } - } - - b.ResetTimer() - for i := 0; i < b.N; i++ { - err := builder.findBoundaryEdges() - if err != nil { - fmt.Printf("error finding boundary edges: %v", err) - } - } -} - -func BenchmarkAddPointAndRebuildFaces(b *testing.B) { - support := mgl64.Vec3{2, 0.5, 0.5} - closestIndex := 0 - builder := &PolytopeBuilder{} - - b.ResetTimer() - for i := 0; i < b.N; i++ { - b.StopTimer() - // Setup initial faces - builder.Reset() - builder.faces = append(builder.faces, - Face{Points: [3]mgl64.Vec3{{1, 0, 0}, {0, 1, 0}, {0, 0, 0}}, Normal: mgl64.Vec3{0, 0, 1}, Distance: 0.1}, - Face{Points: [3]mgl64.Vec3{{0, 0, 1}, {1, 0, 1}, {0, 1, 1}}, Normal: mgl64.Vec3{0, 0, -1}, Distance: 0.1}, - Face{Points: [3]mgl64.Vec3{{0, 0, 0}, {0, 0, 1}, {1, 0, 0}}, Normal: mgl64.Vec3{0, 1, 0}, Distance: 0.1}) - b.StartTimer() - - err := builder.AddPointAndRebuildFaces(support, closestIndex) - if err != nil { - fmt.Printf("error adding faces: %v", err) - } - } -} diff --git a/epa/manifold.go b/epa/manifold.go index f2177db..ed450b0 100644 --- a/epa/manifold.go +++ b/epa/manifold.go @@ -9,479 +9,341 @@ import ( "github.com/go-gl/mathgl/mgl64" ) -// Manifold generation configuration constants const ( - // maxContactPoints is the maximum number of contact points in a manifold. - // Limited to 4 for constraint solver stability (see Erin Catto, GDC 2007). - maxContactPoints = 4 - - // maxBufferSize is the size of pre-allocated working buffers. - // Must be >= maxContactPoints * 2 to handle worst-case Sutherland-Hodgman clipping. + // maxBufferSize: a quad clipped by 4 planes has at most 8 vertices maxBufferSize = 8 -) -// Numerical tolerance constants for geometric computation stability -const ( - // epsilonColinear is the tolerance for detecting colinear edges. - // If |edge.Cross(normal)| < epsilonColinear, the edge is parallel to the normal. - epsilonColinear = 1e-6 + // minFaceAlignment: a face is in contact if its normal is aligned with the contact normal (0.5°). + // Otherwise the contact is an edge or a vertex + minFaceAlignment = 0.99996 - // epsilonDistance is the distance tolerance for Sutherland-Hodgman clipping. - // Points at distance >= -epsilonDistance from the plane are considered "inside". - epsilonDistance = 1e-6 + // edgeTolerance: 2 points at the same height along the normal (relative to the size of the feature) form an edge + edgeTolerance = 1e-4 - // epsilonParallel is the tolerance for detecting a line parallel to a plane. - // If |direction.Dot(planeNormal)| < epsilonParallel, the line is parallel. - epsilonParallel = 1e-10 + // parallelSin: 2 edges closer to parallel than ~1.1° touch along a line + parallelSin = 0.02 - // tangentBasisThreshold determines which axis to use for building the tangent basis. - // If |normal.X()| > tangentBasisThreshold, use Y instead of X as the first tangent. - tangentBasisThreshold = 0.9 -) + // faceTieTolerance: if both faces are aligned, the face of A is the reference (the choice must not change between 2 steps) + faceTieTolerance = 1e-3 -// ManifoldBuilder contains all working buffers with fixed-size arrays to avoid allocations. -type ManifoldBuilder struct { - // Fixed-size arrays to avoid allocations - localFeatureA [maxBufferSize]mgl64.Vec3 - localFeatureB [maxBufferSize]mgl64.Vec3 - worldFeatureA [maxBufferSize]mgl64.Vec3 - worldFeatureB [maxBufferSize]mgl64.Vec3 - clipBuffer1 [maxBufferSize]mgl64.Vec3 - clipBuffer2 [maxBufferSize]mgl64.Vec3 - tempPoints [maxBufferSize]constraint.ContactPoint - - // Counters - localFeatureACount int - localFeatureBCount int - worldFeatureACount int - worldFeatureBCount int - clipBuffer1Count int - clipBuffer2Count int - clippedResultCount int - tempPointsCount int -} + // reduceDepthTolerance (m): the deepest point of a contact changes only for a point deeper by this. The points of a + // flat contact are as deep, to the rounding (as EPATieTolerance) + reduceDepthTolerance = EPATieTolerance -// Pool of builders for reuse -var manifoldBuilderPool = sync.Pool{ - New: func() interface{} { - return &ManifoldBuilder{} - }, -} + // reduceBias: a point replaces the best one only if its score is higher by 1/reduceBias (Box3D) + reduceBias = 0.95 -// Reset prepares the builder for a new use -func (b *ManifoldBuilder) Reset() { - b.localFeatureACount = 0 - b.localFeatureBCount = 0 - b.worldFeatureACount = 0 - b.worldFeatureBCount = 0 - b.clipBuffer1Count = 0 - b.clipBuffer2Count = 0 - b.clippedResultCount = 0 - b.tempPointsCount = 0 -} + // epsilonDistance of the Sutherland-Hodgman clipping + epsilonDistance = 1e-9 -// GenerateManifold is the main entry point -func GenerateManifold(bodyA, bodyB *actor.RigidBody, normal mgl64.Vec3, depth float64) []constraint.ContactPoint { - builder := manifoldBuilderPool.Get().(*ManifoldBuilder) - defer manifoldBuilderPool.Put(builder) - - builder.Reset() + epsilonLength = 1e-12 +) - return builder.Generate(bodyA, bodyB, normal, depth) +type polygon struct { + points [maxBufferSize]mgl64.Vec3 + count int } -// Generate generates the manifold using internal buffers -func (b *ManifoldBuilder) Generate(bodyA, bodyB *actor.RigidBody, normal mgl64.Vec3, depth float64) []constraint.ContactPoint { - // Convert normal to local space - localNormalA := bodyA.Transform.Rotation.Conjugate().Rotate(normal) - localNormalB := bodyB.Transform.Rotation.Conjugate().Rotate(normal.Mul(-1)) - - // Get features into buffers - bodyA.Shape.GetContactFeature(localNormalA, &b.localFeatureA, &b.localFeatureACount) - bodyB.Shape.GetContactFeature(localNormalB, &b.localFeatureB, &b.localFeatureBCount) - - // Transform into buffers - b.transformFeature(&b.localFeatureA, b.localFeatureACount, bodyA.Transform, bodyA.Shape, &b.worldFeatureA, &b.worldFeatureACount) - b.transformFeature(&b.localFeatureB, b.localFeatureBCount, bodyB.Transform, bodyB.Shape, &b.worldFeatureB, &b.worldFeatureBCount) - - // Determine incident and reference - var incident *[8]mgl64.Vec3 - var incidentCount int - var reference *[8]mgl64.Vec3 - var referenceCount int - - if b.worldFeatureBCount <= b.worldFeatureACount { - incident = &b.worldFeatureB - incidentCount = b.worldFeatureBCount - reference = &b.worldFeatureA - referenceCount = b.worldFeatureACount - } else { - incident = &b.worldFeatureA - incidentCount = b.worldFeatureACount - reference = &b.worldFeatureB - referenceCount = b.worldFeatureBCount +func (p *polygon) add(v mgl64.Vec3) { + if p.count < maxBufferSize { + p.points[p.count] = v + p.count++ } +} - // Trivial case: single incident point - if incidentCount == 1 { - b.tempPoints[0] = constraint.ContactPoint{ - Position: incident[0], - Penetration: depth, +// Manifold generates the contact points of A and B, from the result of EPA: +// - face contact (a face aligned with the normal): the other feature is clipped by the sides of this face (Sutherland-Hodgman) +// - parallel edges (a box on an edge, a capsule along an edge): one edge is clipped by the other +// - otherwise (crossing edges, vertex, sphere): the witness point of EPA +// +// The deepest point has the separation of EPA, the other points are higher along the normal. +// Points further than the margin are removed, and 4 points are kept at most +func Manifold(a, b *actor.RigidBody, result Result, margin float64, m *constraint.Manifold) { + m.Reset(a, b) + normal := result.Normal + m.Normal = normal + separation := margin - result.Depth + + buffers := featuresPool.Get().(*features) + defer featuresPool.Put(buffers) + featureA, featureB := &buffers.a, &buffers.b + feature(a, normal, featureA) + feature(b, normal.Mul(-1), featureB) + + if referenceIsA, ok := chooseReference(featureA, featureB, normal); ok { + reference, incident := featureA, featureB + direction := normal // from the reference body towards the incident one + if !referenceIsA { + reference, incident = featureB, featureA + direction = normal.Mul(-1) + } + clipFeatures(reference, incident, direction, separation, margin, m) + } else { + edgeA := deepest(featureA, normal) + edgeB := deepest(featureB, normal.Mul(-1)) + if edgeA.count == 2 && edgeB.count == 2 && parallel(&edgeA, &edgeB) { + clipped := edgeB + clipToSlab(&clipped, edgeA.points[0], edgeA.points[1]) + keepPoints(&clipped, normal, separation, margin, m) } - b.tempPointsCount = 1 - return b.buildResult() } - // Clip incident against reference - clippedCount := b.clipIncidentAgainstReference(incident, incidentCount, reference, referenceCount, normal) - - // Final clip against reference plane - if clippedCount > 0 && referenceCount > 0 { - b.clipAgainstReferencePlane(clippedCount, reference, referenceCount, normal, depth) + if m.Count == 0 { + // Witness point, halfway between A and B. WitnessA is on A + margin + onA := result.WitnessA.Sub(normal.Mul(margin)) + m.Add(onA.Add(result.WitnessB).Mul(0.5), separation) } +} - // Fallback - if b.tempPointsCount == 0 { - deepest := bodyB.SupportWorld(normal.Mul(-1)) - b.tempPoints[0] = constraint.ContactPoint{ - Position: deepest, - Penetration: depth, - } - b.tempPointsCount = 1 +// deepest keeps the points of the feature the furthest along the direction: the deepest edge or vertex of a face +func deepest(p *polygon, direction mgl64.Vec3) polygon { + var out polygon + if p.count == 0 { + return out } - - // Limit to maxContactPoints - if b.tempPointsCount > maxContactPoints { - b.reduceTo4Points(normal) + size := 0.0 + top := math.Inf(-1) + for i := 0; i < p.count; i++ { + top = math.Max(top, p.points[i].Dot(direction)) + size = math.Max(size, p.points[i].Sub(p.points[0]).Len()) } - - return b.buildResult() + for i := 0; i < p.count; i++ { + if top-p.points[i].Dot(direction) <= edgeTolerance*size { + out.add(p.points[i]) + } + } + return out } -// transformFeature transforms features to world space -func (b *ManifoldBuilder) transformFeature(input *[8]mgl64.Vec3, inputCount int, transform actor.Transform, shape actor.ShapeInterface, output *[8]mgl64.Vec3, outputCount *int) { - *outputCount = 0 - - // Transform points from local to world space - for i := 0; i < inputCount; i++ { - rotated := transform.Rotation.Rotate(input[i]) - output[i] = transform.Position.Add(rotated) - } - *outputCount = inputCount +func parallel(a, b *polygon) bool { + da := a.points[1].Sub(a.points[0]) + db := b.points[1].Sub(b.points[0]) + lengths := da.Len() * db.Len() + return lengths > epsilonLength && da.Cross(db).Len() <= parallelSin*lengths } -// clipIncidentAgainstReference clips the incident feature against the reference feature. -// Always returns the result in clipBuffer1 for consistent downstream consumption. -func (b *ManifoldBuilder) clipIncidentAgainstReference(incident *[8]mgl64.Vec3, incidentCount int, reference *[8]mgl64.Vec3, referenceCount int, normal mgl64.Vec3) int { - // Handle insufficient reference (need at least 2 points for edges) - if referenceCount < 2 { - for i := 0; i < incidentCount; i++ { - b.clipBuffer1[i] = incident[i] - } - b.clipBuffer1Count = incidentCount - return incidentCount +// clipToSlab keeps the part of the segment between the planes at both ends of [start, end] +func clipToSlab(segment *polygon, start, end mgl64.Vec3) { + axis := end.Sub(start) + length := axis.Len() + if length < epsilonLength { + return } + axis = axis.Mul(1 / length) + var scratch polygon + clipAgainstPlane(segment, start, axis, &scratch) + clipAgainstPlane(&scratch, end, axis.Mul(-1), segment) +} - // Copy incident to clipBuffer1 - for i := 0; i < incidentCount; i++ { - b.clipBuffer1[i] = incident[i] +// feature returns the feature of the body facing the direction, in world space +func feature(body *actor.RigidBody, direction mgl64.Vec3, out *polygon) { + body.Shape.GetContactFeature(body.Transform.Rotation.Conjugate().Rotate(direction), &out.points, &out.count) + for i := 0; i < out.count; i++ { + out.points[i] = body.Transform.ToWorld(out.points[i]) } - b.clipBuffer1Count = incidentCount - b.clipBuffer2Count = 0 - - useBuffer1 := true - - // Clip against each edge - for i := 0; i < referenceCount; i++ { - var inputBuffer *[8]mgl64.Vec3 - var inputCount int - var outputBuffer *[8]mgl64.Vec3 - var outputCount *int - - if useBuffer1 { - inputBuffer = &b.clipBuffer1 - inputCount = b.clipBuffer1Count - outputBuffer = &b.clipBuffer2 - outputCount = &b.clipBuffer2Count - } else { - inputBuffer = &b.clipBuffer2 - inputCount = b.clipBuffer2Count - outputBuffer = &b.clipBuffer1 - outputCount = &b.clipBuffer1Count - } - - *outputCount = 0 - - if inputCount == 0 { - break - } - - v1 := reference[i] - v2 := reference[(i+1)%referenceCount] +} - edge := v2.Sub(v1) - edgeCrossNormal := edge.Cross(normal) +// features are the buffers of Manifold: they escape to the heap through the interface of the shapes, +// so they are reused +type features struct { + a polygon + b polygon +} - // Skip if edge is colinear with normal (no lateral clipping needed) - edgeCrossLen := edgeCrossNormal.Len() - if edgeCrossLen < epsilonColinear { - continue - } +var featuresPool = sync.Pool{New: func() any { return &features{} }} - clipNormal := edgeCrossNormal.Mul(1.0 / edgeCrossLen) +// chooseReference returns the reference face: the face aligned with the normal (the face of A if both are) +func chooseReference(featureA, featureB *polygon, normal mgl64.Vec3) (bool, bool) { + alignA := -1.0 + if featureA.count >= 3 { + alignA = math.Abs(faceNormal(featureA).Dot(normal)) + } + alignB := -1.0 + if featureB.count >= 3 { + alignB = math.Abs(faceNormal(featureB).Dot(normal)) + } - // Verify direction - center := b.computeCenter(reference, referenceCount) - toCenter := center.Sub(v1) - if toCenter.Dot(clipNormal) < 0 { - clipNormal = clipNormal.Mul(-1) - } + switch { + case alignA >= minFaceAlignment && alignA >= alignB-faceTieTolerance: + return true, true + case alignB >= minFaceAlignment: + return false, true + } + return false, false +} - // Clip - b.clipPolygonAgainstPlane(inputBuffer, inputCount, v1, clipNormal, outputBuffer, outputCount) +// faceNormal returns the normal of the polygon, in any orientation +func faceNormal(p *polygon) mgl64.Vec3 { + n := p.points[1].Sub(p.points[0]).Cross(p.points[2].Sub(p.points[0])) + length := n.Len() + if length < epsilonLength { + return mgl64.Vec3{} + } + return n.Mul(1 / length) +} - useBuffer1 = !useBuffer1 +// clipFeatures clips the incident feature with the side planes of the reference face +func clipFeatures(reference, incident *polygon, direction mgl64.Vec3, separation, margin float64, m *constraint.Manifold) { + refNormal := faceNormal(reference) + if refNormal.Dot(direction) < 0 { + refNormal = refNormal.Mul(-1) } - // Always put the result in clipBuffer1 - var finalCount int - if useBuffer1 { - // Result already in clipBuffer1 - finalCount = b.clipBuffer1Count - } else { - // Result in clipBuffer2, copy to clipBuffer1 - finalCount = b.clipBuffer2Count - for i := 0; i < finalCount; i++ { - b.clipBuffer1[i] = b.clipBuffer2[i] + center := mgl64.Vec3{} + for i := 0; i < reference.count; i++ { + center = center.Add(reference.points[i]) + } + center = center.Mul(1 / float64(reference.count)) + + clipped := *incident + var scratch polygon + for i := 0; i < reference.count && clipped.count > 0; i++ { + v1 := reference.points[i] + v2 := reference.points[(i+1)%reference.count] + sideNormal := v2.Sub(v1).Cross(refNormal) + length := sideNormal.Len() + if length < epsilonLength { + continue } - b.clipBuffer1Count = finalCount + sideNormal = sideNormal.Mul(1 / length) + if sideNormal.Dot(center.Sub(v1)) < 0 { + sideNormal = sideNormal.Mul(-1) + } + clipAgainstPlane(&clipped, v1, sideNormal, &scratch) + clipped, scratch = scratch, clipped } - return finalCount + keepPoints(&clipped, direction, separation, margin, m) } -// clipPolygonAgainstPlane clips a polygon against a plane using the Sutherland-Hodgman algorithm -func (b *ManifoldBuilder) clipPolygonAgainstPlane(input *[8]mgl64.Vec3, inputCount int, planePoint, planeNormal mgl64.Vec3, output *[8]mgl64.Vec3, outputCount *int) { - if inputCount == 0 { - *outputCount = 0 +// keepPoints converts the clipped points into contact points. +// The deepest point has the separation of EPA, the others are higher along the direction (from the reference towards the incident body) +func keepPoints(clipped *polygon, direction mgl64.Vec3, separation, margin float64, m *constraint.Manifold) { + if clipped.count == 0 { return } + lowest := clipped.points[0].Dot(direction) + for i := 1; i < clipped.count; i++ { + lowest = math.Min(lowest, clipped.points[i].Dot(direction)) + } - *outputCount = 0 - - for i := 0; i < inputCount; i++ { - current := input[i] - next := input[(i+1)%inputCount] - - currentDist := current.Sub(planePoint).Dot(planeNormal) - nextDist := next.Sub(planePoint).Dot(planeNormal) - - if currentDist >= -epsilonDistance { - if *outputCount < maxBufferSize { - output[*outputCount] = current - *outputCount++ - } - - if nextDist < -epsilonDistance && *outputCount < maxBufferSize { - intersection := lineIntersectPlane(current, next, planePoint, planeNormal) - output[*outputCount] = intersection - *outputCount++ - } - } else { - if nextDist >= -epsilonDistance && *outputCount < maxBufferSize { - intersection := lineIntersectPlane(current, next, planePoint, planeNormal) - output[*outputCount] = intersection - *outputCount++ - } + var candidates [maxBufferSize]constraint.ContactPoint + count := 0 + for i := 0; i < clipped.count; i++ { + p := clipped.points[i] + pointSeparation := separation + p.Dot(direction) - lowest + if pointSeparation > margin { + continue + } + candidates[count] = constraint.ContactPoint{ + Position: p.Sub(direction.Mul(pointSeparation / 2)), + Separation: pointSeparation, } + count++ } -} -// clipAgainstReferencePlane performs final clipping against the reference plane. -// Reads from clipBuffer1 and writes results to tempPoints. -func (b *ManifoldBuilder) clipAgainstReferencePlane(clippedCount int, reference *[8]mgl64.Vec3, referenceCount int, normal mgl64.Vec3, depth float64) { - b.tempPointsCount = 0 - - // Compute reference normal - edge1 := reference[1].Sub(reference[0]) - edge2 := reference[2].Sub(reference[0]) - refNormal := edge1.Cross(edge2).Normalize() + Reduce(candidates[:count], direction, m) +} - if refNormal.Dot(normal) < 0 { - refNormal = refNormal.Mul(-1) +// clipAgainstPlane keeps the part of the polygon (or segment) in front of the plane +func clipAgainstPlane(in *polygon, point, normal mgl64.Vec3, out *polygon) { + out.count = 0 + if in.count == 1 { + if in.points[0].Sub(point).Dot(normal) >= -epsilonDistance { + out.add(in.points[0]) + } + return } - refPoint := reference[0] - offset := refPoint.Dot(refNormal) + edges := in.count + if in.count == 2 { + edges = 1 // an open segment, not a closed polygon + } - // Always read from clipBuffer1 - for i := 0; i < clippedCount && b.tempPointsCount < maxBufferSize; i++ { - point := b.clipBuffer1[i] - distance := point.Dot(refNormal) - offset + for i := 0; i < edges; i++ { + current := in.points[i] + next := in.points[(i+1)%in.count] + dc := current.Sub(point).Dot(normal) + dn := next.Sub(point).Dot(normal) - if distance <= 0.0 { - b.tempPoints[b.tempPointsCount] = constraint.ContactPoint{ - Position: point, - Penetration: depth, - } - b.tempPointsCount++ + if dc >= -epsilonDistance { + out.add(current) + } + if (dc >= -epsilonDistance) != (dn >= -epsilonDistance) { + t := dc / (dc - dn) + out.add(current.Add(next.Sub(current).Mul(t))) + } + if in.count == 2 && dn >= -epsilonDistance { + out.add(next) } } } -// reduceTo4Points reduces contact points to maxContactPoints, using Farthest Point Sampling (FPS). -// -// Valid manifold sizes before reduction: 1, 2, 3, or 4 points -// - 1 point: Sphere-sphere, point-face contacts -// - 2 points: Edge-face contacts -// - 3 points: Triangular contact region (asymmetric clipping of quadrilateral) -// - 4 points: Face-face contacts (full quadrilateral overlap) -// -// This reduction only applies when tempPointsCount > 4, preserving 3-point manifolds. -func (b *ManifoldBuilder) reduceTo4Points(normal mgl64.Vec3) { - if b.tempPointsCount <= maxContactPoints { +// Reduce adds 4 points to m: the deepest, the furthest from it, then the points adding the most area to the contact polygon. +// A candidate replaces the best one only if it is clearly better: deeper by reduceDepthTolerance, or a score higher by +// 1/reduceBias (the pecking order of Box3D, b3ReduceManifoldPoints). Candidates as good keep their order: the choice +// doesn't flicker from a step to the next, nor depends on the rounding +func Reduce(points []constraint.ContactPoint, normal mgl64.Vec3, m *constraint.Manifold) { + if len(points) <= constraint.MaxContactPoints { + for _, p := range points { + m.Add(p.Position, p.Separation) + } return } - // 1. Initialization: choose the point farthest from the center of mass - center := mgl64.Vec3{0, 0, 0} - for i := 0; i < b.tempPointsCount; i++ { - center = center.Add(b.tempPoints[i].Position) - } - center = center.Mul(1.0 / float64(b.tempPointsCount)) - - // Find the point farthest from the center - maxDistSq := -1.0 - firstIdx := 0 - for i := 0; i < b.tempPointsCount; i++ { - diff := b.tempPoints[i].Position.Sub(center) - distSq := diff.Dot(diff) - if distSq > maxDistSq { - maxDistSq = distSq - firstIdx = i + chosen := [constraint.MaxContactPoints]int{} + deepest := 0 + for i, p := range points { + if p.Separation < points[deepest].Separation-reduceDepthTolerance { + deepest = i } } + chosen[0] = deepest - // 2. Farthest Point Sampling (FPS) - selectedIndices := [maxContactPoints]int{firstIdx} - selectedCount := 1 - - // Array to store minimum squared distances - var minDistSq [8]float64 - for i := 0; i < b.tempPointsCount; i++ { - diff := b.tempPoints[i].Position.Sub(b.tempPoints[firstIdx].Position) - minDistSq[i] = diff.Dot(diff) - } - - // Iterate until we have 4 points - for selectedCount < maxContactPoints { - // Find the point with the largest minimum distance - maxMinDistSq := -1.0 - nextIdx := -1 - for i := 0; i < b.tempPointsCount; i++ { - if minDistSq[i] > maxMinDistSq { - // Check if the point is not already selected - isSelected := false - for j := 0; j < selectedCount; j++ { - if selectedIndices[j] == i { - isSelected = true - break - } - } - if !isSelected { - maxMinDistSq = minDistSq[i] - nextIdx = i - } - } - } - - if nextIdx == -1 { - break // Safety case (should not happen) - } - - // Add the selected point - selectedIndices[selectedCount] = nextIdx - selectedCount++ - - // Update minimum distances - for i := 0; i < b.tempPointsCount; i++ { - diff := b.tempPoints[i].Position.Sub(b.tempPoints[nextIdx].Position) - distSq := diff.Dot(diff) - if distSq < minDistSq[i] { - minDistSq[i] = distSq - } + farthest, best := -1, -1.0 + for i, p := range points { + d := planar(p.Position.Sub(points[deepest].Position), normal).LenSqr() + if reduceBias*d > best { + farthest, best = i, d } } + chosen[1] = farthest - // 3. Copy the selected points to a temporary buffer - // Use a temporary buffer to avoid overwriting original data - var tempPoints [maxContactPoints]constraint.ContactPoint - for i := 0; i < maxContactPoints; i++ { - if i < selectedCount { - tempPoints[i] = b.tempPoints[selectedIndices[i]] - } else { - // Fallback (should not happen) - tempPoints[i] = b.tempPoints[0] + third, best := -1, -1.0 + for i, p := range points { + area := math.Abs(signedArea(points[deepest].Position, points[farthest].Position, p.Position, normal)) + if reduceBias*area > best { + third, best = i, area } } - - // 4. Copy the points from the temporary buffer to b.tempPoints - for i := 0; i < maxContactPoints; i++ { - b.tempPoints[i] = tempPoints[i] - } - - b.tempPointsCount = maxContactPoints -} - -// buildResult is the ONLY function that allocates (final copy) -func (b *ManifoldBuilder) buildResult() []constraint.ContactPoint { - result := make([]constraint.ContactPoint, b.tempPointsCount) - for i := 0; i < b.tempPointsCount; i++ { - result[i] = b.tempPoints[i] + chosen[2] = third + + orientation := math.Copysign(1, signedArea(points[deepest].Position, points[farthest].Position, points[third].Position, normal)) + fourth, best := -1, 0.0 + triangle := [3]int{deepest, farthest, third} + for i, p := range points { + for e := 0; e < 3; e++ { + // area added outside the edge e + added := -orientation * signedArea(points[triangle[e]].Position, points[triangle[(e+1)%3]].Position, p.Position, normal) + if reduceBias*added > best { + fourth, best = i, added + } + } } - return result -} -// computeCenter computes the centroid of a set of points -func (b *ManifoldBuilder) computeCenter(points *[8]mgl64.Vec3, count int) mgl64.Vec3 { - if count == 0 { - return mgl64.Vec3{0, 0, 0} + for k := 0; k < 3; k++ { + m.Add(points[chosen[k]].Position, points[chosen[k]].Separation) } - - sum := mgl64.Vec3{0, 0, 0} - for i := 0; i < count; i++ { - sum = sum.Add(points[i]) + if fourth >= 0 { + m.Add(points[fourth].Position, points[fourth].Separation) } - return sum.Mul(1.0 / float64(count)) } -// lineIntersectPlane computes the intersection point between a line segment and a plane. -// Returns p1 if the line is parallel to the plane. Clamps t to [0,1]. -func lineIntersectPlane(p1, p2, planePoint, planeNormal mgl64.Vec3) mgl64.Vec3 { - dir := p2.Sub(p1) - dist := p1.Sub(planePoint).Dot(planeNormal) - denom := dir.Dot(planeNormal) - - if math.Abs(denom) < epsilonParallel { - return p1 - } - - t := -dist / denom - t = math.Max(0, math.Min(1, t)) - - return p1.Add(dir.Mul(t)) +func planar(v, normal mgl64.Vec3) mgl64.Vec3 { + return v.Sub(normal.Mul(v.Dot(normal))) } -// getTangentBasis constructs an orthonormal tangent basis from a normal vector. -// Returns two tangent vectors perpendicular to the normal and to each other. -func getTangentBasis(normal mgl64.Vec3) (mgl64.Vec3, mgl64.Vec3) { - tangent1 := mgl64.Vec3{1, 0, 0} - if math.Abs(normal.X()) > tangentBasisThreshold { - tangent1 = mgl64.Vec3{0, 1, 0} - } - - tangent1 = tangent1.Sub(normal.Mul(tangent1.Dot(normal))).Normalize() - tangent2 := normal.Cross(tangent1).Normalize() - - return tangent1, tangent2 +// signedArea returns 2x the signed area of the triangle, seen along the normal +func signedArea(a, b, c, normal mgl64.Vec3) float64 { + return b.Sub(a).Cross(c.Sub(a)).Dot(normal) } diff --git a/epa/manifold_test.go b/epa/manifold_test.go index a177bd8..a95176b 100644 --- a/epa/manifold_test.go +++ b/epa/manifold_test.go @@ -2,1271 +2,294 @@ package epa import ( "math" + "math/rand" + "sort" "testing" "github.com/akmonengine/feather/actor" "github.com/akmonengine/feather/constraint" + "github.com/akmonengine/feather/gjk" "github.com/go-gl/mathgl/mgl64" ) -// TestGetTangentBasis tests the creation of orthogonal tangent basis -func TestGetTangentBasis(t *testing.T) { - tests := []struct { - name string - normal mgl64.Vec3 - }{ - { - name: "normal_aligned_with_x", - normal: mgl64.Vec3{1, 0, 0}, - }, - { - name: "normal_aligned_with_neg_x", - normal: mgl64.Vec3{-1, 0, 0}, - }, - { - name: "normal_aligned_with_y", - normal: mgl64.Vec3{0, 1, 0}, - }, - { - name: "normal_aligned_with_z", - normal: mgl64.Vec3{0, 0, 1}, - }, - { - name: "normal_at_threshold", - normal: mgl64.Vec3{0.9, 0.436, 0}.Normalize(), - }, - { - name: "normal_diagonal", - normal: mgl64.Vec3{1, 1, 1}.Normalize(), - }, - { - name: "normal_negative_diagonal", - normal: mgl64.Vec3{-1, -1, -1}.Normalize(), - }, - } - - for _, tt := range tests { - t.Run(tt.name, func(t *testing.T) { - tangent1, tangent2 := getTangentBasis(tt.normal) - - // Both tangents should be normalized - if !isNormalized(tangent1, 1e-6) { - t.Errorf("tangent1 is not normalized: length = %v", tangent1.Len()) - } - if !isNormalized(tangent2, 1e-6) { - t.Errorf("tangent2 is not normalized: length = %v", tangent2.Len()) - } - - // tangent1 perpendicular to normal - dot1 := tangent1.Dot(tt.normal) - if math.Abs(dot1) > 1e-6 { - t.Errorf("tangent1 not perpendicular to normal: dot = %v", dot1) - } - - // tangent2 perpendicular to normal - dot2 := tangent2.Dot(tt.normal) - if math.Abs(dot2) > 1e-6 { - t.Errorf("tangent2 not perpendicular to normal: dot = %v", dot2) - } - - // tangent1 perpendicular to tangent2 - dot12 := tangent1.Dot(tangent2) - if math.Abs(dot12) > 1e-6 { - t.Errorf("tangent1 not perpendicular to tangent2: dot = %v", dot12) - } - - // Verify cross product: normal x tangent1 ≈ tangent2 - cross := tt.normal.Cross(tangent1) - if !vec3ApproxEqual(cross, tangent2, 1e-6) { - t.Errorf("normal.Cross(tangent1) = %v, want %v", cross, tangent2) - } - }) - } +func manifold(t *testing.T, a, b *actor.RigidBody, margin float64) constraint.Manifold { + t.Helper() + result := runEPA(t, a, b, margin) + var m constraint.Manifold + Manifold(a, b, result, margin, &m) + return m } -// TestLineIntersectPlane tests line-plane intersection with clamping -func TestLineIntersectPlane(t *testing.T) { - tests := []struct { - name string - p1 mgl64.Vec3 - p2 mgl64.Vec3 - planePoint mgl64.Vec3 - planeNormal mgl64.Vec3 - expected mgl64.Vec3 - }{ - { - name: "perpendicular_intersection", - p1: mgl64.Vec3{0, -1, 0}, - p2: mgl64.Vec3{0, 1, 0}, - planePoint: mgl64.Vec3{0, 0, 0}, - planeNormal: mgl64.Vec3{0, 1, 0}, - expected: mgl64.Vec3{0, 0, 0}, - }, - { - name: "parallel_line", - p1: mgl64.Vec3{0, 1, 0}, - p2: mgl64.Vec3{1, 1, 0}, - planePoint: mgl64.Vec3{0, 0, 0}, - planeNormal: mgl64.Vec3{0, 1, 0}, - expected: mgl64.Vec3{0, 1, 0}, // Should return p1 - }, - { - name: "intersection_at_p1", - p1: mgl64.Vec3{0, 0, 0}, - p2: mgl64.Vec3{0, 2, 0}, - planePoint: mgl64.Vec3{0, 0, 0}, - planeNormal: mgl64.Vec3{0, 1, 0}, - expected: mgl64.Vec3{0, 0, 0}, - }, - { - name: "intersection_at_p2", - p1: mgl64.Vec3{0, -1, 0}, - p2: mgl64.Vec3{0, 0, 0}, - planePoint: mgl64.Vec3{0, 0, 0}, - planeNormal: mgl64.Vec3{0, 1, 0}, - expected: mgl64.Vec3{0, 0, 0}, - }, - { - name: "clamping_below_zero", - p1: mgl64.Vec3{0, 1, 0}, - p2: mgl64.Vec3{0, 2, 0}, - planePoint: mgl64.Vec3{0, 0, 0}, - planeNormal: mgl64.Vec3{0, 1, 0}, - expected: mgl64.Vec3{0, 1, 0}, // t clamped to 0 - }, - { - name: "clamping_above_one", - p1: mgl64.Vec3{0, -2, 0}, - p2: mgl64.Vec3{0, -1, 0}, - planePoint: mgl64.Vec3{0, 0, 0}, - planeNormal: mgl64.Vec3{0, 1, 0}, - expected: mgl64.Vec3{0, -1, 0}, // t clamped to 1 - }, - { - name: "diagonal_intersection", - p1: mgl64.Vec3{-1, -1, -1}, - p2: mgl64.Vec3{1, 1, 1}, - planePoint: mgl64.Vec3{0, 0, 0}, - planeNormal: mgl64.Vec3{0, 1, 0}, - expected: mgl64.Vec3{0, 0, 0}, - }, +func sortedPositions(m constraint.Manifold) []mgl64.Vec3 { + points := make([]mgl64.Vec3, m.Count) + for i := 0; i < m.Count; i++ { + points[i] = m.Points[i].Position } - - for _, tt := range tests { - t.Run(tt.name, func(t *testing.T) { - result := lineIntersectPlane(tt.p1, tt.p2, tt.planePoint, tt.planeNormal) - if !vec3ApproxEqual(result, tt.expected, 1e-6) { - t.Errorf("lineIntersectPlane() = %v, want %v", result, tt.expected) - } - }) - } -} - -// TestComputeCenter tests center computation -func TestComputeCenter(t *testing.T) { - builder := &ManifoldBuilder{} - - tests := []struct { - name string - points [8]mgl64.Vec3 - count int - expected mgl64.Vec3 - }{ - { - name: "zero_count", - points: [8]mgl64.Vec3{}, - count: 0, - expected: mgl64.Vec3{0, 0, 0}, - }, - { - name: "single_point", - points: [8]mgl64.Vec3{{5, 10, 15}}, - count: 1, - expected: mgl64.Vec3{5, 10, 15}, - }, - { - name: "two_points", - points: [8]mgl64.Vec3{{0, 0, 0}, {4, 0, 0}}, - count: 2, - expected: mgl64.Vec3{2, 0, 0}, - }, - { - name: "four_points_square", - points: [8]mgl64.Vec3{{0, 0, 0}, {4, 0, 0}, {0, 4, 0}, {4, 4, 0}}, - count: 4, - expected: mgl64.Vec3{2, 2, 0}, - }, - { - name: "four_points_3d", - points: [8]mgl64.Vec3{{0, 0, 0}, {2, 0, 0}, {0, 2, 0}, {0, 0, 2}}, - count: 4, - expected: mgl64.Vec3{0.5, 0.5, 0.5}, - }, - } - - for _, tt := range tests { - t.Run(tt.name, func(t *testing.T) { - result := builder.computeCenter(&tt.points, tt.count) - if !vec3ApproxEqual(result, tt.expected, 1e-6) { - t.Errorf("computeCenter() = %v, want %v", result, tt.expected) - } - }) - } -} - -// TestTransformFeatureNormalShapes tests transformation for Box and Sphere -func TestTransformFeatureNormalShapes(t *testing.T) { - builder := &ManifoldBuilder{} - - t.Run("box_transformation", func(t *testing.T) { - // Create a box with 4 points on top face (local space) - input := [8]mgl64.Vec3{ - {-1, 1, -1}, - {1, 1, -1}, - {1, 1, 1}, - {-1, 1, 1}, - } - inputCount := 4 - - // Transform: rotation 45° around Y, translation {5, 10, 15} - angleY := math.Pi / 4 // 45 degrees - transform := actor.Transform{ - Position: mgl64.Vec3{5, 10, 15}, - Rotation: mgl64.QuatRotate(angleY, mgl64.Vec3{0, 1, 0}), - } - - box := &actor.Box{HalfExtents: mgl64.Vec3{1, 1, 1}} - - var output [8]mgl64.Vec3 - var outputCount int - - builder.transformFeature(&input, inputCount, transform, box, &output, &outputCount) - - // Should have 4 output points - if outputCount != 4 { - t.Errorf("outputCount = %d, want 4", outputCount) - } - - // Each point should be rotated and translated - for i := 0; i < outputCount; i++ { - rotated := transform.Rotation.Rotate(input[i]) - expected := transform.Position.Add(rotated) - if !vec3ApproxEqual(output[i], expected, 1e-6) { - t.Errorf("output[%d] = %v, want %v", i, output[i], expected) - } + sort.Slice(points, func(i, j int) bool { + if points[i].X() != points[j].X() { + return points[i].X() < points[j].X() } + return points[i].Z() < points[j].Z() }) - - t.Run("sphere_transformation", func(t *testing.T) { - // Sphere has 1 support point - input := [8]mgl64.Vec3{{0, 1, 0}} - inputCount := 1 - - transform := actor.Transform{ - Position: mgl64.Vec3{-3, 2, 7}, - Rotation: mgl64.QuatIdent(), - } - - sphere := &actor.Sphere{Radius: 1.0} - - var output [8]mgl64.Vec3 - var outputCount int - - builder.transformFeature(&input, inputCount, transform, sphere, &output, &outputCount) - - // Should have 1 output point - if outputCount != 1 { - t.Errorf("outputCount = %d, want 1", outputCount) - } - - // Point should be translated - expected := mgl64.Vec3{-3, 3, 7} - if !vec3ApproxEqual(output[0], expected, 1e-6) { - t.Errorf("output[0] = %v, want %v", output[0], expected) - } - }) -} - -// TestClipPolygonAgainstPlane tests Sutherland-Hodgman single plane clipping -func TestClipPolygonAgainstPlane(t *testing.T) { - builder := &ManifoldBuilder{} - - tests := []struct { - name string - input []mgl64.Vec3 - planePoint mgl64.Vec3 - planeNormal mgl64.Vec3 - expectedCount int - checkPoints bool - expectedOut []mgl64.Vec3 - }{ - { - name: "empty_input", - input: []mgl64.Vec3{}, - planePoint: mgl64.Vec3{0, 0, 0}, - planeNormal: mgl64.Vec3{0, 1, 0}, - expectedCount: 0, - }, - { - name: "all_inside", - input: []mgl64.Vec3{ - {-1, 1, -1}, - {1, 1, -1}, - {1, 1, 1}, - {-1, 1, 1}, - }, - planePoint: mgl64.Vec3{0, 0, 0}, - planeNormal: mgl64.Vec3{0, 1, 0}, - expectedCount: 4, - }, - { - name: "all_outside", - input: []mgl64.Vec3{ - {-1, -2, -1}, - {1, -2, -1}, - {1, -2, 1}, - {-1, -2, 1}, - }, - planePoint: mgl64.Vec3{0, 0, 0}, - planeNormal: mgl64.Vec3{0, 1, 0}, - expectedCount: 0, - }, - { - name: "partial_clip", - input: []mgl64.Vec3{ - {-1, 0, 1}, // inside - {1, 0, 1}, // inside - {1, 0, -1}, // outside - {-1, 0, -1}, // outside - }, - planePoint: mgl64.Vec3{0, 0, 0}, - planeNormal: mgl64.Vec3{0, 0, 1}, - expectedCount: 4, // 2 original + 2 intersections - }, - { - name: "boundary_tolerance", - input: []mgl64.Vec3{ - {0, 0, -1e-6}, // Exactly at tolerance, should be included - }, - planePoint: mgl64.Vec3{0, 0, 0}, - planeNormal: mgl64.Vec3{0, 0, 1}, - expectedCount: 1, - }, - } - - for _, tt := range tests { - t.Run(tt.name, func(t *testing.T) { - // Copy input to buffer - var inputBuf [8]mgl64.Vec3 - copy(inputBuf[:], tt.input) - - var outputBuf [8]mgl64.Vec3 - var outputCount int - - builder.clipPolygonAgainstPlane(&inputBuf, len(tt.input), tt.planePoint, tt.planeNormal, &outputBuf, &outputCount) - - if outputCount != tt.expectedCount { - t.Errorf("outputCount = %d, want %d", outputCount, tt.expectedCount) - } - - if tt.checkPoints && len(tt.expectedOut) > 0 { - for i := 0; i < outputCount; i++ { - if !vec3ApproxEqual(outputBuf[i], tt.expectedOut[i], 1e-6) { - t.Errorf("output[%d] = %v, want %v", i, outputBuf[i], tt.expectedOut[i]) - } - } - } - }) - } + return points } -// TestClipIncidentAgainstReference tests multi-edge Sutherland-Hodgman with buffer ping-pong -func TestClipIncidentAgainstReference(t *testing.T) { - builder := &ManifoldBuilder{} - - t.Run("insufficient_reference", func(t *testing.T) { - builder.Reset() - - var reference [8]mgl64.Vec3 - reference[0] = mgl64.Vec3{0, 0, 0} - referenceCount := 1 // < 2 - - var incident [8]mgl64.Vec3 - incident[0] = mgl64.Vec3{1, 0, 0} - incidentCount := 1 - - normal := mgl64.Vec3{0, 0, 1} - - count := builder.clipIncidentAgainstReference(&incident, incidentCount, &reference, referenceCount, normal) - - // Should copy incident directly - if count != incidentCount { - t.Errorf("count = %d, want %d", count, incidentCount) - } - }) - - t.Run("colinear_edge_skip", func(t *testing.T) { - builder.Reset() - - // Create reference with one edge parallel to normal - var reference [8]mgl64.Vec3 - normal := mgl64.Vec3{0, 0, 1} - - // Edge from (0,0,0) to (0,0,1) is parallel to normal - reference[0] = mgl64.Vec3{0, 0, 0} - reference[1] = mgl64.Vec3{0, 0, 1} // Colinear edge - reference[2] = mgl64.Vec3{1, 0, 1} - reference[3] = mgl64.Vec3{1, 0, 0} - referenceCount := 4 - - var incident [8]mgl64.Vec3 - incident[0] = mgl64.Vec3{0.5, 0, 0.5} - incidentCount := 1 - - count := builder.clipIncidentAgainstReference(&incident, incidentCount, &reference, referenceCount, normal) - - // Should still return some result (colinear edge skipped) - if count == 0 { - t.Error("count = 0, colinear edge should be skipped but not fail") - } - }) - - t.Run("normal_clipping_square", func(t *testing.T) { - builder.Reset() - - // Create a square reference face - var reference [8]mgl64.Vec3 - reference[0] = mgl64.Vec3{-1, 0, -1} - reference[1] = mgl64.Vec3{1, 0, -1} - reference[2] = mgl64.Vec3{1, 0, 1} - reference[3] = mgl64.Vec3{-1, 0, 1} - referenceCount := 4 - - // Incident polygon (slightly overlapping) - var incident [8]mgl64.Vec3 - incident[0] = mgl64.Vec3{-0.5, 0, -0.5} - incident[1] = mgl64.Vec3{0.5, 0, -0.5} - incident[2] = mgl64.Vec3{0.5, 0, 0.5} - incident[3] = mgl64.Vec3{-0.5, 0, 0.5} - incidentCount := 4 - - normal := mgl64.Vec3{0, 1, 0} - - count := builder.clipIncidentAgainstReference(&incident, incidentCount, &reference, referenceCount, normal) - - // Should clip successfully (exact count depends on geometry) - if count == 0 { - t.Error("count = 0, expected some points after clipping") - } - - // Result should be in clipBuffer1 (even number of edges = 4) - if builder.clipBuffer1Count == 0 { - t.Error("clipBuffer1Count = 0, expected result in clipBuffer1") - } - }) - - t.Run("clip_normal_inversion", func(t *testing.T) { - builder.Reset() - - // Create reference where center is on the opposite side - var reference [8]mgl64.Vec3 - reference[0] = mgl64.Vec3{10, 0, 10} - reference[1] = mgl64.Vec3{11, 0, 10} - reference[2] = mgl64.Vec3{11, 0, 11} - reference[3] = mgl64.Vec3{10, 0, 11} - referenceCount := 4 - - // Incident at origin - var incident [8]mgl64.Vec3 - incident[0] = mgl64.Vec3{0, 0, 0} - incident[1] = mgl64.Vec3{1, 0, 0} - incident[2] = mgl64.Vec3{1, 0, 1} - incident[3] = mgl64.Vec3{0, 0, 1} - incidentCount := 4 - - normal := mgl64.Vec3{0, 1, 0} - - count := builder.clipIncidentAgainstReference(&incident, incidentCount, &reference, referenceCount, normal) - - // Should handle clip normal inversion - if count < 0 { - t.Errorf("count = %d, should be >= 0", count) - } - }) - - t.Run("odd_number_of_edges", func(t *testing.T) { - builder.Reset() - - // Create a triangular reference (3 edges) - var reference [8]mgl64.Vec3 - reference[0] = mgl64.Vec3{-1, 0, -1} - reference[1] = mgl64.Vec3{1, 0, -1} - reference[2] = mgl64.Vec3{0, 0, 1} - referenceCount := 3 - - // Incident polygon - var incident [8]mgl64.Vec3 - incident[0] = mgl64.Vec3{-0.5, 0, -0.5} - incident[1] = mgl64.Vec3{0.5, 0, -0.5} - incident[2] = mgl64.Vec3{0.5, 0, 0.5} - incident[3] = mgl64.Vec3{-0.5, 0, 0.5} - incidentCount := 4 - - normal := mgl64.Vec3{0, 1, 0} - - count := builder.clipIncidentAgainstReference(&incident, incidentCount, &reference, referenceCount, normal) - - // With odd number of edges, result should be copied to clipBuffer1 - if count == 0 { - t.Error("count = 0, expected some points after clipping") - } - - // Verify result is in clipBuffer1 - if builder.clipBuffer1Count == 0 { - t.Error("clipBuffer1Count = 0, expected result in clipBuffer1 after odd edges") - } - }) +func unitBox(position mgl64.Vec3, rotation mgl64.Quat) *actor.RigidBody { + return body(position, rotation, &actor.Box{HalfExtents: mgl64.Vec3{0.5, 0.5, 0.5}}) } -// TestClipAgainstReferencePlane tests final clipping against reference plane -func TestClipAgainstReferencePlane(t *testing.T) { - builder := &ManifoldBuilder{} - - t.Run("points_behind_plane", func(t *testing.T) { - builder.Reset() - - // Set up clipBuffer1 with 4 points - builder.clipBuffer1[0] = mgl64.Vec3{0, 1, 0} // Above plane (behind) - builder.clipBuffer1[1] = mgl64.Vec3{0, -1, 0} // Below plane (in front) - builder.clipBuffer1[2] = mgl64.Vec3{1, -1, 0} // Below plane (in front) - builder.clipBuffer1[3] = mgl64.Vec3{1, 1, 0} // Above plane (behind) - clippedCount := 4 - - // Reference triangle defining plane at Y=0 - var reference [8]mgl64.Vec3 - reference[0] = mgl64.Vec3{0, 0, 0} - reference[1] = mgl64.Vec3{1, 0, 0} - reference[2] = mgl64.Vec3{0, 0, 1} - referenceCount := 3 - - normal := mgl64.Vec3{0, 1, 0} - depth := 0.1 - - builder.clipAgainstReferencePlane(clippedCount, &reference, referenceCount, normal, depth) - - // Should keep only points with distance <= 0 (below plane) - if builder.tempPointsCount != 2 { - t.Errorf("tempPointsCount = %d, want 2", builder.tempPointsCount) - } - - // Verify the kept points are the ones below the plane - for i := 0; i < builder.tempPointsCount; i++ { - if builder.tempPoints[i].Position.Y() > 0 { - t.Errorf("tempPoints[%d].Y = %v, should be <= 0", i, builder.tempPoints[i].Position.Y()) - } - } - }) - - t.Run("all_points_pass", func(t *testing.T) { - builder.Reset() - - // All points below plane - builder.clipBuffer1[0] = mgl64.Vec3{0, -1, 0} - builder.clipBuffer1[1] = mgl64.Vec3{1, -1, 0} - builder.clipBuffer1[2] = mgl64.Vec3{1, -1, 1} - builder.clipBuffer1[3] = mgl64.Vec3{0, -1, 1} - clippedCount := 4 - - var reference [8]mgl64.Vec3 - reference[0] = mgl64.Vec3{0, 0, 0} - reference[1] = mgl64.Vec3{1, 0, 0} - reference[2] = mgl64.Vec3{0, 0, 1} - referenceCount := 3 - - normal := mgl64.Vec3{0, 1, 0} - depth := 0.1 - - builder.clipAgainstReferencePlane(clippedCount, &reference, referenceCount, normal, depth) - - // All points should pass - if builder.tempPointsCount != 4 { - t.Errorf("tempPointsCount = %d, want 4", builder.tempPointsCount) - } - }) - - t.Run("buffer_limit", func(t *testing.T) { - builder.Reset() - - // Fill clipBuffer1 with 8 points all below plane - for i := 0; i < 8; i++ { - builder.clipBuffer1[i] = mgl64.Vec3{float64(i), -1, 0} - } - clippedCount := 8 - - // Add 4 more to tempPoints first (to test limit) - for i := 0; i < 4; i++ { - builder.tempPoints[i] = constraint.ContactPoint{ - Position: mgl64.Vec3{float64(i), -1, 0}, - Penetration: 0.1, - } +// A box resting flat on a larger box: four points at its corners, halfway into the overlap. +func TestManifoldFaceOnFace(t *testing.T) { + ground := body(mgl64.Vec3{}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{2, 0.5, 2}}) + box := unitBox(mgl64.Vec3{0.3, 0.99, -0.4}, mgl64.QuatIdent()) + m := manifold(t, ground, box, 0) + if m.Count != 4 || !near(m.Normal, mgl64.Vec3{0, 1, 0}, 1e-9) { + t.Fatalf("manifold %+v, want 4 points along +Y", m) + } + want := []mgl64.Vec3{{-0.2, 0.495, -0.9}, {-0.2, 0.495, 0.1}, {0.8, 0.495, -0.9}, {0.8, 0.495, 0.1}} + for i, p := range sortedPositions(m) { + if !near(p, want[i], 1e-9) { + t.Errorf("point %d = %v, want %v", i, p, want[i]) } - builder.tempPointsCount = 4 - - var reference [8]mgl64.Vec3 - reference[0] = mgl64.Vec3{0, 0, 0} - reference[1] = mgl64.Vec3{10, 0, 0} - reference[2] = mgl64.Vec3{0, 0, 10} - referenceCount := 3 - - normal := mgl64.Vec3{0, 1, 0} - depth := 0.1 - - builder.clipAgainstReferencePlane(clippedCount, &reference, referenceCount, normal, depth) - - // Should stop at 8 total (4 existing + 4 new) - if builder.tempPointsCount > 8 { - t.Errorf("tempPointsCount = %d, should not exceed 8", builder.tempPointsCount) + } + for i := 0; i < m.Count; i++ { + if math.Abs(m.Points[i].Separation+0.01) > 1e-9 { + t.Errorf("separation %d = %.9f, want -0.01", i, m.Points[i].Separation) } - }) + } } -// TestReduceTo4Points tests point reduction algorithm -func TestReduceTo4Points(t *testing.T) { - builder := &ManifoldBuilder{} - - t.Run("already_4_or_fewer", func(t *testing.T) { - builder.Reset() - - // Create 4 points - for i := 0; i < 4; i++ { - builder.tempPoints[i] = constraint.ContactPoint{ - Position: mgl64.Vec3{float64(i), 0, 0}, - Penetration: 0.1, - } - } - builder.tempPointsCount = 4 - - normal := mgl64.Vec3{0, 1, 0} - builder.reduceTo4Points(normal) - - // Should not change - if builder.tempPointsCount != 4 { - t.Errorf("tempPointsCount = %d, want 4", builder.tempPointsCount) - } - }) - - t.Run("reduce_from_8", func(t *testing.T) { - builder.Reset() - - // Create 8 points forming an octagon in XY plane - for i := 0; i < 8; i++ { - angle := float64(i) * math.Pi / 4 - builder.tempPoints[i] = constraint.ContactPoint{ - Position: mgl64.Vec3{math.Cos(angle), math.Sin(angle), 0}, - Penetration: 0.1, - } - } - builder.tempPointsCount = 8 - - normal := mgl64.Vec3{0, 0, 1} - builder.reduceTo4Points(normal) - - // Should reduce to 4 extremes - if builder.tempPointsCount > 4 { - t.Errorf("tempPointsCount = %d, want <= 4", builder.tempPointsCount) - } - if builder.tempPointsCount < 1 { - t.Errorf("tempPointsCount = %d, want >= 1", builder.tempPointsCount) +// A box hanging over the edge of its support: the points are clipped to the support face. +func TestManifoldClippedToReference(t *testing.T) { + ground := body(mgl64.Vec3{}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{1, 0.5, 1}}) + box := unitBox(mgl64.Vec3{0.8, 0.99, 0}, mgl64.QuatIdent()) + m := manifold(t, ground, box, 0) + if m.Count != 4 { + t.Fatalf("got %d points, want 4", m.Count) + } + for i := 0; i < m.Count; i++ { + if x := m.Points[i].Position.X(); x < 0.3-1e-9 || x > 1+1e-9 { + t.Errorf("point %v outside the support face (x in [0.3, 1])", m.Points[i].Position) } - }) + } } -// TestManifoldGenerate tests the main Generate orchestrator -func TestManifoldGenerate(t *testing.T) { - t.Run("trivial_case_single_incident", func(t *testing.T) { - // Sphere vs Box: Sphere has 1 point, Box has 4 - bodyA := &actor.RigidBody{ - Shape: &actor.Sphere{Radius: 1.0}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 0, 0}, - Rotation: mgl64.QuatIdent(), - }, - } - - bodyB := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{1, 1, 1}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 1.9, 0}, - Rotation: mgl64.QuatIdent(), - }, - } - - normal := mgl64.Vec3{0, 1, 0} - depth := 0.1 - - points := GenerateManifold(bodyA, bodyB, normal, depth) - - // Should return 1 point (trivial case) - if len(points) != 1 { - t.Errorf("len(points) = %d, want 1", len(points)) - } - - if len(points) > 0 && points[0].Penetration != depth { - t.Errorf("points[0].Penetration = %v, want %v", points[0].Penetration, depth) - } - }) - - t.Run("fallback_case_empty_clipping", func(t *testing.T) { - // Create a scenario where all points get clipped away - // Box-Box with rotations that produce difficult clipping - bodyA := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{0.1, 0.1, 0.1}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 0, 0}, - Rotation: mgl64.QuatRotate(math.Pi/4, mgl64.Vec3{0, 1, 0}), - }, - } - - bodyB := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{0.1, 0.1, 0.1}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 0.15, 0}, - Rotation: mgl64.QuatRotate(math.Pi/3, mgl64.Vec3{0, 1, 0}), - }, - } - - normal := mgl64.Vec3{0, 1, 0} - depth := 0.05 - - points := GenerateManifold(bodyA, bodyB, normal, depth) - - // Should use fallback and still produce at least 1 point - if len(points) == 0 { - t.Error("len(points) = 0, fallback should produce at least 1 point") - } - }) - - t.Run("reduction_case_more_than_4", func(t *testing.T) { - // Box-Box aligned to produce maximum contact points (potentially 8) - bodyA := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{2, 0.5, 2}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 0, 0}, - Rotation: mgl64.QuatIdent(), - }, - } - - bodyB := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{2, 0.5, 2}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 0.99, 0}, - Rotation: mgl64.QuatIdent(), - }, - } - - normal := mgl64.Vec3{0, 1, 0} - depth := 0.01 - - points := GenerateManifold(bodyA, bodyB, normal, depth) - - // Should reduce to max 4 points - if len(points) > 4 { - t.Errorf("len(points) = %d, should reduce to max 4", len(points)) - } - - if len(points) == 0 { - t.Error("len(points) = 0, expected contact points") - } - }) - - t.Run("clippedCount_zero_skip_reference_plane", func(t *testing.T) { - // Create scenario where clipping produces 0 points - // This will skip clipAgainstReferencePlane (line 110 condition false) - bodyA := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{0.01, 0.01, 0.01}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 0, 0}, - Rotation: mgl64.QuatRotate(math.Pi/2, mgl64.Vec3{1, 0, 0}), - }, - } - - bodyB := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{0.01, 0.01, 0.01}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{10, 10, 10}, - Rotation: mgl64.QuatRotate(math.Pi/3, mgl64.Vec3{0, 0, 1}), - }, - } - - normal := mgl64.Vec3{0, 1, 0} - depth := 0.001 - - points := GenerateManifold(bodyA, bodyB, normal, depth) - - // With clippedCount == 0, should use fallback - if len(points) == 0 { - t.Error("len(points) = 0, fallback should produce at least 1 point") - } - }) - - t.Run("both_features_equal_count", func(t *testing.T) { - // Test exact equality case (worldFeatureBCount == worldFeatureACount) - // This ensures the <= branch (line 84) is properly tested - bodyA := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{1, 1, 1}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 0, 0}, - Rotation: mgl64.QuatIdent(), - }, - } - - bodyB := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{1, 1, 1}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 1.5, 0}, - Rotation: mgl64.QuatIdent(), - }, - } - - normal := mgl64.Vec3{0, 1, 0} - depth := 0.5 - - points := GenerateManifold(bodyA, bodyB, normal, depth) - - // Both boxes should have same feature count (4 points each) - // Should still produce valid manifold - if len(points) == 0 { - t.Error("len(points) = 0, expected contact points") - } - }) - - t.Run("skip_reduction_exactly_4", func(t *testing.T) { - // Test case where tempPointsCount == 4 exactly - // This ensures the > 4 check (line 125) is false - bodyA := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{0.5, 0.5, 0.5}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 0, 0}, - Rotation: mgl64.QuatIdent(), - }, - } - - bodyB := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{0.5, 0.5, 0.5}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 0.99, 0}, - Rotation: mgl64.QuatIdent(), - }, - } - - normal := mgl64.Vec3{0, 1, 0} - depth := 0.01 - - points := GenerateManifold(bodyA, bodyB, normal, depth) - - // Should produce points without needing reduction - if len(points) > 4 { - t.Errorf("len(points) = %d, should not exceed 4", len(points)) - } - }) - - t.Run("normal_clipping_path", func(t *testing.T) { - // Ensure normal path where clippedCount > 0 AND referenceCount > 0 - // This makes line 110 condition TRUE - bodyA := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{1, 1, 1}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 0, 0}, - Rotation: mgl64.QuatIdent(), - }, - } - - bodyB := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{1, 1, 1}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 1.5, 0}, - Rotation: mgl64.QuatIdent(), - }, - } - - normal := mgl64.Vec3{0, 1, 0} - depth := 0.5 - - points := GenerateManifold(bodyA, bodyB, normal, depth) +// A slightly tilted box: each corner gets its own separation (v0.2.0 gave every point the +// same depth, which put the wrong torque on the box). +func TestManifoldPerPointSeparation(t *testing.T) { + ground := body(mgl64.Vec3{}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{2, 0.5, 2}}) + tilt := mgl64.QuatRotate(0.02, mgl64.Vec3{0, 0, 1}) + box := unitBox(mgl64.Vec3{0, 1, 0}, tilt) + // Lowest corner of the box, to sink it by 5 mm. + lowest := math.Inf(1) + for _, c := range [4]mgl64.Vec3{{-0.5, -0.5, 0}, {0.5, -0.5, 0}} { + lowest = math.Min(lowest, box.Transform.ToWorld(c).Y()) + } + box.Transform.Position = box.Transform.Position.Add(mgl64.Vec3{0, 0.5 - lowest - 0.005, 0}) - // Normal clipping should work - if len(points) == 0 { - t.Error("len(points) = 0, expected contact points from normal clipping") + m := manifold(t, ground, box, 0.02) + if m.Count != 4 { + t.Fatalf("got %d points, want 4", m.Count) + } + for i := 0; i < m.Count; i++ { + p := m.Points[i] + // The corner lies at p + separation/2 above the ground face y=0.5. + corner := p.Position.Y() + p.Separation/2 + if math.Abs(corner-0.5-p.Separation) > 1e-6 { + t.Errorf("point %d: separation %.6f but corner %.6f above the face", i, p.Separation, corner-0.5) } - }) + } + if min := m.MinSeparation(); math.Abs(min+0.005) > 1e-6 { + t.Errorf("deepest separation %.6f, want -0.005", min) + } } -// TestGenerateManifold tests public API -func TestGenerateManifold(t *testing.T) { - t.Run("box_box_typical", func(t *testing.T) { - bodyA := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{1.5, 1.5, 1.5}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 0, 0}, - Rotation: mgl64.QuatIdent(), - }, - } - - bodyB := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{1.5, 1.5, 1.5}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 2.9, 0}, - Rotation: mgl64.QuatIdent(), - }, - } - - normal := mgl64.Vec3{0, 1, 0} - depth := 0.1 - - points := GenerateManifold(bodyA, bodyB, normal, depth) - - if len(points) == 0 { - t.Error("len(points) = 0, expected at least 1") - } - - if len(points) > 4 { - t.Errorf("len(points) = %d, should not exceed 4", len(points)) +// Two boxes a few millimetres apart, within the margin: speculative points with a positive +// separation. +func TestManifoldSpeculative(t *testing.T) { + ground := body(mgl64.Vec3{}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{2, 0.5, 2}}) + box := unitBox(mgl64.Vec3{0, 1.005, 0}, mgl64.QuatIdent()) + m := manifold(t, ground, box, 0.02) + if m.Count != 4 { + t.Fatalf("got %d points, want 4", m.Count) + } + for i := 0; i < m.Count; i++ { + if math.Abs(m.Points[i].Separation-0.005) > 1e-6 { + t.Errorf("separation %.6f, want +0.005", m.Points[i].Separation) } - }) + } } -// TestManifoldBuilderReset tests Reset method -func TestManifoldBuilderReset(t *testing.T) { - builder := &ManifoldBuilder{} - - // Set non-zero counts - builder.localFeatureACount = 5 - builder.localFeatureBCount = 3 - builder.worldFeatureACount = 4 - builder.worldFeatureBCount = 2 - builder.clipBuffer1Count = 1 - builder.clipBuffer2Count = 6 - builder.clippedResultCount = 7 - builder.tempPointsCount = 8 - - builder.Reset() - - // All should be zero - if builder.localFeatureACount != 0 { - t.Errorf("localFeatureACount = %d, want 0", builder.localFeatureACount) - } - if builder.localFeatureBCount != 0 { - t.Errorf("localFeatureBCount = %d, want 0", builder.localFeatureBCount) +// A box balanced on one of its edges: the contact is that edge, two points. +func TestManifoldEdgeOnFace(t *testing.T) { + ground := body(mgl64.Vec3{}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{2, 0.5, 2}}) + box := unitBox(mgl64.Vec3{0, 0.5 + math.Sqrt2/2 - 0.01, 0}, mgl64.QuatRotate(math.Pi/4, mgl64.Vec3{0, 0, 1})) + m := manifold(t, ground, box, 0) + if m.Count != 2 { + t.Fatalf("got %d points %+v, want the 2 ends of the edge", m.Count, m.Points[:m.Count]) } - if builder.worldFeatureACount != 0 { - t.Errorf("worldFeatureACount = %d, want 0", builder.worldFeatureACount) + zs := []float64{m.Points[0].Position.Z(), m.Points[1].Position.Z()} + sort.Float64s(zs) + if math.Abs(zs[0]+0.5) > 1e-6 || math.Abs(zs[1]-0.5) > 1e-6 || math.Abs(m.MinSeparation()+0.01) > 1e-6 { + t.Errorf("edge points z=%v separation %.6f, want z=±0.5 and -0.01", zs, m.MinSeparation()) } - if builder.worldFeatureBCount != 0 { - t.Errorf("worldFeatureBCount = %d, want 0", builder.worldFeatureBCount) +} + +// Two boxes crossing edge to edge: no face is involved, the contact is the single witness +// point on the edges. +func TestManifoldEdgeOnEdge(t *testing.T) { + a := unitBox(mgl64.Vec3{}, mgl64.QuatRotate(math.Pi/4, mgl64.Vec3{0, 0, 1})) + b := unitBox(mgl64.Vec3{0, math.Sqrt2 - 0.01, 0}, mgl64.QuatRotate(math.Pi/4, mgl64.Vec3{1, 0, 0})) + m := manifold(t, a, b, 0) + if m.Count != 1 { + t.Fatalf("got %d points, want 1", m.Count) } - if builder.clipBuffer1Count != 0 { - t.Errorf("clipBuffer1Count = %d, want 0", builder.clipBuffer1Count) + if !near(m.Points[0].Position, mgl64.Vec3{0, math.Sqrt2/2 - 0.005, 0}, 1e-5) || math.Abs(m.Points[0].Separation+0.01) > 1e-6 { + t.Errorf("point %+v, want (0, %.4f, 0) separation -0.01", m.Points[0], math.Sqrt2/2-0.005) } - if builder.clipBuffer2Count != 0 { - t.Errorf("clipBuffer2Count = %d, want 0", builder.clipBuffer2Count) +} + +// Two squares turned by 45°: their overlap is an octagon, reduced to four points that keep +// the deepest one and span most of the area. +func TestManifoldReducedToFour(t *testing.T) { + ground := body(mgl64.Vec3{}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{0.5, 0.5, 0.5}}) + box := body(mgl64.Vec3{0, 0.99, 0}, mgl64.QuatRotate(math.Pi/4, mgl64.Vec3{0, 1, 0}), &actor.Box{HalfExtents: mgl64.Vec3{0.5, 0.5, 0.5}}) + m := manifold(t, ground, box, 0) + if m.Count != 4 { + t.Fatalf("got %d points, want 4", m.Count) } - if builder.clippedResultCount != 0 { - t.Errorf("clippedResultCount = %d, want 0", builder.clippedResultCount) + // Area of the kept quadrilateral against the octagon's (2(√2-1)... = 0.828 for unit squares). + pts := sortedByAngle(m) + area := 0.0 + for i := range pts { + j := (i + 1) % len(pts) + area += pts[i].X()*pts[j].Z() - pts[j].X()*pts[i].Z() } - if builder.tempPointsCount != 0 { - t.Errorf("tempPointsCount = %d, want 0", builder.tempPointsCount) + area = math.Abs(area) / 2 + octagon := 2 * (math.Sqrt2 - 1) + if area < 0.6*octagon { + t.Errorf("kept area %.3f of the %.3f octagon", area, octagon) } } -// TestBuildResult tests final result building -func TestBuildResult(t *testing.T) { - builder := &ManifoldBuilder{} - - t.Run("zero_points", func(t *testing.T) { - builder.Reset() - result := builder.buildResult() - - if len(result) != 0 { - t.Errorf("len(result) = %d, want 0", len(result)) - } - }) - - t.Run("four_points", func(t *testing.T) { - builder.Reset() - - for i := 0; i < 4; i++ { - builder.tempPoints[i] = constraint.ContactPoint{ - Position: mgl64.Vec3{float64(i), 0, 0}, - Penetration: 0.1, - } - } - builder.tempPointsCount = 4 - - result := builder.buildResult() - - if len(result) != 4 { - t.Errorf("len(result) = %d, want 4", len(result)) - } - - // Verify values copied correctly - for i := 0; i < 4; i++ { - if !vec3ApproxEqual(result[i].Position, mgl64.Vec3{float64(i), 0, 0}, 1e-6) { - t.Errorf("result[%d].Position = %v, want %v", i, result[i].Position, mgl64.Vec3{float64(i), 0, 0}) - } +// Random points reduced to 4 at most (3 if the others are inside their triangle): the deepest is always kept +func TestReduceKeepsDeepest(t *testing.T) { + r := rand.New(rand.NewSource(1)) + normal := mgl64.Vec3{0, 1, 0} + for i := 0; i < 500; i++ { + points := make([]constraint.ContactPoint, 5+r.Intn(12)) + deepest := math.Inf(1) + for k := range points { + points[k] = constraint.ContactPoint{Position: mgl64.Vec3{r.Float64() - 0.5, 0, r.Float64() - 0.5}, Separation: r.Float64()*0.1 - 0.05} + deepest = math.Min(deepest, points[k].Separation) + } + var m constraint.Manifold + Reduce(points, normal, &m) + if m.Count < 3 || m.MinSeparation() != deepest { + t.Fatalf("%d points kept, the deepest at %.4f, want %.4f", m.Count, m.MinSeparation(), deepest) } - }) + } } -// TestManifoldShapeCombinations tests different shape pairs -func TestManifoldShapeCombinations(t *testing.T) { - t.Run("box_sphere", func(t *testing.T) { - bodyA := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{1, 1, 1}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 0, 0}, - Rotation: mgl64.QuatIdent(), - }, - } - - bodyB := &actor.RigidBody{ - Shape: &actor.Sphere{Radius: 0.5}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 1.4, 0}, - Rotation: mgl64.QuatIdent(), - }, - } - - normal := mgl64.Vec3{0, 1, 0} - depth := 0.1 - - points := GenerateManifold(bodyA, bodyB, normal, depth) - - if len(points) == 0 { - t.Error("len(points) = 0, expected at least 1") - } - }) - - t.Run("sphere_sphere", func(t *testing.T) { - bodyA := &actor.RigidBody{ - Shape: &actor.Sphere{Radius: 1.0}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 0, 0}, - Rotation: mgl64.QuatIdent(), - }, - } - - bodyB := &actor.RigidBody{ - Shape: &actor.Sphere{Radius: 1.0}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 1.9, 0}, - Rotation: mgl64.QuatIdent(), - }, - } - - normal := mgl64.Vec3{0, 1, 0} - depth := 0.1 - - points := GenerateManifold(bodyA, bodyB, normal, depth) - - // Should return 1 point (both spheres have 1 point) - if len(points) != 1 { - t.Errorf("len(points) = %d, want 1", len(points)) - } - }) - - t.Run("box_plane", func(t *testing.T) { - bodyA := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{1, 1, 1}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 0.9, 0}, - Rotation: mgl64.QuatIdent(), - }, - } - - bodyB := &actor.RigidBody{ - Shape: &actor.Plane{ - Normal: mgl64.Vec3{0, 1, 0}, - Distance: 0, - }, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 0, 0}, - Rotation: mgl64.QuatIdent(), - }, - } - - normal := mgl64.Vec3{0, 1, 0} - depth := 0.1 - - points := GenerateManifold(bodyA, bodyB, normal, depth) - - // Plane generates 4 large corners, box has 4 points - // Should produce contact points - if len(points) == 0 { - t.Error("len(points) = 0, expected contact points for box-plane") - } - }) - - t.Run("sphere_plane", func(t *testing.T) { - bodyA := &actor.RigidBody{ - Shape: &actor.Sphere{Radius: 1.0}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 0.9, 0}, - Rotation: mgl64.QuatIdent(), - }, - } - - bodyB := &actor.RigidBody{ - Shape: &actor.Plane{ - Normal: mgl64.Vec3{0, 1, 0}, - Distance: 0, - }, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 0, 0}, - Rotation: mgl64.QuatIdent(), - }, - } - - normal := mgl64.Vec3{0, 1, 0} - depth := 0.1 - - points := GenerateManifold(bodyA, bodyB, normal, depth) - - // Sphere has 1 point, should use trivial case - if len(points) != 1 { - t.Errorf("len(points) = %d, want 1", len(points)) - } - }) - - t.Run("rotated_box_box", func(t *testing.T) { - bodyA := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{1, 1, 1}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 0, 0}, - Rotation: mgl64.QuatRotate(math.Pi/6, mgl64.Vec3{0, 1, 0}), - }, - } - - bodyB := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{1, 1, 1}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 1.8, 0}, - Rotation: mgl64.QuatRotate(math.Pi/4, mgl64.Vec3{0, 1, 0}), - }, - } - - normal := mgl64.Vec3{0, 1, 0} - depth := 0.2 - - points := GenerateManifold(bodyA, bodyB, normal, depth) - - if len(points) == 0 { - t.Error("len(points) = 0, expected contact points for rotated boxes") - } - - if len(points) > 4 { - t.Errorf("len(points) = %d, should not exceed 4", len(points)) - } +func sortedByAngle(m constraint.Manifold) []mgl64.Vec3 { + pts := sortedPositions(m) + center := mgl64.Vec3{} + for _, p := range pts { + center = center.Add(p) + } + center = center.Mul(1 / float64(len(pts))) + sort.Slice(pts, func(i, j int) bool { + return math.Atan2(pts[i].Z()-center.Z(), pts[i].X()-center.X()) < math.Atan2(pts[j].Z()-center.Z(), pts[j].X()-center.X()) }) + return pts } -// TestManifoldEdgeCases tests edge cases and numerical stability -func TestManifoldEdgeCases(t *testing.T) { - t.Run("zero_depth", func(t *testing.T) { - bodyA := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{1, 1, 1}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 0, 0}, - Rotation: mgl64.QuatIdent(), - }, - } - - bodyB := &actor.RigidBody{ - Shape: &actor.Box{HalfExtents: mgl64.Vec3{1, 1, 1}}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 2, 0}, - Rotation: mgl64.QuatIdent(), - }, - } - - normal := mgl64.Vec3{0, 1, 0} - depth := 0.0 - - points := GenerateManifold(bodyA, bodyB, normal, depth) - - // Should still work with zero depth - if len(points) == 0 { - t.Error("len(points) = 0, should handle zero depth") - } - - for i, p := range points { - if p.Penetration != 0.0 { - t.Errorf("points[%d].Penetration = %v, want 0", i, p.Penetration) - } - } - }) - - t.Run("tiny_penetration", func(t *testing.T) { - bodyA := &actor.RigidBody{ - Shape: &actor.Sphere{Radius: 1.0}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 0, 0}, - Rotation: mgl64.QuatIdent(), - }, - } +// A capsule lying on a box face: its side line, clipped to the face. +func TestManifoldCapsuleOnFace(t *testing.T) { + ground := body(mgl64.Vec3{}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{1, 0.5, 1}}) + capsule := body(mgl64.Vec3{0.5, 0.79, 0}, mgl64.QuatRotate(math.Pi/2, mgl64.Vec3{0, 0, 1}), &actor.Capsule{HalfHeight: 1, Radius: 0.3}) + m := manifold(t, ground, capsule, 0) + if m.Count != 2 { + t.Fatalf("got %d points, want 2", m.Count) + } + xs := []float64{m.Points[0].Position.X(), m.Points[1].Position.X()} + sort.Float64s(xs) + // Capsule line x in [-0.5, 1.5], face x in [-1, 1]: clipped to [-0.5, 1]. + if math.Abs(xs[0]+0.5) > 1e-6 || math.Abs(xs[1]-1) > 1e-6 { + t.Errorf("points x=%v, want [-0.5 1]", xs) + } +} - bodyB := &actor.RigidBody{ - Shape: &actor.Sphere{Radius: 1.0}, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 1.999999, 0}, - Rotation: mgl64.QuatIdent(), - }, +func TestClipAgainstPlane(t *testing.T) { + square := polygon{points: [maxBufferSize]mgl64.Vec3{{-1, 0, -1}, {1, 0, -1}, {1, 0, 1}, {-1, 0, 1}}, count: 4} + var out polygon + clipAgainstPlane(&square, mgl64.Vec3{0, 0, 0}, mgl64.Vec3{1, 0, 0}, &out) + if out.count != 4 { + t.Fatalf("half square: %d points, want 4", out.count) + } + for i := 0; i < out.count; i++ { + if out.points[i].X() < -1e-12 { + t.Errorf("point %v kept on the wrong side", out.points[i]) } + } - normal := mgl64.Vec3{0, 1, 0} - depth := 1e-12 - - points := GenerateManifold(bodyA, bodyB, normal, depth) - - if len(points) == 0 { - t.Error("len(points) = 0, should handle tiny penetration") - } - }) + segment := polygon{points: [maxBufferSize]mgl64.Vec3{{-1, 0, 0}, {1, 0, 0}}, count: 2} + clipAgainstPlane(&segment, mgl64.Vec3{0.5, 0, 0}, mgl64.Vec3{-1, 0, 0}, &out) + if out.count != 2 || !near(out.points[0], mgl64.Vec3{-1, 0, 0}, 1e-12) || !near(out.points[1], mgl64.Vec3{0.5, 0, 0}, 1e-12) { + t.Errorf("clipped segment %v, want [-1, 0.5] once (no duplicate)", out.points[:out.count]) + } } -// BenchmarkManifoldBoxBox-16 1000000 1030 ns/op 128 B/op 1 allocs/op func BenchmarkManifoldBoxBox(b *testing.B) { - bodyA := &actor.RigidBody{ - Shape: &actor.Box{ - HalfExtents: mgl64.Vec3{1.5, 1.5, 1.5}, - }, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 0, 0}, - Rotation: mgl64.QuatIdent(), - }, + ground := body(mgl64.Vec3{}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{2, 0.5, 2}}) + box := unitBox(mgl64.Vec3{0.3, 0.99, -0.4}, mgl64.QuatRotate(0.3, mgl64.Vec3{0, 1, 0})) + simplex := &gjk.Simplex{} + gjk.GJKMargin(ground, box, 0.02, simplex) + result, _ := EPA(ground, box, simplex, 0.02) + var m constraint.Manifold + b.ReportAllocs() + for i := 0; i < b.N; i++ { + Manifold(ground, box, result, 0.02, &m) } - bodyA.Transform.InverseRotation = bodyA.Transform.Rotation.Inverse() +} - bodyB := &actor.RigidBody{ - Shape: &actor.Box{ - HalfExtents: mgl64.Vec3{1.5, 1.5, 1.5}, - }, - Transform: actor.Transform{ - Position: mgl64.Vec3{0, 3, 0}, - Rotation: mgl64.QuatIdent(), - }, +// The reference face is whichever face lies flat against the normal, A's or B's. +func TestChooseReference(t *testing.T) { + flat := polygon{points: [maxBufferSize]mgl64.Vec3{{-1, 0, -1}, {1, 0, -1}, {1, 0, 1}, {-1, 0, 1}}, count: 4} + tilted := polygon{points: [maxBufferSize]mgl64.Vec3{{-1, -0.1, -1}, {1, 0.1, -1}, {1, 0.1, 1}, {-1, -0.1, 1}}, count: 4} + segment := polygon{points: [maxBufferSize]mgl64.Vec3{{-1, 0, 0}, {1, 0, 0}}, count: 2} + up := mgl64.Vec3{0, 1, 0} + for _, tc := range []struct { + name string + a, b *polygon + isA, found bool + }{ + {"both flat: A", &flat, &flat, true, true}, + {"only B flat", &tilted, &flat, false, true}, + {"only A flat", &flat, &tilted, true, true}, + {"segment on a face", &segment, &flat, false, true}, + {"no face in contact", &tilted, &segment, false, false}, + } { + isA, found := chooseReference(tc.a, tc.b, up) + if found != tc.found || (found && isA != tc.isA) { + t.Errorf("%s: reference A=%v found=%v, want A=%v found=%v", tc.name, isA, found, tc.isA, tc.found) + } } - bodyB.Transform.InverseRotation = bodyB.Transform.Rotation.Inverse() - - normal := mgl64.Vec3{0, 1, 0} - depth := 0.01 - - b.ResetTimer() - b.ReportAllocs() +} - for i := 0; i < b.N; i++ { - points := GenerateManifold(bodyA, bodyB, normal, depth) - if len(points) == 0 { - b.Fatal("No contact points generated") +// A capsule lying along a box edge touches it along a line: two points, not one. +func TestManifoldCapsuleAlongEdge(t *testing.T) { + box := unitBox(mgl64.Vec3{}, mgl64.QuatIdent()) + direction := mgl64.Vec3{1, 1, 0}.Normalize() + center := mgl64.Vec3{0.5, 0.5, 0}.Add(direction.Mul(0.29)) + capsule := body(center, mgl64.QuatRotate(math.Pi/2, mgl64.Vec3{1, 0, 0}), &actor.Capsule{HalfHeight: 1, Radius: 0.3}) + m := manifold(t, box, capsule, 0) + if m.Count != 2 { + t.Fatalf("got %d points, want the edge z in [-0.5, 0.5]", m.Count) + } + for i := 0; i < m.Count; i++ { + if math.Abs(math.Abs(m.Points[i].Position.Z())-0.5) > 1e-6 || math.Abs(m.Points[i].Separation+0.01) > 1e-6 { + t.Errorf("point %+v, want z=±0.5 separation -0.01", m.Points[i]) } } } diff --git a/epa/polytope.go b/epa/polytope.go deleted file mode 100644 index 8980367..0000000 --- a/epa/polytope.go +++ /dev/null @@ -1,435 +0,0 @@ -package epa - -import ( - "fmt" - "math" - "sync" - - "github.com/akmonengine/feather/gjk" - "github.com/go-gl/mathgl/mgl64" -) - -// PolytopeBuilder manages polytope expansion with dynamic buffers and initial capacity. -type PolytopeBuilder struct { - // Face storage - dynamic with initial capacity - // Stores all faces in the current polytope - faces []Face - - // Point deduplication buffer for centroid calculation - // Uses sorted slice with binary search for deduplication - uniquePoints []mgl64.Vec3 - - // Edge tracking for boundary detection - // Normalized edges (A < B) with occurrence count - edges []EdgeEntry - - // Visible face tracking - visibleIndices []int -} - -// EdgeEntry represents an edge with occurrence counting for boundary detection. -// An edge is a boundary edge if it appears exactly once (count == 1). -// Edges are normalized so A < B lexicographically for consistent deduplication. -type EdgeEntry struct { - A, B mgl64.Vec3 // Edge vertices (normalized: A < B) - Count int // Occurrence count (1 = boundary edge, 2+ = internal edge) -} - -// polytopeBuilderPool is the single sync.Pool for PolytopeBuilder instances. -// This eliminates allocation of builder structures during EPA iterations. -var polytopeBuilderPool = sync.Pool{ - New: func() interface{} { - return &PolytopeBuilder{ - faces: make([]Face, 0, polytopeInitialCapacity), - uniquePoints: make([]mgl64.Vec3, 0, polytopeInitialCapacity), - edges: make([]EdgeEntry, 0, polytopeInitialCapacity), - visibleIndices: make([]int, 0, polytopeInitialCapacity), - } - }, -} - -// Reset prepares the builder for reuse by clearing all slices. -// This allows the builder to be reused from the pool without reallocation. -func (b *PolytopeBuilder) Reset() { - b.faces = b.faces[:0] - b.uniquePoints = b.uniquePoints[:0] - b.edges = b.edges[:0] - b.visibleIndices = b.visibleIndices[:0] -} - -// BuildInitialFaces creates the initial polytope from a GJK tetrahedron simplex. -// Creates 4 triangular faces from the 4 simplex points, filtering degenerate faces. -// -// Returns error if simplex is invalid (count != 4). -func (b *PolytopeBuilder) BuildInitialFaces(simplex *gjk.Simplex) error { - if simplex.Count != 4 { - return fmt.Errorf("invalid simplex count: %d (expected 4)", simplex.Count) - } - - p0, p1, p2, p3 := simplex.Points[0], simplex.Points[1], simplex.Points[2], simplex.Points[3] - - // Create 4 candidate faces (one for each tetrahedron face) - // Each face is defined by 3 points + the opposite point for normal orientation - candidateFaces := [4]Face{ - b.createFaceOutward(p0, p1, p2, p3), // Face ABC, opposite point is D - b.createFaceOutward(p0, p2, p3, p1), // Face ACD, opposite point is B - b.createFaceOutward(p0, p3, p1, p2), // Face ADB, opposite point is C - b.createFaceOutward(p1, p3, p2, p0), // Face BDC, opposite point is A - } - - // Filter valid faces (distance >= EPAMinFaceDistance) - for i := 0; i < 4; i++ { - if candidateFaces[i].Distance >= EPAMinFaceDistance { - b.faces = append(b.faces, candidateFaces[i]) - } - } - - // Safety: need at least 3 faces for valid polytope - if len(b.faces) < 3 { - // Keep all faces for degenerate case - b.faces = b.faces[:0] - for i := 0; i < 4; i++ { - b.faces = append(b.faces, candidateFaces[i]) - } - } - - return nil -} - -// createFaceOutward creates a Face with normal pointing outward from the polytope. -// Uses the opposite point as a reference to determine the correct normal orientation. -// -// Algorithm: -// 1. Compute normal via cross product: (b-a) × (c-a) -// 2. Check if normal points toward opposite point (inward) → flip if needed -// 3. Ensure distance is positive (normal away from origin) -// 4. Snap near-zero components for numerical stability -func (b *PolytopeBuilder) createFaceOutward(p0, p1, p2, oppositePoint mgl64.Vec3) Face { - var face Face - face.Points = [3]mgl64.Vec3{p0, p1, p2} - - // Calculate two edges of the triangle - edge1 := p1.Sub(p0) - edge2 := p2.Sub(p0) - - // Normal via cross product (right-hand rule) - normal := edge1.Cross(edge2) - - // Normalize the normal - normalLength := math.Sqrt(normal.Dot(normal)) - if normalLength < 1e-8 { - // Degenerate triangle (zero area) - face.Normal = mgl64.Vec3{0, 1, 0} - face.Distance = EPAMinFaceDistance - return face - } - normal = normal.Mul(1.0 / normalLength) - - // === CRITICAL: Ensure normal points OUTWARD === - // Vector from face point p0 to the opposite point - toOpposite := oppositePoint.Sub(p0) - - // If normal points TOWARDS the opposite point, it's pointing INWARD - // We need to flip it to point OUTWARD - if normal.Dot(toOpposite) > 0 { - normal = normal.Mul(-1) - } - - // Calculate distance from origin to the plane - distance := p0.Dot(normal) - - // Distance should be positive (normal points away from origin) - if distance < 0 { - normal = normal.Mul(-1) - distance = -distance - } - - // Force minimum distance to avoid degenerate cases - if distance < EPAMinFaceDistance { - distance = EPAMinFaceDistance - } - - face.Normal = snapNormalToAxis(normal) - face.Distance = distance - - return face -} - -// FindClosestFaceIndex returns the index of the face closest to the origin. -// Returns -1 if no faces exist. -func (b *PolytopeBuilder) FindClosestFaceIndex() int { - if len(b.faces) == 0 { - return -1 - } - - closestIndex := 0 - minDistance := b.faces[0].Distance - - for i := 1; i < len(b.faces); i++ { - if b.faces[i].Distance < minDistance { - closestIndex = i - minDistance = b.faces[i].Distance - } - } - - return closestIndex -} - -// calculateCentroid computes the centroid (average position) of all unique points -// in the current polytope. Uses dynamic slice with binary search for -// point deduplication. -func (b *PolytopeBuilder) calculateCentroid() mgl64.Vec3 { - // Collect unique points using sorted slice - b.uniquePoints = b.uniquePoints[:0] // Clear existing points - - for i := 0; i < len(b.faces); i++ { - face := &b.faces[i] - for j := 0; j < 3; j++ { - point := face.Points[j] - - // Binary search for insertion point - insertIdx := b.findPointInsertionIndex(point) - - // Check if point already exists - if insertIdx < len(b.uniquePoints) && vec3Equal(b.uniquePoints[insertIdx], point) { - continue // Already have this point - } - - // Insert point (shift slice right) - if insertIdx < len(b.uniquePoints) { - // Make sure we have enough capacity - if cap(b.uniquePoints) == len(b.uniquePoints) { - newCap := len(b.uniquePoints) * 2 - if newCap == 0 { - newCap = polytopeInitialCapacity - } - newPoints := make([]mgl64.Vec3, len(b.uniquePoints), newCap) - copy(newPoints, b.uniquePoints) - b.uniquePoints = newPoints - } - - // Shift elements to make space - b.uniquePoints = append(b.uniquePoints, mgl64.Vec3{}) // Add space - copy(b.uniquePoints[insertIdx+1:], b.uniquePoints[insertIdx:]) - b.uniquePoints[insertIdx] = point - } else { - b.uniquePoints = append(b.uniquePoints, point) - } - } - } - - // Calculate average - if len(b.uniquePoints) == 0 { - return mgl64.Vec3{0, 0, 0} - } - - sum := mgl64.Vec3{0, 0, 0} - for i := 0; i < len(b.uniquePoints); i++ { - sum = sum.Add(b.uniquePoints[i]) - } - - return sum.Mul(1.0 / float64(len(b.uniquePoints))) -} - -// findPointInsertionIndex performs binary search to find the correct insertion -// index for a point in the sorted uniquePoints array. -func (b *PolytopeBuilder) findPointInsertionIndex(point mgl64.Vec3) int { - left, right := 0, len(b.uniquePoints) - - for left < right { - mid := (left + right) / 2 - cmp := compareVec3(b.uniquePoints[mid], point) - - if cmp < 0 { - left = mid + 1 - } else { - right = mid - } - } - - return left -} - -// findBoundaryEdges identifies boundary edges from visible faces. -// A boundary edge appears exactly once (count == 1), while internal edges -// appear twice and are filtered out. -// -// Uses dynamic slice with linear search for edge tracking. -func (b *PolytopeBuilder) findBoundaryEdges() error { - b.edges = b.edges[:0] // Clear existing edges - - // Collect all edges from visible faces - for i := 0; i < len(b.visibleIndices); i++ { - faceIdx := b.visibleIndices[i] - face := &b.faces[faceIdx] - - // Three edges per triangle - edges := [3][2]mgl64.Vec3{ - {face.Points[0], face.Points[1]}, - {face.Points[1], face.Points[2]}, - {face.Points[2], face.Points[0]}, - } - - for _, edge := range edges { - // Normalize edge (A < B lexicographically) - edgeA, edgeB := edge[0], edge[1] - if compareVec3(edgeA, edgeB) > 0 { - edgeA, edgeB = edgeB, edgeA - } - - // Find or insert edge - edgeIdx := b.findEdgeIndex(edgeA, edgeB) - - if edgeIdx >= 0 { - // Edge exists, increment count - b.edges[edgeIdx].Count++ - } else { - // New edge - no buffer overflow possible with dynamic slices - b.edges = append(b.edges, EdgeEntry{ - A: edgeA, - B: edgeB, - Count: 1, - }) - } - } - } - - return nil -} - -// findEdgeIndex performs linear search for an edge in the edges buffer. -// Returns the index if found, -1 otherwise. -// Linear search is efficient for small edge counts (typically < 30). -func (b *PolytopeBuilder) findEdgeIndex(edgeA, edgeB mgl64.Vec3) int { - for i := 0; i < len(b.edges); i++ { - edge := &b.edges[i] - if vec3Equal(edge.A, edgeA) && vec3Equal(edge.B, edgeB) { - return i - } - } - return -1 -} - -// findVisibleFaces populates visibleIndices with faces visible from the support point. -// A face is visible if the vector from the face to the support point points in the -// same direction as the face normal (dot product > 0). -func (b *PolytopeBuilder) findVisibleFaces(support mgl64.Vec3) { - b.visibleIndices = b.visibleIndices[:0] // Clear existing indices - - for i := 0; i < len(b.faces); i++ { - face := &b.faces[i] - toSupport := support.Sub(face.Points[0]) - - if toSupport.Dot(face.Normal) > 0 { - b.visibleIndices = append(b.visibleIndices, i) - } - } -} - -// removeVisibleFaces removes faces marked in visibleIndices using swap-with-last pattern. -// Indices are sorted descending to prevent index invalidation during removal. -func (b *PolytopeBuilder) removeVisibleFaces() { - // Sort indices descending to remove from end first - for i := 0; i < len(b.visibleIndices)-1; i++ { - for j := i + 1; j < len(b.visibleIndices); j++ { - if b.visibleIndices[i] < b.visibleIndices[j] { - b.visibleIndices[i], b.visibleIndices[j] = b.visibleIndices[j], b.visibleIndices[i] - } - } - } - - // Remove faces using swap-with-last - for i := 0; i < len(b.visibleIndices); i++ { - idx := b.visibleIndices[i] - - if idx < len(b.faces) { - // Swap with last element - b.faces[idx] = b.faces[len(b.faces)-1] - b.faces = b.faces[:len(b.faces)-1] - } - } -} - -// addBoundaryFaces creates new faces connecting boundary edges to the support point. -// Only processes edges with count == 1 (boundary edges). -func (b *PolytopeBuilder) addBoundaryFaces(support mgl64.Vec3, centroid mgl64.Vec3) error { - // Iterate through edges with count == 1 (boundary) - for i := 0; i < len(b.edges); i++ { - edge := &b.edges[i] - - if edge.Count != 1 { - continue // Not a boundary edge - } - - // Create new face - newFace := b.createFaceOutward(edge.A, edge.B, support, centroid) - - // Add to slice - no buffer overflow possible with dynamic slices - b.faces = append(b.faces, newFace) - } - - return nil -} - -// AddPointAndRebuildFaces expands the polytope by adding a support point. -// This is the main EPA expansion step that: -// 1. Finds visible faces from the support point -// 2. Identifies boundary edges of the visible region -// 3. Removes visible faces -// 4. Creates new faces connecting boundary edges to the support point -// -// All operations use fixed buffers for zero allocations. -func (b *PolytopeBuilder) AddPointAndRebuildFaces(support mgl64.Vec3, closestIndex int) error { - // Calculate centroid (zero allocations) - centroid := b.calculateCentroid() - - // Find visible faces - b.findVisibleFaces(support) - - // Safety: don't remove all faces - if len(b.visibleIndices) >= len(b.faces) { - b.visibleIndices = b.visibleIndices[:0] - b.visibleIndices = append(b.visibleIndices, closestIndex) - } - - // Find boundary edges (zero allocations) - if err := b.findBoundaryEdges(); err != nil { - return err - } - - // Remove visible faces - b.removeVisibleFaces() - - // Add new faces from boundary - if err := b.addBoundaryFaces(support, centroid); err != nil { - return err - } - - // Safety check: ensure at least one face exists - if len(b.faces) == 0 { - // Create fallback face - b.faces = append(b.faces, Face{ - Points: [3]mgl64.Vec3{support, support, support}, - Normal: mgl64.Vec3{0, 1, 0}, - Distance: EPAMinFaceDistance, - }) - } - - return nil -} - -// GetClosestFace returns a pointer to the closest face for EPA result. -// Returns nil if no faces exist. -func (b *PolytopeBuilder) GetClosestFace() *Face { - if len(b.faces) == 0 { - return nil - } - idx := b.FindClosestFaceIndex() - return &b.faces[idx] -} - -// vec3Equal performs exact equality check for point deduplication. -// Uses exact float comparison (no epsilon) since we need exact deduplication. -func vec3Equal(a, b mgl64.Vec3) bool { - return a[0] == b[0] && a[1] == b[1] && a[2] == b[2] -} diff --git a/event.go b/event.go index f960438..16c8191 100644 --- a/event.go +++ b/event.go @@ -1,21 +1,19 @@ package feather import ( - "unsafe" - "github.com/akmonengine/feather/actor" "github.com/akmonengine/feather/constraint" ) const ( - TRIGGER_ENTER EventType = iota - COLLISION_ENTER - TRIGGER_STAY - COLLISION_STAY - TRIGGER_EXIT - COLLISION_EXIT - ON_SLEEP - ON_WAKE + EventTriggerEnter EventType = iota + EventCollisionEnter + EventTriggerStay + EventCollisionStay + EventTriggerExit + EventCollisionExit + EventSleep + EventWake ) type pairKey struct { @@ -23,12 +21,10 @@ type pairKey struct { bodyB *actor.RigidBody } -// makePairKey creates a normalized pair key with consistent ordering +// makePairKey creates a normalized pair key with consistent ordering (the serials of the bodies). +// The key is only used to find a pair, never to order the computations func makePairKey(bodyA, bodyB *actor.RigidBody) pairKey { - ptrA := uintptr(unsafe.Pointer(bodyA)) - ptrB := uintptr(unsafe.Pointer(bodyB)) - - if ptrB < ptrA { + if bodyB.Serial() < bodyA.Serial() { bodyA, bodyB = bodyB, bodyA } @@ -48,21 +44,21 @@ type TriggerEnterEvent struct { BodyB *actor.RigidBody } -func (e TriggerEnterEvent) Type() EventType { return TRIGGER_ENTER } +func (e TriggerEnterEvent) Type() EventType { return EventTriggerEnter } type TriggerStayEvent struct { BodyA *actor.RigidBody BodyB *actor.RigidBody } -func (e TriggerStayEvent) Type() EventType { return TRIGGER_STAY } +func (e TriggerStayEvent) Type() EventType { return EventTriggerStay } type TriggerExitEvent struct { BodyA *actor.RigidBody BodyB *actor.RigidBody } -func (e TriggerExitEvent) Type() EventType { return TRIGGER_EXIT } +func (e TriggerExitEvent) Type() EventType { return EventTriggerExit } // Collision events type CollisionEnterEvent struct { @@ -70,34 +66,34 @@ type CollisionEnterEvent struct { BodyB *actor.RigidBody } -func (e CollisionEnterEvent) Type() EventType { return COLLISION_ENTER } +func (e CollisionEnterEvent) Type() EventType { return EventCollisionEnter } type CollisionStayEvent struct { BodyA *actor.RigidBody BodyB *actor.RigidBody } -func (e CollisionStayEvent) Type() EventType { return COLLISION_STAY } +func (e CollisionStayEvent) Type() EventType { return EventCollisionStay } type CollisionExitEvent struct { BodyA *actor.RigidBody BodyB *actor.RigidBody } -func (e CollisionExitEvent) Type() EventType { return COLLISION_EXIT } +func (e CollisionExitEvent) Type() EventType { return EventCollisionExit } // Sleep/Wake events type SleepEvent struct { Body *actor.RigidBody } -func (e SleepEvent) Type() EventType { return ON_SLEEP } +func (e SleepEvent) Type() EventType { return EventSleep } type WakeEvent struct { Body *actor.RigidBody } -func (e WakeEvent) Type() EventType { return ON_WAKE } +func (e WakeEvent) Type() EventType { return EventWake } // EventListener - callback for events type EventListener func(event Event) @@ -111,6 +107,9 @@ type Events struct { buffer []Event // Collision tracking for Enter/Stay/Exit detection + // The slices keep the order of the pairs, so the events are always sent in the same order + previousPairs []pairKey + currentPairs []pairKey previousActivePairs map[pairKey]bool currentActivePairs map[pairKey]bool @@ -132,28 +131,58 @@ func (e *Events) Subscribe(eventType EventType, listener EventListener) { e.listeners[eventType] = append(e.listeners[eventType], listener) } -// recordCollision is called during substeps to record a collision/trigger -func (e *Events) recordCollisions(constraints []*constraint.ContactConstraint) []*constraint.ContactConstraint { - n := 0 - for _, c := range constraints { - pair := makePairKey(c.BodyA, c.BodyB) - e.currentActivePairs[pair] = true +// touchingDistance: bodies closer than this distance are touching (for the collision events) +const touchingDistance = LinearSlop - if !c.BodyA.IsTrigger && !c.BodyB.IsTrigger { - constraints[n] = c +// recordCollisions records the pairs in contact, and returns the manifolds to solve (triggers are removed). +// Speculative contacts are solved but do not send events +func (e *Events) recordCollisions(manifolds []constraint.Manifold) []constraint.Manifold { + tracked := e.tracksCollisions() + n := 0 + for i := range manifolds { + m := &manifolds[i] + isTrigger := m.BodyA.IsTrigger || m.BodyB.IsTrigger + if tracked && (isTrigger || m.MinSeparation() <= touchingDistance) { + e.record(makePairKey(m.BodyA, m.BodyB)) + } + if !isTrigger { + manifolds[n] = *m n++ } } - constraints = constraints[:n] + return manifolds[:n] +} + +func (e *Events) record(pair pairKey) { + if e.currentActivePairs == nil { + *e = NewEvents() + } + if !e.currentActivePairs[pair] { + e.currentActivePairs[pair] = true + e.currentPairs = append(e.currentPairs, pair) + } +} - return constraints +// forget a removed body +func (e *Events) forget(body *actor.RigidBody) { + delete(e.sleepStates, body) + n := 0 + for _, pair := range e.previousPairs { + if pair.bodyA == body || pair.bodyB == body { + delete(e.previousActivePairs, pair) + continue + } + e.previousPairs[n] = pair + n++ + } + e.previousPairs = e.previousPairs[:n] } // processCollisionEvents compares current and previous pairs to detect Enter/Stay/Exit // Should be called after all substeps func (e *Events) processCollisionEvents() { // Detect Enter and Stay events - for pair := range e.currentActivePairs { + for _, pair := range e.currentPairs { // Skip if both bodies are sleeping, to avoid spamming events if pair.bodyA.IsSleeping && pair.bodyB.IsSleeping { continue @@ -164,58 +193,82 @@ func (e *Events) processCollisionEvents() { if e.previousActivePairs[pair] { // Pair was active before and still is, Stay if isTrigger { - e.buffer = append(e.buffer, TriggerStayEvent{ - BodyA: pair.bodyA, - BodyB: pair.bodyB, - }) + if e.hasListeners(EventTriggerStay) { + e.buffer = append(e.buffer, TriggerStayEvent{ + BodyA: pair.bodyA, + BodyB: pair.bodyB, + }) + } } else { - e.buffer = append(e.buffer, CollisionStayEvent{ - BodyA: pair.bodyA, - BodyB: pair.bodyB, - }) + if e.hasListeners(EventCollisionStay) { + e.buffer = append(e.buffer, CollisionStayEvent{ + BodyA: pair.bodyA, + BodyB: pair.bodyB, + }) + } } } else { // New pair, Enter if isTrigger { - e.buffer = append(e.buffer, TriggerEnterEvent{ - BodyA: pair.bodyA, - BodyB: pair.bodyB, - }) + if e.hasListeners(EventTriggerEnter) { + e.buffer = append(e.buffer, TriggerEnterEvent{ + BodyA: pair.bodyA, + BodyB: pair.bodyB, + }) + } } else { - e.buffer = append(e.buffer, CollisionEnterEvent{ - BodyA: pair.bodyA, - BodyB: pair.bodyB, - }) + if e.hasListeners(EventCollisionEnter) { + e.buffer = append(e.buffer, CollisionEnterEvent{ + BodyA: pair.bodyA, + BodyB: pair.bodyB, + }) + } } } } // Detect Exit events - for pair := range e.previousActivePairs { + for _, pair := range e.previousPairs { + // Sleeping pairs are not detected anymore, but they are still touching + if !e.currentActivePairs[pair] && pair.bodyA.IsSleeping && pair.bodyB.IsSleeping { + e.record(pair) + continue + } if !e.currentActivePairs[pair] { // Pair was active but is no longer, Exit isTrigger := pair.bodyA.IsTrigger || pair.bodyB.IsTrigger if isTrigger { - e.buffer = append(e.buffer, TriggerExitEvent{ - BodyA: pair.bodyA, - BodyB: pair.bodyB, - }) + if e.hasListeners(EventTriggerExit) { + e.buffer = append(e.buffer, TriggerExitEvent{ + BodyA: pair.bodyA, + BodyB: pair.bodyB, + }) + } } else { - e.buffer = append(e.buffer, CollisionExitEvent{ - BodyA: pair.bodyA, - BodyB: pair.bodyB, - }) + if e.hasListeners(EventCollisionExit) { + e.buffer = append(e.buffer, CollisionExitEvent{ + BodyA: pair.bodyA, + BodyB: pair.bodyB, + }) + } } } } // Swap for next frame and clear current e.previousActivePairs, e.currentActivePairs = e.currentActivePairs, e.previousActivePairs + e.previousPairs, e.currentPairs = e.currentPairs, e.previousPairs[:0] clear(e.currentActivePairs) } func (e *Events) processSleepEvents(bodies []*actor.RigidBody) { + if e.sleepStates == nil { + *e = NewEvents() + } + if !e.hasListeners(EventSleep) && !e.hasListeners(EventWake) { + return + } for _, body := range bodies { trackedState, exists := e.sleepStates[body] if !exists { @@ -224,15 +277,34 @@ func (e *Events) processSleepEvents(bodies []*actor.RigidBody) { } if !trackedState && body.IsSleeping { - e.buffer = append(e.buffer, SleepEvent{Body: body}) + if e.hasListeners(EventSleep) { + e.buffer = append(e.buffer, SleepEvent{Body: body}) + } e.sleepStates[body] = true } else if trackedState && !body.IsSleeping { - e.buffer = append(e.buffer, WakeEvent{Body: body}) + if e.hasListeners(EventWake) { + e.buffer = append(e.buffer, WakeEvent{Body: body}) + } e.sleepStates[body] = false } } } +// hasListeners: an event is only created if somebody listens to it (creating an event allocates) +func (e *Events) hasListeners(eventType EventType) bool { + return len(e.listeners[eventType]) > 0 +} + +// tracksCollisions: the pairs in contact are recorded only if somebody listens to the collisions or the triggers +func (e *Events) tracksCollisions() bool { + for _, t := range [6]EventType{EventTriggerEnter, EventCollisionEnter, EventTriggerStay, EventCollisionStay, EventTriggerExit, EventCollisionExit} { + if e.hasListeners(t) { + return true + } + } + return false +} + // flush sends all buffered events and clears the buffer func (e *Events) flush() { e.processCollisionEvents() diff --git a/event_test.go b/event_test.go index e67f47c..8fee0e9 100644 --- a/event_test.go +++ b/event_test.go @@ -23,19 +23,11 @@ func createTestBody(id interface{}, isTrigger, isSleeping bool) *actor.RigidBody return rb } -// createTestConstraint creates a ContactConstraint for testing -func createTestConstraint(bodyA, bodyB *actor.RigidBody) *constraint.ContactConstraint { - return &constraint.ContactConstraint{ - BodyA: bodyA, - BodyB: bodyB, - Normal: mgl64.Vec3{1, 0, 0}, - Points: []constraint.ContactPoint{ - { - Position: mgl64.Vec3{0, 0, 0}, - Penetration: 0.1, - }, - }, - } +// createTestConstraint creates a touching manifold for testing +func createTestConstraint(bodyA, bodyB *actor.RigidBody) constraint.Manifold { + m := constraint.Manifold{BodyA: bodyA, BodyB: bodyB, Normal: mgl64.Vec3{1, 0, 0}} + m.Add(mgl64.Vec3{0, 0, 0}, -0.1) + return m } type eventCapture struct { @@ -71,11 +63,11 @@ func TestEvents_Subscribe(t *testing.T) { events := NewEvents() capture := &eventCapture{} - events.Subscribe(COLLISION_ENTER, capture.capture) + events.Subscribe(EventCollisionEnter, capture.capture) // Verify listener is registered - if len(events.listeners[COLLISION_ENTER]) != 1 { - t.Errorf("Expected 1 listener for COLLISION_ENTER, got %d", len(events.listeners[COLLISION_ENTER])) + if len(events.listeners[EventCollisionEnter]) != 1 { + t.Errorf("Expected 1 listener for EventCollisionEnter, got %d", len(events.listeners[EventCollisionEnter])) } } @@ -86,13 +78,13 @@ func TestEvents_MultipleListeners(t *testing.T) { capture3 := &eventCapture{} // Subscribe multiple listeners to the same event type - events.Subscribe(COLLISION_ENTER, capture1.capture) - events.Subscribe(COLLISION_ENTER, capture2.capture) - events.Subscribe(COLLISION_ENTER, capture3.capture) + events.Subscribe(EventCollisionEnter, capture1.capture) + events.Subscribe(EventCollisionEnter, capture2.capture) + events.Subscribe(EventCollisionEnter, capture3.capture) // Verify all listeners are registered - if len(events.listeners[COLLISION_ENTER]) != 3 { - t.Errorf("Expected 3 listeners for COLLISION_ENTER, got %d", len(events.listeners[COLLISION_ENTER])) + if len(events.listeners[EventCollisionEnter]) != 3 { + t.Errorf("Expected 3 listeners for EventCollisionEnter, got %d", len(events.listeners[EventCollisionEnter])) } // Trigger an event @@ -100,7 +92,7 @@ func TestEvents_MultipleListeners(t *testing.T) { bodyB := createTestBody("B", false, false) c := createTestConstraint(bodyA, bodyB) - events.recordCollisions([]*constraint.ContactConstraint{c}) + events.recordCollisions([]constraint.Manifold{c}) events.flush() // All listeners should have received the event @@ -120,15 +112,15 @@ func TestEvents_DifferentEventTypes(t *testing.T) { captureCollision := &eventCapture{} captureTrigger := &eventCapture{} - events.Subscribe(COLLISION_ENTER, captureCollision.capture) - events.Subscribe(TRIGGER_ENTER, captureTrigger.capture) + events.Subscribe(EventCollisionEnter, captureCollision.capture) + events.Subscribe(EventTriggerEnter, captureTrigger.capture) // Trigger a collision event bodyA := createTestBody("A", false, false) bodyB := createTestBody("B", false, false) c := createTestConstraint(bodyA, bodyB) - events.recordCollisions([]*constraint.ContactConstraint{c}) + events.recordCollisions([]constraint.Manifold{c}) events.flush() // Only collision listener should receive event @@ -192,13 +184,15 @@ func TestMakePairKey_DifferentPairs(t *testing.T) { func TestEvents_RecordCollisions_NormalCollision(t *testing.T) { events := NewEvents() + // the pairs are recorded only if somebody listens + events.Subscribe(EventCollisionEnter, func(Event) {}) // Two normal bodies bodyA := createTestBody("A", false, false) bodyB := createTestBody("B", false, false) c := createTestConstraint(bodyA, bodyB) - constraints := []*constraint.ContactConstraint{c} + constraints := []constraint.Manifold{c} result := events.recordCollisions(constraints) // Normal collision should remain in constraints @@ -215,13 +209,15 @@ func TestEvents_RecordCollisions_NormalCollision(t *testing.T) { func TestEvents_RecordCollisions_TriggerCollision(t *testing.T) { events := NewEvents() + // the pairs are recorded only if somebody listens + events.Subscribe(EventCollisionEnter, func(Event) {}) // One trigger body bodyA := createTestBody("A", true, false) bodyB := createTestBody("B", false, false) c := createTestConstraint(bodyA, bodyB) - constraints := []*constraint.ContactConstraint{c} + constraints := []constraint.Manifold{c} result := events.recordCollisions(constraints) // Trigger collision should be filtered out @@ -238,6 +234,8 @@ func TestEvents_RecordCollisions_TriggerCollision(t *testing.T) { func TestEvents_RecordCollisions_Mixed(t *testing.T) { events := NewEvents() + // the pairs are recorded only if somebody listens + events.Subscribe(EventCollisionEnter, func(Event) {}) // Setup: 1 normal collision + 1 trigger collision bodyA := createTestBody("A", false, false) @@ -248,7 +246,7 @@ func TestEvents_RecordCollisions_Mixed(t *testing.T) { c1 := createTestConstraint(bodyA, bodyB) // Normal c2 := createTestConstraint(bodyC, bodyD) // Trigger - constraints := []*constraint.ContactConstraint{c1, c2} + constraints := []constraint.Manifold{c1, c2} result := events.recordCollisions(constraints) // Only normal collision should remain @@ -269,19 +267,19 @@ func TestEvents_RecordCollisions_Mixed(t *testing.T) { func TestEvents_TriggerEnter(t *testing.T) { events := NewEvents() capture := &eventCapture{} - events.Subscribe(TRIGGER_ENTER, capture.capture) + events.Subscribe(EventTriggerEnter, capture.capture) // First frame: trigger collision bodyA := createTestBody("A", true, false) bodyB := createTestBody("B", false, false) c := createTestConstraint(bodyA, bodyB) - events.recordCollisions([]*constraint.ContactConstraint{c}) + events.recordCollisions([]constraint.Manifold{c}) events.flush() - // Should receive TRIGGER_ENTER event - if !capture.hasEventType(TRIGGER_ENTER) { - t.Error("Expected TRIGGER_ENTER event") + // Should receive EventTriggerEnter event + if !capture.hasEventType(EventTriggerEnter) { + t.Error("Expected EventTriggerEnter event") } if capture.count() != 1 { @@ -298,61 +296,61 @@ func TestEvents_TriggerEnter(t *testing.T) { func TestEvents_TriggerStay(t *testing.T) { events := NewEvents() capture := &eventCapture{} - events.Subscribe(TRIGGER_STAY, capture.capture) + events.Subscribe(EventTriggerStay, capture.capture) bodyA := createTestBody("A", true, false) bodyB := createTestBody("B", false, false) c := createTestConstraint(bodyA, bodyB) // Frame 1: Enter (should not trigger STAY) - events.recordCollisions([]*constraint.ContactConstraint{c}) + events.recordCollisions([]constraint.Manifold{c}) events.flush() - if capture.hasEventType(TRIGGER_STAY) { - t.Error("TRIGGER_STAY should not occur on first frame") + if capture.hasEventType(EventTriggerStay) { + t.Error("EventTriggerStay should not occur on first frame") } capture.reset() // Frame 2: Stay - events.recordCollisions([]*constraint.ContactConstraint{c}) + events.recordCollisions([]constraint.Manifold{c}) events.flush() - // Should receive TRIGGER_STAY event - if !capture.hasEventType(TRIGGER_STAY) { - t.Error("Expected TRIGGER_STAY event on second frame") + // Should receive EventTriggerStay event + if !capture.hasEventType(EventTriggerStay) { + t.Error("Expected EventTriggerStay event on second frame") } } func TestEvents_TriggerExit(t *testing.T) { events := NewEvents() capture := &eventCapture{} - events.Subscribe(TRIGGER_EXIT, capture.capture) + events.Subscribe(EventTriggerExit, capture.capture) bodyA := createTestBody("A", true, false) bodyB := createTestBody("B", false, false) c := createTestConstraint(bodyA, bodyB) // Frame 1: Enter - events.recordCollisions([]*constraint.ContactConstraint{c}) + events.recordCollisions([]constraint.Manifold{c}) events.flush() capture.reset() // Frame 2: Exit (no collision) - events.recordCollisions([]*constraint.ContactConstraint{}) + events.recordCollisions([]constraint.Manifold{}) events.flush() - // Should receive TRIGGER_EXIT event - if !capture.hasEventType(TRIGGER_EXIT) { - t.Error("Expected TRIGGER_EXIT event") + // Should receive EventTriggerExit event + if !capture.hasEventType(EventTriggerExit) { + t.Error("Expected EventTriggerExit event") } } func TestEvents_TriggerStay_SleepingBodies(t *testing.T) { events := NewEvents() capture := &eventCapture{} - events.Subscribe(TRIGGER_STAY, capture.capture) + events.Subscribe(EventTriggerStay, capture.capture) // Both bodies sleeping bodyA := createTestBody("A", true, true) @@ -360,18 +358,18 @@ func TestEvents_TriggerStay_SleepingBodies(t *testing.T) { c := createTestConstraint(bodyA, bodyB) // Frame 1: Enter - events.recordCollisions([]*constraint.ContactConstraint{c}) + events.recordCollisions([]constraint.Manifold{c}) events.flush() capture.reset() // Frame 2: Stay (but both sleeping) - events.recordCollisions([]*constraint.ContactConstraint{c}) + events.recordCollisions([]constraint.Manifold{c}) events.flush() - // Should NOT receive TRIGGER_STAY when both bodies are sleeping - if capture.hasEventType(TRIGGER_STAY) { - t.Error("TRIGGER_STAY should not occur when both bodies are sleeping") + // Should NOT receive EventTriggerStay when both bodies are sleeping + if capture.hasEventType(EventTriggerStay) { + t.Error("EventTriggerStay should not occur when both bodies are sleeping") } } @@ -382,19 +380,19 @@ func TestEvents_TriggerStay_SleepingBodies(t *testing.T) { func TestEvents_CollisionEnter(t *testing.T) { events := NewEvents() capture := &eventCapture{} - events.Subscribe(COLLISION_ENTER, capture.capture) + events.Subscribe(EventCollisionEnter, capture.capture) // First frame: normal collision bodyA := createTestBody("A", false, false) bodyB := createTestBody("B", false, false) c := createTestConstraint(bodyA, bodyB) - events.recordCollisions([]*constraint.ContactConstraint{c}) + events.recordCollisions([]constraint.Manifold{c}) events.flush() - // Should receive COLLISION_ENTER event - if !capture.hasEventType(COLLISION_ENTER) { - t.Error("Expected COLLISION_ENTER event") + // Should receive EventCollisionEnter event + if !capture.hasEventType(EventCollisionEnter) { + t.Error("Expected EventCollisionEnter event") } if capture.count() != 1 { @@ -411,61 +409,61 @@ func TestEvents_CollisionEnter(t *testing.T) { func TestEvents_CollisionStay(t *testing.T) { events := NewEvents() capture := &eventCapture{} - events.Subscribe(COLLISION_STAY, capture.capture) + events.Subscribe(EventCollisionStay, capture.capture) bodyA := createTestBody("A", false, false) bodyB := createTestBody("B", false, false) c := createTestConstraint(bodyA, bodyB) // Frame 1: Enter (should not trigger STAY) - events.recordCollisions([]*constraint.ContactConstraint{c}) + events.recordCollisions([]constraint.Manifold{c}) events.flush() - if capture.hasEventType(COLLISION_STAY) { - t.Error("COLLISION_STAY should not occur on first frame") + if capture.hasEventType(EventCollisionStay) { + t.Error("EventCollisionStay should not occur on first frame") } capture.reset() // Frame 2: Stay - events.recordCollisions([]*constraint.ContactConstraint{c}) + events.recordCollisions([]constraint.Manifold{c}) events.flush() - // Should receive COLLISION_STAY event - if !capture.hasEventType(COLLISION_STAY) { - t.Error("Expected COLLISION_STAY event on second frame") + // Should receive EventCollisionStay event + if !capture.hasEventType(EventCollisionStay) { + t.Error("Expected EventCollisionStay event on second frame") } } func TestEvents_CollisionExit(t *testing.T) { events := NewEvents() capture := &eventCapture{} - events.Subscribe(COLLISION_EXIT, capture.capture) + events.Subscribe(EventCollisionExit, capture.capture) bodyA := createTestBody("A", false, false) bodyB := createTestBody("B", false, false) c := createTestConstraint(bodyA, bodyB) // Frame 1: Enter - events.recordCollisions([]*constraint.ContactConstraint{c}) + events.recordCollisions([]constraint.Manifold{c}) events.flush() capture.reset() // Frame 2: Exit (no collision) - events.recordCollisions([]*constraint.ContactConstraint{}) + events.recordCollisions([]constraint.Manifold{}) events.flush() - // Should receive COLLISION_EXIT event - if !capture.hasEventType(COLLISION_EXIT) { - t.Error("Expected COLLISION_EXIT event") + // Should receive EventCollisionExit event + if !capture.hasEventType(EventCollisionExit) { + t.Error("Expected EventCollisionExit event") } } func TestEvents_CollisionStay_SleepingBodies(t *testing.T) { events := NewEvents() capture := &eventCapture{} - events.Subscribe(COLLISION_STAY, capture.capture) + events.Subscribe(EventCollisionStay, capture.capture) // Both bodies sleeping bodyA := createTestBody("A", false, true) @@ -473,18 +471,18 @@ func TestEvents_CollisionStay_SleepingBodies(t *testing.T) { c := createTestConstraint(bodyA, bodyB) // Frame 1: Enter - events.recordCollisions([]*constraint.ContactConstraint{c}) + events.recordCollisions([]constraint.Manifold{c}) events.flush() capture.reset() // Frame 2: Stay (but both sleeping) - events.recordCollisions([]*constraint.ContactConstraint{c}) + events.recordCollisions([]constraint.Manifold{c}) events.flush() - // Should NOT receive COLLISION_STAY when both bodies are sleeping - if capture.hasEventType(COLLISION_STAY) { - t.Error("COLLISION_STAY should not occur when both bodies are sleeping") + // Should NOT receive EventCollisionStay when both bodies are sleeping + if capture.hasEventType(EventCollisionStay) { + t.Error("EventCollisionStay should not occur when both bodies are sleeping") } } @@ -495,7 +493,7 @@ func TestEvents_CollisionStay_SleepingBodies(t *testing.T) { func TestEvents_OnSleep(t *testing.T) { events := NewEvents() capture := &eventCapture{} - events.Subscribe(ON_SLEEP, capture.capture) + events.Subscribe(EventSleep, capture.capture) // Body starts awake body := createTestBody("A", false, false) @@ -515,9 +513,9 @@ func TestEvents_OnSleep(t *testing.T) { events.processSleepEvents(bodies) events.flush() - // Should receive ON_SLEEP event - if !capture.hasEventType(ON_SLEEP) { - t.Error("Expected ON_SLEEP event") + // Should receive EventSleep event + if !capture.hasEventType(EventSleep) { + t.Error("Expected EventSleep event") } if capture.count() != 1 { @@ -534,7 +532,7 @@ func TestEvents_OnSleep(t *testing.T) { func TestEvents_OnWake(t *testing.T) { events := NewEvents() capture := &eventCapture{} - events.Subscribe(ON_WAKE, capture.capture) + events.Subscribe(EventWake, capture.capture) // Body starts sleeping body := createTestBody("A", false, true) @@ -554,9 +552,9 @@ func TestEvents_OnWake(t *testing.T) { events.processSleepEvents(bodies) events.flush() - // Should receive ON_WAKE event - if !capture.hasEventType(ON_WAKE) { - t.Error("Expected ON_WAKE event") + // Should receive EventWake event + if !capture.hasEventType(EventWake) { + t.Error("Expected EventWake event") } if capture.count() != 1 { @@ -573,7 +571,7 @@ func TestEvents_OnWake(t *testing.T) { func TestEvents_NoSleepEvent_AlreadySleeping(t *testing.T) { events := NewEvents() capture := &eventCapture{} - events.Subscribe(ON_SLEEP, capture.capture) + events.Subscribe(EventSleep, capture.capture) // Body starts sleeping body := createTestBody("A", false, true) @@ -589,16 +587,16 @@ func TestEvents_NoSleepEvent_AlreadySleeping(t *testing.T) { events.processSleepEvents(bodies) events.flush() - // Should NOT receive ON_SLEEP event (already sleeping) - if capture.hasEventType(ON_SLEEP) { - t.Error("Should not receive ON_SLEEP when body is already sleeping") + // Should NOT receive EventSleep event (already sleeping) + if capture.hasEventType(EventSleep) { + t.Error("Should not receive EventSleep when body is already sleeping") } } func TestEvents_NoWakeEvent_AlreadyAwake(t *testing.T) { events := NewEvents() capture := &eventCapture{} - events.Subscribe(ON_WAKE, capture.capture) + events.Subscribe(EventWake, capture.capture) // Body starts awake body := createTestBody("A", false, false) @@ -614,9 +612,9 @@ func TestEvents_NoWakeEvent_AlreadyAwake(t *testing.T) { events.processSleepEvents(bodies) events.flush() - // Should NOT receive ON_WAKE event (already awake) - if capture.hasEventType(ON_WAKE) { - t.Error("Should not receive ON_WAKE when body is already awake") + // Should NOT receive EventWake event (already awake) + if capture.hasEventType(EventWake) { + t.Error("Should not receive EventWake when body is already awake") } } @@ -630,16 +628,16 @@ func TestEvents_CompleteWorkflow(t *testing.T) { captureStay := &eventCapture{} captureExit := &eventCapture{} - events.Subscribe(COLLISION_ENTER, captureEnter.capture) - events.Subscribe(COLLISION_STAY, captureStay.capture) - events.Subscribe(COLLISION_EXIT, captureExit.capture) + events.Subscribe(EventCollisionEnter, captureEnter.capture) + events.Subscribe(EventCollisionStay, captureStay.capture) + events.Subscribe(EventCollisionExit, captureExit.capture) bodyA := createTestBody("A", false, false) bodyB := createTestBody("B", false, false) c := createTestConstraint(bodyA, bodyB) // Frame 1: Enter - events.recordCollisions([]*constraint.ContactConstraint{c}) + events.recordCollisions([]constraint.Manifold{c}) events.flush() if captureEnter.count() != 1 { @@ -654,7 +652,7 @@ func TestEvents_CompleteWorkflow(t *testing.T) { // Frame 2: Stay captureEnter.reset() - events.recordCollisions([]*constraint.ContactConstraint{c}) + events.recordCollisions([]constraint.Manifold{c}) events.flush() if captureEnter.count() != 0 { @@ -669,7 +667,7 @@ func TestEvents_CompleteWorkflow(t *testing.T) { // Frame 3: Exit captureStay.reset() - events.recordCollisions([]*constraint.ContactConstraint{}) + events.recordCollisions([]constraint.Manifold{}) events.flush() if captureEnter.count() != 0 { @@ -688,8 +686,8 @@ func TestEvents_MixedTriggerAndCollision(t *testing.T) { captureTrigger := &eventCapture{} captureCollision := &eventCapture{} - events.Subscribe(TRIGGER_ENTER, captureTrigger.capture) - events.Subscribe(COLLISION_ENTER, captureCollision.capture) + events.Subscribe(EventTriggerEnter, captureTrigger.capture) + events.Subscribe(EventCollisionEnter, captureCollision.capture) // Setup: 1 normal collision + 1 trigger collision bodyA := createTestBody("A", false, false) @@ -700,15 +698,15 @@ func TestEvents_MixedTriggerAndCollision(t *testing.T) { c1 := createTestConstraint(bodyA, bodyB) // Normal c2 := createTestConstraint(bodyC, bodyD) // Trigger - events.recordCollisions([]*constraint.ContactConstraint{c1, c2}) + events.recordCollisions([]constraint.Manifold{c1, c2}) events.flush() // Should receive both event types if captureCollision.count() != 1 { - t.Errorf("Expected 1 COLLISION_ENTER, got %d", captureCollision.count()) + t.Errorf("Expected 1 EventCollisionEnter, got %d", captureCollision.count()) } if captureTrigger.count() != 1 { - t.Errorf("Expected 1 TRIGGER_ENTER, got %d", captureTrigger.count()) + t.Errorf("Expected 1 EventTriggerEnter, got %d", captureTrigger.count()) } } @@ -717,8 +715,8 @@ func TestEvents_SleepWakeWorkflow(t *testing.T) { captureSleep := &eventCapture{} captureWake := &eventCapture{} - events.Subscribe(ON_SLEEP, captureSleep.capture) - events.Subscribe(ON_WAKE, captureWake.capture) + events.Subscribe(EventSleep, captureSleep.capture) + events.Subscribe(EventWake, captureWake.capture) body := createTestBody("A", false, false) bodies := []*actor.RigidBody{body} @@ -737,7 +735,7 @@ func TestEvents_SleepWakeWorkflow(t *testing.T) { events.flush() if captureSleep.count() != 1 { - t.Errorf("Expected 1 ON_SLEEP event, got %d", captureSleep.count()) + t.Errorf("Expected 1 EventSleep event, got %d", captureSleep.count()) } // Frame 3: Wake up @@ -747,21 +745,21 @@ func TestEvents_SleepWakeWorkflow(t *testing.T) { events.flush() if captureWake.count() != 1 { - t.Errorf("Expected 1 ON_WAKE event, got %d", captureWake.count()) + t.Errorf("Expected 1 EventWake event, got %d", captureWake.count()) } } func TestEvents_Flush_ClearsBuffer(t *testing.T) { events := NewEvents() capture := &eventCapture{} - events.Subscribe(COLLISION_ENTER, capture.capture) + events.Subscribe(EventCollisionEnter, capture.capture) bodyA := createTestBody("A", false, false) bodyB := createTestBody("B", false, false) c := createTestConstraint(bodyA, bodyB) // Add events to buffer - events.recordCollisions([]*constraint.ContactConstraint{c}) + events.recordCollisions([]constraint.Manifold{c}) events.flush() // Buffer should be cleared after flush @@ -796,7 +794,7 @@ func TestEvents_NoListeners(t *testing.T) { c := createTestConstraint(bodyA, bodyB) // Process events without any listeners - events.recordCollisions([]*constraint.ContactConstraint{c}) + events.recordCollisions([]constraint.Manifold{c}) events.flush() // Should succeed without error @@ -807,15 +805,15 @@ func TestEvents_MultipleFrames_EnterExitEnter(t *testing.T) { captureEnter := &eventCapture{} captureExit := &eventCapture{} - events.Subscribe(COLLISION_ENTER, captureEnter.capture) - events.Subscribe(COLLISION_EXIT, captureExit.capture) + events.Subscribe(EventCollisionEnter, captureEnter.capture) + events.Subscribe(EventCollisionExit, captureExit.capture) bodyA := createTestBody("A", false, false) bodyB := createTestBody("B", false, false) c := createTestConstraint(bodyA, bodyB) // Frame 1: Enter - events.recordCollisions([]*constraint.ContactConstraint{c}) + events.recordCollisions([]constraint.Manifold{c}) events.flush() if captureEnter.count() != 1 { @@ -824,7 +822,7 @@ func TestEvents_MultipleFrames_EnterExitEnter(t *testing.T) { // Frame 2: Exit captureEnter.reset() - events.recordCollisions([]*constraint.ContactConstraint{}) + events.recordCollisions([]constraint.Manifold{}) events.flush() if captureExit.count() != 1 { @@ -833,7 +831,7 @@ func TestEvents_MultipleFrames_EnterExitEnter(t *testing.T) { // Frame 3: Enter again captureExit.reset() - events.recordCollisions([]*constraint.ContactConstraint{c}) + events.recordCollisions([]constraint.Manifold{c}) events.flush() if captureEnter.count() != 1 { diff --git a/gjk/distance.go b/gjk/distance.go new file mode 100644 index 0000000..be72afe --- /dev/null +++ b/gjk/distance.go @@ -0,0 +1,223 @@ +package gjk + +import ( + "math" + + "github.com/go-gl/mathgl/mgl64" +) + +const ( + // distanceTolerance: GJK stops when the next support point gets the distance closer by less than this ratio + distanceTolerance = 1e-10 + + // overlapEpsilon: under this squared distance (relative to the size of the simplex), the shapes overlap + overlapEpsilon = 1e-20 +) + +// DistanceResult: the distance between 2 convex shapes and their closest points (world space). +// Normal goes from A to B. If the shapes overlap, Distance is 0 and Overlap is true +type DistanceResult struct { + Distance float64 + PointA mgl64.Vec3 + PointB mgl64.Vec3 + Normal mgl64.Vec3 + Overlap bool +} + +// distanceSimplex: the vertices of the simplex with their barycentric weights of the closest point to the origin +type distanceSimplex struct { + vertices [4]Vertex + weights [4]float64 + count int +} + +// Distance between 2 convex shapes: GJK (Gilbert, Johnson, Keerthi; Ericson 9.5). +// The simplex is reduced to the feature closest to the origin (Voronoi regions, Ericson 5.1) +func Distance(a, b *Proxy) DistanceResult { + var s distanceSimplex + direction := b.Position.Sub(a.Position) + if direction.LenSqr() == 0 { + direction = mgl64.Vec3{1, 0, 0} + } + s.vertices[0] = SupportProxies(a, b, direction.Mul(-1), 0) + s.weights[0] = 1 + s.count = 1 + + for i := 0; i < maxIterations; i++ { + v := s.closest() + size := s.size() + if v.LenSqr() <= overlapEpsilon*size { + return DistanceResult{Overlap: true} + } + + w := SupportProxies(a, b, v.Mul(-1), 0) + // no progress: v is the closest point of the Minkowski difference + if v.LenSqr()-v.Dot(w.W) <= distanceTolerance*v.LenSqr() || s.has(w.W) { + break + } + s.vertices[s.count] = w + s.count++ + if !s.reduce() { + return DistanceResult{Overlap: true} + } + } + + pointA, pointB := mgl64.Vec3{}, mgl64.Vec3{} + for k := 0; k < s.count; k++ { + pointA = pointA.Add(s.vertices[k].A.Mul(s.weights[k])) + pointB = pointB.Add(s.vertices[k].B.Mul(s.weights[k])) + } + distance := pointB.Sub(pointA).Len() + if distance == 0 { + return DistanceResult{Overlap: true} + } + return DistanceResult{Distance: distance, PointA: pointA, PointB: pointB, Normal: pointB.Sub(pointA).Mul(1 / distance)} +} + +// closest point of the simplex to the origin, from the weights +func (s *distanceSimplex) closest() mgl64.Vec3 { + v := mgl64.Vec3{} + for k := 0; k < s.count; k++ { + v = v.Add(s.vertices[k].W.Mul(s.weights[k])) + } + return v +} + +func (s *distanceSimplex) size() float64 { + size := 0.0 + for k := 0; k < s.count; k++ { + size = math.Max(size, s.vertices[k].W.LenSqr()) + } + return size +} + +func (s *distanceSimplex) has(w mgl64.Vec3) bool { + for k := 0; k < s.count; k++ { + if s.vertices[k].W == w { + return true + } + } + return false +} + +// reduce the simplex to the smallest feature containing its closest point to the origin, with its weights. +// Returns false if the tetrahedron contains the origin +func (s *distanceSimplex) reduce() bool { + switch s.count { + case 2: + s.segment(0, 1) + case 3: + s.triangle(0, 1, 2) + case 4: + return s.tetrahedron() + } + return true +} + +func (s *distanceSimplex) keep(indices ...int) { + var vertices [4]Vertex + for k, i := range indices { + vertices[k] = s.vertices[i] + } + s.vertices = vertices + s.count = len(indices) +} + +// segment [a, b]: closest point to the origin +func (s *distanceSimplex) segment(ia, ib int) { + a, b := s.vertices[ia].W, s.vertices[ib].W + ab := b.Sub(a) + t := -a.Dot(ab) + if t <= 0 { + s.keep(ia) + s.weights[0] = 1 + return + } + length := ab.LenSqr() + if t >= length { + s.keep(ib) + s.weights[0] = 1 + return + } + t /= length + s.keep(ia, ib) + s.weights[0], s.weights[1] = 1-t, t +} + +// triangle [a, b, c]: closest point to the origin (Ericson 5.1.5) +func (s *distanceSimplex) triangle(ia, ib, ic int) { + a, b, c := s.vertices[ia].W, s.vertices[ib].W, s.vertices[ic].W + ab, ac, ap := b.Sub(a), c.Sub(a), a.Mul(-1) + d1, d2 := ab.Dot(ap), ac.Dot(ap) + if d1 <= 0 && d2 <= 0 { + s.keep(ia) + s.weights[0] = 1 + return + } + bp := b.Mul(-1) + d3, d4 := ab.Dot(bp), ac.Dot(bp) + if d3 >= 0 && d4 <= d3 { + s.keep(ib) + s.weights[0] = 1 + return + } + vc := d1*d4 - d3*d2 + if vc <= 0 && d1 >= 0 && d3 <= 0 { + t := d1 / (d1 - d3) + s.keep(ia, ib) + s.weights[0], s.weights[1] = 1-t, t + return + } + cp := c.Mul(-1) + d5, d6 := ab.Dot(cp), ac.Dot(cp) + if d6 >= 0 && d5 <= d6 { + s.keep(ic) + s.weights[0] = 1 + return + } + vb := d5*d2 - d1*d6 + if vb <= 0 && d2 >= 0 && d6 <= 0 { + t := d2 / (d2 - d6) + s.keep(ia, ic) + s.weights[0], s.weights[1] = 1-t, t + return + } + va := d3*d6 - d5*d4 + if va <= 0 && d4-d3 >= 0 && d5-d6 >= 0 { + t := (d4 - d3) / ((d4 - d3) + (d5 - d6)) + s.keep(ib, ic) + s.weights[0], s.weights[1] = 1-t, t + return + } + denominator := 1 / (va + vb + vc) + v, w := vb*denominator, vc*denominator + s.keep(ia, ib, ic) + s.weights[0], s.weights[1], s.weights[2] = 1-v-w, v, w +} + +// tetrahedron: the closest point is on the faces the origin is in front of (Ericson 5.1.6). +// Returns false if the origin is inside +func (s *distanceSimplex) tetrahedron() bool { + faces := [4][4]int{{0, 1, 2, 3}, {0, 2, 3, 1}, {0, 3, 1, 2}, {1, 3, 2, 0}} + best, bestDistance := *s, math.Inf(1) + inside := true + for _, face := range faces { + a, b, c, d := s.vertices[face[0]].W, s.vertices[face[1]].W, s.vertices[face[2]].W, s.vertices[face[3]].W + n := b.Sub(a).Cross(c.Sub(a)) + // the origin and the opposite vertex on both sides of the face + if a.Mul(-1).Dot(n)*d.Sub(a).Dot(n) >= 0 { + continue + } + inside = false + candidate := *s + candidate.triangle(face[0], face[1], face[2]) + if distance := candidate.closest().LenSqr(); distance < bestDistance { + best, bestDistance = candidate, distance + } + } + if inside { + return false + } + *s = best + return true +} diff --git a/gjk/distance_test.go b/gjk/distance_test.go new file mode 100644 index 0000000..463f6f5 --- /dev/null +++ b/gjk/distance_test.go @@ -0,0 +1,92 @@ +package gjk + +import ( + "math" + "math/rand" + "testing" + + "github.com/akmonengine/feather/actor" + "github.com/go-gl/mathgl/mgl64" +) + +func randomRotation(r *rand.Rand) mgl64.Quat { + return mgl64.QuatRotate(r.Float64()*2*math.Pi, mgl64.Vec3{r.Float64() - 0.5, r.Float64() - 0.5, r.Float64() - 0.5}.Normalize()) +} + +func distanceOf(a, b *actor.RigidBody) DistanceResult { + proxyA, proxyB := NewProxy(a), NewProxy(b) + return Distance(&proxyA, &proxyB) +} + +// A box against a sphere: the exact distance is the distance from the center of the sphere to the box, minus its radius +func TestDistanceBoxSphere(t *testing.T) { + r := rand.New(rand.NewSource(1)) + halfExtents := mgl64.Vec3{0.5, 0.3, 0.8} + for i := 0; i < 1000; i++ { + rotation := randomRotation(r) + box := actor.NewRigidBody(actor.Transform{Position: mgl64.Vec3{0.2, -0.1, 0.3}, Rotation: rotation}, &actor.Box{HalfExtents: halfExtents}, actor.BodyTypeDynamic, 1) + center := mgl64.Vec3{r.Float64()*4 - 2, r.Float64()*4 - 2, r.Float64()*4 - 2} + sphere := actor.NewRigidBody(actor.Transform{Position: center, Rotation: mgl64.QuatIdent()}, &actor.Sphere{Radius: 0.25}, actor.BodyTypeDynamic, 1) + + local := box.Transform.ToLocal(center) + clamped := mgl64.Vec3{} + for k := 0; k < 3; k++ { + clamped[k] = math.Max(-halfExtents[k], math.Min(halfExtents[k], local[k])) + } + want := local.Sub(clamped).Len() - 0.25 + + result := distanceOf(box, sphere) + if want <= 0 { + if !result.Overlap && want < -1e-6 { + t.Fatalf("case %d: overlap %.4f not found (distance %.4f)", i, want, result.Distance) + } + continue + } + if result.Overlap || math.Abs(result.Distance-want) > 1e-6 { + t.Fatalf("case %d: distance %.8f, want %.8f (overlap %v)", i, result.Distance, want, result.Overlap) + } + // the closest points: on the box and on the sphere, along the normal + if math.Abs(result.PointB.Sub(result.PointA).Len()-want) > 1e-6 || math.Abs(result.PointB.Sub(center).Len()-0.25) > 1e-6 { + t.Fatalf("case %d: wrong closest points", i) + } + } +} + +// Capsules: the exact distance is the distance between their segments, minus both radii (sampled) +func TestDistanceCapsules(t *testing.T) { + r := rand.New(rand.NewSource(2)) + for i := 0; i < 300; i++ { + a := actor.NewRigidBody(actor.Transform{Position: mgl64.Vec3{}, Rotation: randomRotation(r)}, &actor.Capsule{HalfHeight: 0.5, Radius: 0.1}, actor.BodyTypeDynamic, 1) + b := actor.NewRigidBody(actor.Transform{Position: mgl64.Vec3{r.Float64()*3 - 1.5, r.Float64()*3 - 1.5, r.Float64()*3 - 1.5}, Rotation: randomRotation(r)}, &actor.Capsule{HalfHeight: 0.3, Radius: 0.2}, actor.BodyTypeDynamic, 1) + a0, a1 := a.Shape.(*actor.Capsule).Segment(a.Transform) + b0, b1 := b.Shape.(*actor.Capsule).Segment(b.Transform) + best := math.Inf(1) + const samples = 400 + for p := 0; p <= samples; p++ { + pa := a0.Add(a1.Sub(a0).Mul(float64(p) / samples)) + // closest point of the segment b + s := math.Max(0, math.Min(1, pa.Sub(b0).Dot(b1.Sub(b0))/b1.Sub(b0).LenSqr())) + best = math.Min(best, pa.Sub(b0.Add(b1.Sub(b0).Mul(s))).Len()) + } + want := best - 0.3 + result := distanceOf(a, b) + if want < -1e-3 { + if !result.Overlap { + t.Fatalf("case %d: overlap not found", i) + } + continue + } + if want > 1e-3 && (result.Overlap || math.Abs(result.Distance-want) > 1e-4) { + t.Fatalf("case %d: distance %.6f, want %.6f", i, result.Distance, want) + } + } +} + +func TestDistanceSpheres(t *testing.T) { + a := actor.NewRigidBody(actor.Transform{Position: mgl64.Vec3{1, 2, 3}, Rotation: mgl64.QuatIdent()}, &actor.Sphere{Radius: 0.5}, actor.BodyTypeDynamic, 1) + b := actor.NewRigidBody(actor.Transform{Position: mgl64.Vec3{4, 6, 3}, Rotation: mgl64.QuatIdent()}, &actor.Sphere{Radius: 1}, actor.BodyTypeDynamic, 1) + result := distanceOf(a, b) + if math.Abs(result.Distance-3.5) > 1e-9 || result.Normal.Sub(mgl64.Vec3{0.6, 0.8, 0}).Len() > 1e-9 { + t.Errorf("distance %v, normal %v", result.Distance, result.Normal) + } +} diff --git a/gjk/gjk.go b/gjk/gjk.go index d07d0d5..e34214c 100644 --- a/gjk/gjk.go +++ b/gjk/gjk.go @@ -4,6 +4,8 @@ // contains the origin. The algorithm builds a simplex incrementally, converging toward // the origin in typically 3-6 iterations. // +// Each vertex of the simplex keeps its support points on A and B, for the witness points of EPA. +// // For detailed algorithm explanation with pseudocode and visual examples, see: // ALGORITHMS.md - "GJK Algorithm" section // @@ -20,11 +22,30 @@ import ( "github.com/go-gl/mathgl/mgl64" ) +const ( + // maxIterations: safety limit to prevent infinite loops + maxIterations = 64 + + // degenerateEpsilon: the direction is null (the origin is on the simplex), relative to the size of the simplex + degenerateEpsilon = 1e-24 + + // hullEpsilon: the new point is on the same point/line/plane as the simplex, relative to its size + hullEpsilon = 1e-18 +) + +// Vertex of the Minkowski difference, with its support points: W = A - B +type Vertex struct { + W mgl64.Vec3 + A mgl64.Vec3 + B mgl64.Vec3 +} + // Simplex represents a set of 1-4 points in the Minkowski difference space. -// The simplex evolves during GJK iterations, always containing the most recent support points. -// Size progression: 1 point → 2 points (line) → 3 points (triangle) → 4 points (tetrahedron) +// The last point is always the most recent type Simplex struct { Points [4]mgl64.Vec3 + A [4]mgl64.Vec3 + B [4]mgl64.Vec3 Count int } @@ -32,366 +53,335 @@ func (s *Simplex) Reset() { s.Count = 0 } +func (s *Simplex) Vertex(i int) Vertex { + return Vertex{W: s.Points[i], A: s.A[i], B: s.B[i]} +} + +func (s *Simplex) set(vertices ...Vertex) { + for i, v := range vertices { + s.Points[i], s.A[i], s.B[i] = v.W, v.A, v.B + } + s.Count = len(vertices) +} + var SimplexPool = sync.Pool{ New: func() interface{} { return &Simplex{} }, } -// MinkowskiSupport computes a support point in the Minkowski difference (A - B). -// -// The Minkowski difference A - B is the set of all vectors (a - b) where a ∈ A and b ∈ B. -// For collision detection, we only need the extreme points (support points) in any direction. -// -// Parameters: -// - a, b: The two rigid bodies to test -// - direction: The direction to find the furthest point -// -// Returns: -// -// Support point: furthestPoint(A, direction) - furthestPoint(B, -direction) -// -// This is the fundamental query that makes GJK work for any convex shape - shapes only -// need to implement a Support() function, not expose their full geometry. +// Supporter is a convex shape, by its support function: the farthest point in a direction, in local space +type Supporter interface { + Support(direction mgl64.Vec3) mgl64.Vec3 +} + +// Proxy is a body prepared for the support queries: its rotation as matrices, computed once per pair +// instead of rotating each direction and each point with the quaternion +type Proxy struct { + Position mgl64.Vec3 + Rotation mgl64.Mat3 // local to world + Inverse mgl64.Mat3 // world to local + Shape Supporter +} + +func NewProxy(body *actor.RigidBody) Proxy { + return NewProxyAt(body.Transform, body.Shape) +} + +// ========== CORES ========== +// The core of a rounded shape is the shape without its radius (the collision margin of Bullet, the convex radius of +// Jolt, the radius of the rounded polygons of Box2D v3): a point for a +// sphere, a segment for a capsule. The distance between the cores is the distance between the shapes minus the radii, +// and GJK finds it exactly against a polytope, where EPA on the rounded shape would tessellate it (13 iterations for a +// sphere against a box) + +type sphereCore actor.Sphere + +func (*sphereCore) Support(mgl64.Vec3) mgl64.Vec3 { return mgl64.Vec3{} } + +type capsuleCore actor.Capsule + +func (c *capsuleCore) Support(direction mgl64.Vec3) mgl64.Vec3 { + if direction.Y() < 0 { + return mgl64.Vec3{0, -c.HalfHeight, 0} + } + return mgl64.Vec3{0, c.HalfHeight, 0} +} + +// NewCoreProxy: the core of the body and its radius. The other shapes are their own core, with a radius of 0 +func NewCoreProxy(body *actor.RigidBody) (Proxy, float64) { + switch shape := body.Shape.(type) { + case *actor.Sphere: + return NewProxyAt(body.Transform, (*sphereCore)(shape)), shape.Radius + case *actor.Capsule: + return NewProxyAt(body.Transform, (*capsuleCore)(shape)), shape.Radius + } + return NewProxy(body), 0 +} + +// NewProxyAt: the shape at the transform (a body during its motion) +func NewProxyAt(transform actor.Transform, shape Supporter) Proxy { + q := transform.Rotation + w, x, y, z := q.W, q.V[0], q.V[1], q.V[2] + rotation := mgl64.Mat3{ + 1 - 2*(y*y+z*z), 2 * (x*y + w*z), 2 * (x*z - w*y), + 2 * (x*y - w*z), 1 - 2*(x*x+z*z), 2 * (y*z + w*x), + 2 * (x*z + w*y), 2 * (y*z - w*x), 1 - 2*(x*x+y*y), + } + return Proxy{Position: transform.Position, Rotation: rotation, Inverse: rotation.Transpose(), Shape: shape} +} + +// SupportWorld returns the farthest point of the shape in the direction, in world space +func (p *Proxy) SupportWorld(direction mgl64.Vec3) mgl64.Vec3 { + return p.Position.Add(p.Rotation.Mul3x1(p.Shape.Support(p.Inverse.Mul3x1(direction)))) +} + +// MinkowskiSupport computes a support point in the Minkowski difference (A - B): +// furthestPoint(A, direction) - furthestPoint(B, -direction) func MinkowskiSupport(a, b *actor.RigidBody, direction mgl64.Vec3) mgl64.Vec3 { + return Support(a, b, direction, 0).W +} + +// Support computes the support point of (A + margin) - B. +// With a margin, shapes closer than the margin overlap: EPA can compute their distance (margin - depth) +func Support(a, b *actor.RigidBody, direction mgl64.Vec3, margin float64) Vertex { + proxyA, proxyB := NewProxy(a), NewProxy(b) + return SupportProxies(&proxyA, &proxyB, direction, margin) +} + +func SupportProxies(a, b *Proxy, direction mgl64.Vec3, margin float64) Vertex { supportA := a.SupportWorld(direction) + if margin > 0 { + if length := direction.Len(); length > 0 { + supportA = supportA.Add(direction.Mul(margin / length)) + } + } supportB := b.SupportWorld(direction.Mul(-1)) - return supportA.Sub(supportB) + return Vertex{W: supportA.Sub(supportB), A: supportA, B: supportB} } -// GJK performs collision detection between two convex rigid bodies. -// -// Algorithm overview: -// 1. Start with initial search direction (toward B from A) -// 2. Get first support point in Minkowski difference -// 3. Iteratively refine simplex toward origin -// 4. If origin is contained → collision -// 5. If can't reach origin → no collision -// -// Typical convergence: 3-6 iterations for most shapes. -// -// Returns: -// - bool: true if collision detected, false otherwise -// -// The simplex is modified in place and contains 1-4 points. For collisions, it's always -// a tetrahedron (4 points) containing the origin, which EPA uses as its initial polytope. +// GJK returns true if both bodies overlap. The simplex is then a tetrahedron containing the origin, for EPA func GJK(a, b *actor.RigidBody, simplex *Simplex) bool { - // Compute initial direction from A to B (optimization over random direction) - // Starting toward the other shape typically reduces iterations - direction := b.Transform.Position.Sub(a.Transform.Position) - if direction.LenSqr() < 1e-8 { - direction = mgl64.Vec3{1, 0, 0} // Fallback if positions are identical - } - - // Get first point of the simplex in the Minkowski difference - simplex.Points[0] = MinkowskiSupport(a, b, direction) - simplex.Count = 1 + return GJKMargin(a, b, 0, simplex) +} - // New direction towards the origin from this first point - direction = simplex.Points[0].Mul(-1) +// GJKMargin returns true if A + margin overlaps B +func GJKMargin(a, b *actor.RigidBody, margin float64, simplex *Simplex) bool { + proxyA, proxyB := NewProxy(a), NewProxy(b) + return GJKProxies(&proxyA, &proxyB, margin, simplex) +} - // If first support point is at/near origin, shapes are touching - if direction.LenSqr() < 1e-16 { - return true // Collision detected (rare: shapes exactly touching at point) +// GJKProxies is GJKMargin for prepared bodies +func GJKProxies(a, b *Proxy, margin float64, simplex *Simplex) bool { + direction := b.Position.Sub(a.Position) + if direction.LenSqr() == 0 { + direction = mgl64.Vec3{1, 0, 0} } - maxIterations := 32 // Safety limit to prevent infinite loops + simplex.set(SupportProxies(a, b, direction, margin)) + direction = simplex.Points[0].Mul(-1) + for i := 0; i < maxIterations; i++ { - // Find a new support point in the direction towards the origin - newPoint := MinkowskiSupport(a, b, direction) - - // Early exit test: If the new point doesn't pass the origin in the search direction, - // the origin cannot be reached, therefore no collision. - // This is the key optimization that makes GJK fast - we prove separation - // without building the full Minkowski difference. - if newPoint.Dot(direction) <= 0 { - return false // No collision detected - shapes are separated + if direction.LenSqr() <= degenerateEpsilon*simplexSize(simplex) { + // The origin is on the simplex: the shapes are touching + fillTetrahedron(a, b, margin, simplex) + return true + } + + v := SupportProxies(a, b, direction, margin) + if v.W.Dot(direction) <= 0 { + return false } - // Add the new point to the simplex - simplex.Points[simplex.Count] = newPoint + simplex.Points[simplex.Count], simplex.A[simplex.Count], simplex.B[simplex.Count] = v.W, v.A, v.B simplex.Count++ - // Check if the simplex contains the origin - // This function also updates the simplex and direction for the next iteration - // by reducing the simplex to its closest feature to the origin if containsOrigin(simplex, &direction) { - return true // Collision detected - origin is inside simplex + return true } } - // Failed to converge after maxIterations (very rare, may indicate numerical issues) - // In practice this almost never happens for valid convex shapes return false } -// containsOrigin tests if the simplex contains the origin and refines the simplex. -// -// This is the heart of GJK - it determines which feature of the simplex (point, edge, face) -// is closest to the origin, keeps only the relevant points, and updates the search direction. -// -// Behavior by simplex dimension: -// - 2 points (line): Test Voronoi regions, reduce to closest point or keep edge -// - 3 points (triangle): Test Voronoi regions, reduce to closest edge or keep face -// - 4 points (tetrahedron): Test if origin is inside; if not, reduce to closest face -// -// Returns: -// - true: Origin is contained (only possible for tetrahedron) → collision! -// - false: Origin is outside, simplex and direction updated for next iteration +// containsOrigin reduces the simplex to its closest feature to the origin, and updates the direction. +// Returns true if the tetrahedron contains the origin func containsOrigin(simplex *Simplex, direction *mgl64.Vec3) bool { switch simplex.Count { case 2: - return line(simplex, direction) + line(simplex, direction) case 3: - return triangle(simplex, direction) + triangle(simplex, direction) case 4: return tetrahedron(simplex, direction) } return false } -// line handles the line simplex case (2 points: A and B). -// -// Tests which Voronoi region contains the origin: -// - Region A: Origin is closest to point A alone -// - Region B: Origin is closest to point B alone -// - Region AB: Origin is closest to the line segment AB -// -// Returns true only if origin is on the line segment (not just the infinite line). -// Updates direction to point toward origin from the closest feature. -func line(simplex *Simplex, direction *mgl64.Vec3) bool { - a := simplex.Points[1] - b := simplex.Points[0] - ab := b.Sub(a) - ao := a.Mul(-1) - - // Handle degenerate case: identical points - if ab.LenSqr() < 1e-8 { - if ao.LenSqr() < 1e-8 { - return true // origin is at the point - } - // Origin is not at the point, but simplex is degenerate - simplex.Points[0] = a - simplex.Count = 1 - *direction = ao - return false - } - - // Check if origin is in Voronoi region A (behind A, opposite direction from B) - // If ab.Dot(ao) <= 0, the origin is closest to point A alone - if ab.Dot(ao) <= 0 { - // Reduce simplex to point A - simplex.Points[0] = a - simplex.Count = 1 - *direction = ao - return false - } +// line: segment [b, a], a is the most recent point +func line(simplex *Simplex, direction *mgl64.Vec3) { + a, b := simplex.Vertex(1), simplex.Vertex(0) + ab := b.W.Sub(a.W) + ao := a.W.Mul(-1) - // Check if origin is in Voronoi region B (behind B, opposite direction from A) - bo := b.Mul(-1) - if ab.Dot(bo) >= 0 { - // Reduce simplex to point B - simplex.Points[0] = b - simplex.Count = 1 - *direction = bo - return false - } - - // Origin is in Voronoi region AB (between A and B direction-wise) - abPerp := ab.Cross(ao).Cross(ab) - if abPerp.LenSqr() < 1e-8 { - // Origin is on the line, but check if it's on the segment [A, B] - abLengthSqr := ab.LenSqr() - t := ao.Dot(ab) / abLengthSqr - - // Check if origin is on the segment [A, B] with tolerance - // Using 1e-6 tolerance for segment inclusion - if t >= -1e-6 && t <= 1.0+1e-6 { - return true // Collision - origin is on the segment - } - - // Origin is on the infinite line but not on the segment - // Find closest point on segment and continue - if t < 0 { - // Closest to A - simplex.Points[0] = a - simplex.Count = 1 - *direction = ao - } else { - // Closest to B - simplex.Points[0] = b - simplex.Count = 1 - *direction = bo - } - return false + if ab.Dot(ao) > 0 { + *direction = ab.Cross(ao).Cross(ab) + return } - - *direction = abPerp - return false + simplex.set(a) + *direction = ao } -// triangle handles the triangle simplex case (3 points: A, B, C). -// -// Tests which Voronoi region contains the origin: -// - Region A: Origin closest to point A alone -// - Region AB: Origin closest to edge AB -// - Region AC: Origin closest to edge AC -// - Region ABC (above): Origin above triangle plane -// - Region ABC (below): Origin below triangle plane -// -// Degenerate case: If points are collinear (flat triangle), treats as line instead. -// -// Returns false (a triangle cannot contain origin in 3D, we need tetrahedron). -// Reduces simplex to closest feature and updates direction. -func triangle(simplex *Simplex, direction *mgl64.Vec3) bool { - a := simplex.Points[2] // Most recent point - b := simplex.Points[1] - c := simplex.Points[0] - - ab := b.Sub(a) - ac := c.Sub(a) - ao := a.Mul(-1) - - abc := ab.Cross(ac) // Triangle normal - - // Check for degenerate triangle (colinear points) - // If normal is nearly zero, points are on a line - if abc.LenSqr() < 1e-10 { - // Treat as line instead of triangle - // Keep A and B (discard C which is furthest from recent history) - simplex.Points[0] = b - simplex.Points[1] = a - simplex.Count = 2 - return line(simplex, direction) - } - - // Test the 3 regions around the triangle +// triangle: [c, b, a], a is the most recent point +func triangle(simplex *Simplex, direction *mgl64.Vec3) { + a, b, c := simplex.Vertex(2), simplex.Vertex(1), simplex.Vertex(0) + ab := b.W.Sub(a.W) + ac := c.W.Sub(a.W) + ao := a.W.Mul(-1) + abc := ab.Cross(ac) - // Region AB (edge) - abPerp := ab.Cross(abc) - if abPerp.Dot(ao) > 0 { - simplex.Points[0] = b - simplex.Points[1] = a - simplex.Count = 2 - *direction = ab.Cross(ao).Cross(ab) - return false + if abc.Cross(ac).Dot(ao) > 0 { + if ac.Dot(ao) > 0 { + simplex.set(c, a) + *direction = ac.Cross(ao).Cross(ac) + return + } + simplex.set(b, a) + line(simplex, direction) + return } - // Region AC (edge) - acPerp := abc.Cross(ac) - if acPerp.Dot(ao) > 0 { - simplex.Points[0] = c - simplex.Points[1] = a - simplex.Count = 2 - *direction = ac.Cross(ao).Cross(ac) - return false + if ab.Cross(abc).Dot(ao) > 0 { + simplex.set(b, a) + line(simplex, direction) + return } - // Origin is above or below the triangle if abc.Dot(ao) > 0 { - // Above the triangle + simplex.set(c, b, a) *direction = abc } else { - // Below, reverse order to maintain correct orientation - simplex.Points[0] = a - simplex.Points[1] = c - simplex.Points[2] = b - simplex.Count = 3 + simplex.set(b, c, a) *direction = abc.Mul(-1) } - - return false // Triangle never contains origin in 3D (we need tetrahedron) } -// tetrahedron handles the tetrahedron simplex case (4 points: A, B, C, D). -// -// This is the only case that can return true (collision detected). -// -// Tests if origin is inside the tetrahedron by checking which side of each face -// the origin lies on: -// - If outside face ABC → reduce to triangle ABC -// - If outside face ACD → reduce to triangle ACD -// - If outside face ADB → reduce to triangle ADB -// - If inside all faces → origin contained, collision! -// -// Face normals must point outward (away from the 4th vertex) to correctly test -// which side of each face the origin is on. -// -// Returns true if origin is inside tetrahedron, false otherwise. +// tetrahedron: [d, c, b, a], a is the most recent point func tetrahedron(simplex *Simplex, direction *mgl64.Vec3) bool { - a := simplex.Points[3] // Most recent point - b := simplex.Points[2] - c := simplex.Points[1] - d := simplex.Points[0] - - ab := b.Sub(a) - ac := c.Sub(a) - ad := d.Sub(a) - ao := a.Mul(-1) + a, b, c, d := simplex.Vertex(3), simplex.Vertex(2), simplex.Vertex(1), simplex.Vertex(0) + ab := b.W.Sub(a.W) + ac := c.W.Sub(a.W) + ad := d.W.Sub(a.W) + ao := a.W.Mul(-1) - // Compute face normals - // IMPORTANT: Normal direction must point AWAY from the 4th vertex - // to correctly represent the "outside" of each face - - // Face ABC (opposite to D) abc := ab.Cross(ac) - // Check if normal points toward D or away from D + acd := ac.Cross(ad) + adb := ad.Cross(ab) + + // The normals must point away from the opposite vertex if abc.Dot(ad) > 0 { - // Normal points toward D, we want it pointing away abc = abc.Mul(-1) } - - // Face ACD (opposite to B) - acd := ac.Cross(ad) if acd.Dot(ab) > 0 { acd = acd.Mul(-1) } - - // Face ADB (opposite to C) - adb := ad.Cross(ab) if adb.Dot(ac) > 0 { adb = adb.Mul(-1) } - // Check for degenerate tetrahedron - if abc.LenSqr() < 1e-10 || acd.LenSqr() < 1e-10 || adb.LenSqr() < 1e-10 { - simplex.Points[0] = c - simplex.Points[1] = b - simplex.Points[2] = a - simplex.Count = 3 - return triangle(simplex, direction) + if abc.Dot(ao) > 0 { + simplex.set(c, b, a) + triangle(simplex, direction) + return false + } + if acd.Dot(ao) > 0 { + simplex.set(d, c, a) + triangle(simplex, direction) + return false + } + if adb.Dot(ao) > 0 { + simplex.set(b, d, a) + triangle(simplex, direction) + return false } - // Now test if origin is outside any face - // If abc.Dot(ao) > 0, origin is on the outside of face ABC + return true +} - // Face ABC - if abc.Dot(ao) > 0 { - simplex.Points[0] = c - simplex.Points[1] = b - simplex.Points[2] = a - simplex.Count = 3 - return triangle(simplex, direction) +// simplexSize returns the largest squared distance of a vertex to the origin +func simplexSize(simplex *Simplex) float64 { + size := 0.0 + for i := 0; i < simplex.Count; i++ { + size = max(size, simplex.Points[i].LenSqr()) } + return size +} - // Face ACD - if acd.Dot(ao) > 0 { - simplex.Points[0] = d - simplex.Points[1] = c - simplex.Points[2] = a - simplex.Count = 3 - return triangle(simplex, direction) +// fillTetrahedron completes the simplex into a tetrahedron when the shapes are only touching, +// so that EPA can start. Returns false if the Minkowski difference is flat +func fillTetrahedron(a, b *Proxy, margin float64, simplex *Simplex) bool { + axes := [6]mgl64.Vec3{{1, 0, 0}, {-1, 0, 0}, {0, 1, 0}, {0, -1, 0}, {0, 0, 1}, {0, 0, -1}} + + var candidates [len(axes)]mgl64.Vec3 + for simplex.Count < 4 { + added := false + count := candidateDirections(simplex, &axes, &candidates) + for _, axis := range candidates[:count] { + v := SupportProxies(a, b, axis, margin) + if isNewVertex(simplex, v.W) { + simplex.Points[simplex.Count], simplex.A[simplex.Count], simplex.B[simplex.Count] = v.W, v.A, v.B + simplex.Count++ + added = true + break + } + } + if !added { + return false + } } + return true +} - // Face ADB - if adb.Dot(ao) > 0 { - simplex.Points[0] = b - simplex.Points[1] = d - simplex.Points[2] = a - simplex.Count = 3 - return triangle(simplex, direction) +// candidateDirections to add a dimension to the simplex, written in out (their count is returned, nothing is +// allocated): the axes for a point, perpendicular directions for a segment, both normals for a triangle +func candidateDirections(simplex *Simplex, axes, out *[6]mgl64.Vec3) int { + switch simplex.Count { + case 1: + *out = *axes + return len(axes) + case 2: + edge := simplex.Points[1].Sub(simplex.Points[0]) + count := 0 + for _, axis := range axes { + if d := edge.Cross(axis); d.LenSqr() > 0 { + out[count] = d + count++ + } + } + return count + default: + n := simplex.Points[1].Sub(simplex.Points[0]).Cross(simplex.Points[2].Sub(simplex.Points[0])) + out[0], out[1] = n, n.Mul(-1) + return 2 } +} - // The origin is inside the tetrahedron - return true +// isNewVertex returns true if w is not on the point/line/plane of the simplex +func isNewVertex(simplex *Simplex, w mgl64.Vec3) bool { + p0 := simplex.Points[0] + scale := max(simplexSize(simplex), w.LenSqr()) + if scale == 0 { + return false + } + switch simplex.Count { + case 1: + return w.Sub(p0).LenSqr() > hullEpsilon*scale + case 2: + edge := simplex.Points[1].Sub(p0) + return edge.Cross(w.Sub(p0)).LenSqr() > hullEpsilon*scale*edge.LenSqr() + default: + n := simplex.Points[1].Sub(p0).Cross(simplex.Points[2].Sub(p0)) + h := n.Dot(w.Sub(p0)) + return h*h > hullEpsilon*scale*n.LenSqr() + } } diff --git a/gjk/gjk_test.go b/gjk/gjk_test.go index 224603c..762d3b9 100644 --- a/gjk/gjk_test.go +++ b/gjk/gjk_test.go @@ -391,116 +391,8 @@ func TestGJK_ExtremePrecision(t *testing.T) { }) } -// Degenerate simplex cases -func TestGJK_DegenerateSimplex(t *testing.T) { - t.Run("colinear points in tetrahedron", func(t *testing.T) { - simplex := Simplex{ - Points: [4]mgl64.Vec3{ - {0, 0, 0}, - {1, 0, 0}, - {2, 0, 0}, - {3, 0, 0}, - }, - Count: 4, - } - direction := mgl64.Vec3{0, 1, 0} - - // This should be reduced to a line and eventually return false - result := tetrahedron(&simplex, &direction) - if result { - t.Error("Expected tetrahedron with colinear points to not contain origin (origin not on any segment)") - } - }) - - t.Run("identical points in simplex", func(t *testing.T) { - simplex := Simplex{ - Points: [4]mgl64.Vec3{ - {0, 0, 0}, - {0, 0, 0}, - {1, 0, 0}, - {0, 1, 0}, - }, - Count: 4, - } - direction := mgl64.Vec3{0, 0, 1} - - // This should be handled gracefully and not cause panic - result := tetrahedron(&simplex, &direction) - if result { - t.Error("Expected tetrahedron with identical points to not contain origin") - } - }) - - t.Run("zero-length edge in line", func(t *testing.T) { - simplex := Simplex{ - Points: [4]mgl64.Vec3{ - {1e-15, 0, 0}, - {1e-15, 1e-15, 0}, - {0, 0, 0}, - {0, 0, 0}, - }, - Count: 2, - } - direction := mgl64.Vec3{0, 1, 0} - - // This should be handled as a degenerate line - result := line(&simplex, &direction) - if !result { - t.Error("Expected degenerate line with near-identical points to contain origin") - } - }) -} - // Tetrahedron face normal orientation -func TestGJK_TetrahedronFaceNormal(t *testing.T) { - t.Run("origin nearly on face (distance < 1e-12)", func(t *testing.T) { - // Move origin extremely close to the face (ABC) but outside - // The face ABC is the triangle with points A, B, C - // The normal should point away from D (0,0,0) - // Origin is at (0,0,0) which is point D, so we need to move the tetrahedron - // so origin is near face ABC but not inside - - // Create a tetrahedron with face ABC at z=1e-12 and origin at (0,0,0) - simplex := Simplex{ - Points: [4]mgl64.Vec3{ - {1, 1, -1e-12}, // D - {1, 0, 1e-12}, // C - {0, 1, 1e-12}, // B - {0, 0, 1e-12}, // A - }, - Count: 4, - } - direction := mgl64.Vec3{0, 0, 1} - - result := tetrahedron(&simplex, &direction) - if result { - t.Error("Expected origin outside tetrahedron near face to not contain origin") - } - }) - - t.Run("face normal with near-zero magnitude", func(t *testing.T) { - // Create a tetrahedron where one face has a normal with near-zero magnitude - // This can happen when three points are nearly colinear - simplex := Simplex{ - Points: [4]mgl64.Vec3{ - {0, 0, 0}, - {1, 0, 0}, - {1, 1e-15, 0}, - {0, 0, 1}, - }, - Count: 4, - } - direction := mgl64.Vec3{0, 0, 1} - - // This should be handled gracefully and not cause division by zero - result := tetrahedron(&simplex, &direction) - if result { - t.Error("Expected tetrahedron with near-zero face normal to not contain origin") - } - }) -} - // GJK with zero-volume shapes func TestGJK_ZeroVolumeShapes(t *testing.T) { t.Run("zero-radius sphere (point)", func(t *testing.T) { @@ -552,267 +444,6 @@ func Inf() float64 { return math.Inf(1) } -// Simplex helper function tests -func TestLine(t *testing.T) { - t.Run("origin near line (normal case)", func(t *testing.T) { - // Normal case: origin is near the line but not on it - simplex := Simplex{ - Points: [4]mgl64.Vec3{ - {-1, 1, 0}, // B (old point) - {1, 1, 0}, // A (most recent point) - {0, 0, 0}, - {0, 0, 0}, - }, - Count: 2, - } - direction := mgl64.Vec3{0, 1, 0} - - result := line(&simplex, &direction) - - if result { - t.Error("Line not passing through origin should not detect collision") - } - // Origin is in direction of B, so both points should be kept - if simplex.Count != 2 { - t.Errorf("Expected simplex length 2, got %d", simplex.Count) - } - }) - - t.Run("origin ON line segment (degenerate)", func(t *testing.T) { - // Special case: origin is exactly on the line segment AB - // This is a degenerate case that indicates collision - simplex := Simplex{ - Points: [4]mgl64.Vec3{ - {-1, 0, 0}, // B (old point) - {1, 0, 0}, // A (most recent point) - {0, 0, 0}, - {0, 0, 0}, - }, - Count: 2, - } - direction := mgl64.Vec3{0, 1, 0} - - result := line(&simplex, &direction) - - if !result { - t.Error("Line passing through origin should detect collision") - } - }) - - t.Run("origin on line segment", func(t *testing.T) { - // Test that origin is detected as on segment when t is between 0 and 1 - simplex := Simplex{ - Points: [4]mgl64.Vec3{ - {2, 0, 0}, // B - {0, 0, 0}, // A - {0, 0, 0}, - {0, 0, 0}, - }, - Count: 2, - } - direction := mgl64.Vec3{0, 1, 0} - - // Origin is at (0,0,0) which is exactly point A (t=0) - // Correctly identifies this as Voronoi region A - // and reduces to point A, returning false (no collision in this case) - result := line(&simplex, &direction) - if result { - t.Error("Expected no collision when origin is exactly at point A (Voronoi region A)") - } - }) - - t.Run("origin on line segment middle", func(t *testing.T) { - // Test that origin is detected as on segment when t is between 0 and 1 - simplex := Simplex{ - Points: [4]mgl64.Vec3{ - {2, 0, 0}, // B - {0, 0, 0}, // A - {0, 0, 0}, - {0, 0, 0}, - }, - Count: 2, - } - direction := mgl64.Vec3{0, 1, 0} - - // Move simplex so origin is in the middle of segment AB - simplex.Points[1] = mgl64.Vec3{1, 0, 0} // A - simplex.Points[0] = mgl64.Vec3{-1, 0, 0} // B - // Origin (0,0,0) is exactly in the middle (t=0.5) - result := line(&simplex, &direction) - if !result { - t.Error("Expected collision when origin is in the middle of segment (t=0.5)") - } - }) - - t.Run("origin on infinite line but not on segment", func(t *testing.T) { - // Test that origin on infinite line but outside segment returns false - simplex := Simplex{ - Points: [4]mgl64.Vec3{ - {1, 0, 0}, // B - {2, 0, 0}, // A - {0, 0, 0}, - {0, 0, 0}, - }, - Count: 2, - } - direction := mgl64.Vec3{0, 1, 0} - - // Origin (0,0,0) is on the infinite line but not on segment [A,B] - // Segment is from (2,0,0) to (1,0,0), origin is at (0,0,0) which is outside - result := line(&simplex, &direction) - if result { - t.Error("Expected no collision when origin is on infinite line but not on segment") - } - }) - - t.Run("origin behind point A", func(t *testing.T) { - simplex := Simplex{ - Points: [4]mgl64.Vec3{ - {3, 0, 0}, // B - {1, 0, 0}, // A - {0, 0, 0}, - {0, 0, 0}, - }, - Count: 2, - } - direction := mgl64.Vec3{-1, 0, 0} - result := line(&simplex, &direction) - if result { - t.Error("Line should not contain origin") - } - // When origin is behind point A, simplex should be reduced to point A only - if simplex.Count != 1 { - t.Errorf("Expected simplex to be reduced to 1 point, got %d", simplex.Count) - } - // Direction should point from A toward origin - if direction.Dot(mgl64.Vec3{-1, 0, 0}) != 1.0 { - t.Errorf("Expected direction to be (-1,0,0), got %v", direction) - } - }) -} - -func TestTriangle(t *testing.T) { - t.Run("origin above triangle", func(t *testing.T) { - simplex := Simplex{ - Points: [4]mgl64.Vec3{ - {1, 0, 0}, // C (oldest) - {0, 1, 0}, // B - {0, 0, 0.5}, // A (most recent) - {0, 0, 0}, - }, - Count: 3, - } - direction := mgl64.Vec3{0, 0, 1} - - result := triangle(&simplex, &direction) - - if result { - t.Error("Triangle should never contain origin in 3D") - } - // Simplex should remain a triangle (3 points) - if simplex.Count != 3 { - t.Errorf("Expected simplex to remain triangle (3 points), got %d", simplex.Count) - } - }) - - t.Run("origin in AB edge region", func(t *testing.T) { - // Create a proper triangle (not degenerate) - // Triangle vertices: A=(2,0,0), B=(0,2,0), C=(3,3,0) - // Origin should be in the Voronoi region of edge AB - simplex := Simplex{ - Points: [4]mgl64.Vec3{ - {3, 3, 0}, // C (oldest) - {0, 2, 0}, // B - {2, 0, 0}, // A (most recent) - {0, 0, 0}, - }, - Count: 3, - } - direction := mgl64.Vec3{0, 0, 1} - - result := triangle(&simplex, &direction) - - if result { - t.Error("Triangle should never contain origin in 3D") - } - // Origin is in AB region, so simplex should be reduced to edge AB (2 points) - if simplex.Count != 2 { - t.Errorf("Expected simplex reduced to edge (2 points), got %d", simplex.Count) - } - }) - - t.Run("origin in AC edge region", func(t *testing.T) { - // Create a proper triangle where origin is in AC edge region - // Triangle vertices: A=(2,0,0), B=(3,3,0), C=(0,2,0) - simplex := Simplex{ - Points: [4]mgl64.Vec3{ - {0, 2, 0}, // C (oldest) - {3, 3, 0}, // B - {2, 0, 0}, // A (most recent) - {0, 0, 0}, - }, - Count: 3, - } - direction := mgl64.Vec3{0, 0, 1} - - result := triangle(&simplex, &direction) - - if result { - t.Error("Triangle should never contain origin in 3D") - } - // Origin is in AC region, so simplex should be reduced to edge AC (2 points) - if simplex.Count != 2 { - t.Errorf("Expected simplex reduced to edge (2 points), got %d", simplex.Count) - } - }) -} - -func TestTetrahedron(t *testing.T) { - t.Run("origin inside tetrahedron", func(t *testing.T) { - // Create a tetrahedron that actually contains the origin - // Using a regular tetrahedron centered near origin - simplex := Simplex{ - Points: [4]mgl64.Vec3{ - {-1, -1, -1}, // D (oldest) - {1, 1, -1}, // C - {1, -1, 1}, // B - {-1, 1, 1}, // A (most recent) - }, - Count: 4, - } - direction := mgl64.Vec3{0, 0, 1} - - result := tetrahedron(&simplex, &direction) - - if !result { - t.Error("Expected tetrahedron to contain origin") - } - }) - - t.Run("origin outside ABC face", func(t *testing.T) { - // Tetrahedron with origin clearly outside - simplex := Simplex{ - Points: [4]mgl64.Vec3{ - {5, 5, 5}, // D (oldest) - {6, 5, 5}, // C - {5, 6, 5}, // B - {5, 5, 6}, // A (most recent) - }, - Count: 4, - } - direction := mgl64.Vec3{0, 0, 1} - - result := tetrahedron(&simplex, &direction) - - if result { - t.Error("Expected origin to be outside tetrahedron") - } - if simplex.Count > 3 { - t.Errorf("Expected simplex reduced to triangle (3 points), got %d", simplex.Count) - } - }) -} - // Benchmark tests func BenchmarkGJK_Spheres_Intersecting(b *testing.B) { @@ -858,3 +489,79 @@ func BenchmarkGJK_MixedShapes(b *testing.B) { GJK(box, sphere, simplex) } } + +// With a margin, body A is inflated: shapes closer than the margin overlap, farther ones +// do not. +func TestGJKMargin(t *testing.T) { + a := createBoxBody(mgl64.Vec3{0, 0, 0}, mgl64.Vec3{1, 1, 1}) + b := createBoxBody(mgl64.Vec3{0, 2.01, 0}, mgl64.Vec3{1, 1, 1}) + simplex := &Simplex{} + if GJKMargin(a, b, 0, simplex) { + t.Error("boxes 1 cm apart overlap without margin") + } + simplex.Reset() + if !GJKMargin(a, b, 0.02, simplex) { + t.Error("boxes 1 cm apart do not overlap with a 2 cm margin") + } + if simplex.Count != 4 { + t.Errorf("simplex has %d points, want a tetrahedron for EPA", simplex.Count) + } + simplex.Reset() + if GJKMargin(a, b, 0.005, simplex) { + t.Error("boxes 1 cm apart overlap with a 5 mm margin") + } +} + +// Every vertex of the final simplex remembers its support points: W = A - B, A on body A +// (inflated), B on body B. +func TestGJKSimplexKeepsSupports(t *testing.T) { + a := createSphereBody(mgl64.Vec3{0, 0, 0}, 1) + b := createBoxBody(mgl64.Vec3{0.5, 1.2, 0.3}, mgl64.Vec3{0.5, 0.5, 0.5}) + simplex := &Simplex{} + if !GJK(a, b, simplex) { + t.Fatal("no overlap") + } + for i := 0; i < simplex.Count; i++ { + v := simplex.Vertex(i) + if v.W.Sub(v.A.Sub(v.B)).Len() > 1e-12 { + t.Errorf("vertex %d: W %v != A - B %v", i, v.W, v.A.Sub(v.B)) + } + if math.Abs(v.A.Len()-1) > 1e-9 { + t.Errorf("vertex %d: A %v is not on the sphere", i, v.A) + } + } +} + +// Shapes exactly touching (the origin on the Minkowski boundary) are an overlap, and the +// simplex is grown into a tetrahedron so EPA can start. +func TestGJKTouchingFillsTetrahedron(t *testing.T) { + a := createBoxBody(mgl64.Vec3{0, 0, 0}, mgl64.Vec3{1, 1, 1}) + b := createBoxBody(mgl64.Vec3{2, 0, 0}, mgl64.Vec3{1, 1, 1}) + simplex := &Simplex{} + if !GJK(a, b, simplex) { + t.Fatal("touching boxes not reported") + } + if simplex.Count != 4 { + t.Errorf("simplex has %d points, want 4", simplex.Count) + } + // the step of the world doesn't allocate: filling the tetrahedron neither + proxyA, proxyB := NewProxy(a), NewProxy(b) + if allocs := testing.AllocsPerRun(10, func() { + simplex.Reset() + GJKProxies(&proxyA, &proxyB, 0, simplex) + }); allocs > 0 { + t.Errorf("%.1f allocations to fill the tetrahedron, want 0", allocs) + } +} + +// A direction of zero length gives a finite support point (the sphere used to return NaN). +func TestSupportZeroDirection(t *testing.T) { + a := createSphereBody(mgl64.Vec3{0, 0, 0}, 1) + b := createSphereBody(mgl64.Vec3{0, 0, 0}, 1) + v := Support(a, b, mgl64.Vec3{}, 0.1) + for _, x := range v.W { + if math.IsNaN(x) || math.IsInf(x, 0) { + t.Fatalf("support %v is not finite", v) + } + } +} diff --git a/graph.go b/graph.go new file mode 100644 index 0000000..ab10a37 --- /dev/null +++ b/graph.go @@ -0,0 +1,118 @@ +package feather + +// Graph coloring, as in Box2D v3 (constraint_graph.c): the constraints of a color don't share any dynamic body, +// so a color can be solved in parallel. The static bodies don't count, they never move. +// The contacts (in the order of the pairs) then the joints are colored, and solved color by color: the result is the +// same whatever the number of workers. The joints of the articulations (solved together, before the colors) keep their +// other rows in the colors. +const ( + // graphColorsCount: the constraints without a free color go to the overflow, solved sequentially + graphColorsCount = 16 + + // constraintsChunk: the workers take the constraints of a color by chunks of this size. + // A color smaller than a chunk is solved by a single worker. + constraintsChunk = 32 + + // bodiesChunk: same for the bodies integration + bodiesChunk = 64 + + // minParallelBodies: under this count of bodies, the step runs on a single goroutine + minParallelBodies = 256 + + // pairsPerChunk: the workers take the pairs of the narrow phase by chunks of this size + pairsPerChunk = 16 +) + +type graphColor struct { + items []int // the items of the color: a contact constraint, or a joint after the constraints + bodies []uint64 // bitset of the dynamic bodies used by the color +} + +// graphItem: the dynamic bodies of a constraint to color (-1 for a static body) +type graphItem struct { + indexA, indexB int +} + +type constraintGraph struct { + colors [graphColorsCount]graphColor + overflow []int +} + +// color assigns each item to the first color where both of its dynamic bodies are free. An item with a static body +// never takes the color 0 (as in Box2D v3): it is solved after the contacts between dynamic bodies, the ground has the +// last word. Solved first, a body pressed by a heavier one would leave the step moving into the ground +func (g *constraintGraph) color(items []graphItem, bodiesCount int) { + words := (bodiesCount + 63) / 64 + for i := range g.colors { + color := &g.colors[i] + color.items = color.items[:0] + if cap(color.bodies) < words { + color.bodies = make([]uint64, words) + } + color.bodies = color.bodies[:words] + clear(color.bodies) + } + g.overflow = g.overflow[:0] + + for i := range items { + indexA, indexB := items[i].indexA, items[i].indexB + colored := false + first := 0 + if indexA < 0 || indexB < 0 { + first = 1 + } + for k := first; k < len(g.colors); k++ { + color := &g.colors[k] + if isUsed(color.bodies, indexA) || isUsed(color.bodies, indexB) { + continue + } + use(color.bodies, indexA) + use(color.bodies, indexB) + color.items = append(color.items, i) + colored = true + break + } + if !colored { + g.overflow = append(g.overflow, i) + } + } +} + +func isUsed(bits []uint64, index int) bool { + return index >= 0 && bits[index/64]&(1<<(index%64)) != 0 +} + +func use(bits []uint64, index int) { + if index >= 0 { + bits[index/64] |= 1 << (index % 64) + } +} + +// solveConstraints: the overflow first (sequential), then each color (parallel). The joints go through the joint +// stage (none for the restitution) +func (s *solver) solveConstraints(solve func(c *contactConstraint), solveJoint func(j Joint)) { + s.stage, s.jointStage = solve, solveJoint + for _, i := range s.graph.overflow { + s.solveItem(i) + } + for k := range s.graph.colors { + s.color = s.graph.colors[k].items + s.pool.run(len(s.color), constraintsChunk, s.jobs.color) + } +} + +// solveItem: a contact constraint, or a joint after the constraints +func (s *solver) solveItem(item int) { + if item < len(s.constraints) { + s.stage(&s.constraints[item]) + return + } + if s.jointStage != nil { + s.jointStage(s.joints[item-len(s.constraints)]) + } +} + +// forEachBody runs fn for each body state, in parallel for large scenes +func (s *solver) forEachBody(fn func(i int)) { + s.pool.run(len(s.states), bodiesChunk, fn) +} diff --git a/heightfield_test.go b/heightfield_test.go new file mode 100644 index 0000000..51f7988 --- /dev/null +++ b/heightfield_test.go @@ -0,0 +1,284 @@ +package feather + +import ( + "math" + "math/rand" + "testing" + + "github.com/akmonengine/feather/actor" + "github.com/akmonengine/feather/constraint" + "github.com/go-gl/mathgl/mgl64" +) + +// slopeTerrain: a flat terrain tilted along X (height = tan(angle) * x), 64x64 samples every 0.5 m +func slopeTerrain(w *World, angle float64, friction float64) *actor.RigidBody { + const samples, spacing = 64, 0.5 + heights := make([]float32, samples*samples) + for x := 0; x < samples; x++ { + for z := 0; z < samples; z++ { + heights[x*samples+z] = float32((float64(x) - (samples-1)/2.0) * spacing * math.Tan(angle)) + } + } + field := actor.NewHeightfield(samples, samples, heights, mgl64.Vec3{spacing, 1, spacing}) + return addBody(w, mgl64.Vec3{}, mgl64.QuatIdent(), field, actor.BodyTypeStatic, friction, 0) +} + +// On a flat slope, a sphere rolls exactly like on a plane: the inner edges of the terrain are invisible +func TestHeightfieldSphereRollsLikeOnPlane(t *testing.T) { + angle := 15 * math.Pi / 180 + normal := mgl64.Vec3{-math.Sin(angle), math.Cos(angle), 0} + start := mgl64.Vec3{3.1, 3.1*math.Tan(angle) + 0.3/math.Cos(angle), 0.37} + + onPlane := newScene(1) + addBody(onPlane, mgl64.Vec3{}, mgl64.QuatIdent(), &actor.Plane{Normal: normal}, actor.BodyTypeStatic, 0.5, 0) + planeSphere := addBody(onPlane, start, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.3}, actor.BodyTypeDynamic, 0.5, 0) + + onTerrain := newScene(1) + slopeTerrain(onTerrain, angle, 0.5) + terrainSphere := addBody(onTerrain, start, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.3}, actor.BodyTypeDynamic, 0.5, 0) + + worst := 0.0 + for step := 0; step < int(math.Round(2/sceneDt)); step++ { + onPlane.Step(sceneDt) + onTerrain.Step(sceneDt) + worst = math.Max(worst, planeSphere.Transform.Position.Sub(terrainSphere.Transform.Position).Len()) + } + travelled := terrainSphere.Transform.Position.Sub(start).Len() + t.Logf("travelled %.2f m, worst gap with the plane %.4f mm", travelled, worst*1000) + if travelled < 2 { + t.Errorf("the sphere didn't roll: %.2f m", travelled) + } + if worst > 0.001 { + t.Errorf("the sphere is %.3f mm from the sphere on the plane", worst*1000) + } +} + +// A box sliding on a flat terrain crosses the inner edges without being kicked +func TestHeightfieldBoxSlidesOverInnerEdges(t *testing.T) { + for _, angle := range []float64{0, 10 * math.Pi / 180} { + w := newScene(1) + slopeTerrain(w, angle, 0.1) + normal := mgl64.Vec3{-math.Sin(angle), math.Cos(angle), 0} + rotation := mgl64.QuatBetweenVectors(mgl64.Vec3{0, 1, 0}, normal) + position := mgl64.Vec3{-4, -4 * math.Tan(angle), 0.2}.Add(normal.Mul(cubeHalf)) + box := addBody(w, position, rotation, cube(), actor.BodyTypeDynamic, 0.1, 0) + // sliding up the slope and sideways: across the diagonals and the sides of the cells + box.Velocity = rotation.Rotate(mgl64.Vec3{5, 0, 2}) + + worstNormalSpeed, worstSpin := 0.0, 0.0 + simulate(w, 1, func() { + worstNormalSpeed = math.Max(worstNormalSpeed, math.Abs(box.Velocity.Dot(normal))) + worstSpin = math.Max(worstSpin, box.AngularVelocity.Len()) + }) + travelled := box.Transform.Position.Sub(position).Len() + t.Logf("slope %.0f°: travelled %.2f m, worst normal speed %.4f m/s, worst spin %.4f rad/s", degrees(angle), travelled, worstNormalSpeed, worstSpin) + if travelled < 2 { + t.Errorf("slope %.0f°: the box stopped after %.2f m", degrees(angle), travelled) + } + if worstNormalSpeed > 0.01 || worstSpin > 0.05 { + t.Errorf("slope %.0f°: the box was kicked by an inner edge", degrees(angle)) + } + } +} + +// bumpyTerrain: random hills in a bowl (the bodies stay on the terrain), 48x48 samples every 0.5 m +func bumpyTerrain(w *World, seed int64) *actor.RigidBody { + const samples = 48 + r := rand.New(rand.NewSource(seed)) + heights := make([]float32, samples*samples) + phases := [4]float64{r.Float64() * 6, r.Float64() * 6, r.Float64() * 6, r.Float64() * 6} + for x := 0; x < samples; x++ { + for z := 0; z < samples; z++ { + bowlX, bowlZ := (float64(x)-(samples-1)/2.0)*0.5, (float64(z)-(samples-1)/2.0)*0.5 + heights[x*samples+z] = float32(0.03*(bowlX*bowlX+bowlZ*bowlZ) + 0.6*math.Sin(float64(x)*0.35+phases[0])*math.Cos(float64(z)*0.3+phases[1]) + + 0.3*math.Sin(float64(x+z)*0.8+phases[2]) + 0.05*math.Cos(float64(x-z)*1.7+phases[3])) + } + } + field := actor.NewHeightfield(samples, samples, heights, mgl64.Vec3{0.5, 1, 0.5}) + return addBody(w, mgl64.Vec3{}, mgl64.QuatIdent(), field, actor.BodyTypeStatic, 0.6, 0) +} + +// A body falls through a hole of the terrain, and rests beside it +func TestHeightfieldHoles(t *testing.T) { + w := newScene(1) + heights := make([]float32, 9*9) + field := actor.NewHeightfield(9, 9, heights, mgl64.Vec3{1, 1, 1}) + field.Holes = make([]bool, 8*8) + // 2x2 cells around the center + for _, cell := range [4][2]int{{3, 3}, {3, 4}, {4, 3}, {4, 4}} { + field.Holes[cell[0]*8+cell[1]] = true + } + field.Update(0, 0, 8, 8) + addBody(w, mgl64.Vec3{}, mgl64.QuatIdent(), field, actor.BodyTypeStatic, 0.5, 0) + falling := addBody(w, mgl64.Vec3{0, 1, 0}, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.3}, actor.BodyTypeDynamic, 0.5, 0) + resting := addBody(w, mgl64.Vec3{2.5, 1, 2.5}, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.3}, actor.BodyTypeDynamic, 0.5, 0) + simulate(w, 1.5, nil) + if falling.Transform.Position.Y() > -2 { + t.Errorf("the sphere didn't fall through the hole: %v", falling.Transform.Position) + } + if math.Abs(resting.Transform.Position.Y()-0.3) > 0.001 { + t.Errorf("the sphere beside the hole is at %.4f m, want 0.3", resting.Transform.Position.Y()) + } +} + +// Digging the terrain under a sleeping body wakes it up: it falls in the pit +func TestHeightfieldUpdateWakesBodies(t *testing.T) { + w := newScene(1) + terrain := slopeTerrain(w, 0, 0.5) + field := terrain.Shape.(*actor.Heightfield) + box := addBody(w, mgl64.Vec3{0.1, cubeHalf, 0.2}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.5, 0) + far := addBody(w, mgl64.Vec3{8, cubeHalf, 8}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.5, 0) + simulate(w, 2, nil) + if !box.IsSleeping || !far.IsSleeping { + t.Fatal("the boxes are not asleep") + } + + // a pit of 1 m under the box: the samples around the center + for x := 29; x <= 34; x++ { + for z := 29; z <= 34; z++ { + field.Heights[x*64+z] = -1 + } + } + w.UpdateHeightfield(terrain, 29, 29, 34, 34) + simulate(w, 2, nil) + if box.Transform.Position.Y() > -0.7 { + t.Errorf("the box didn't fall in the pit: %v", box.Transform.Position) + } + if !far.IsSleeping { + t.Error("the box far from the pit woke up") + } +} + +// A box resting in a V valley touches both slopes: 2 patches, and it falls asleep +func TestHeightfieldValley(t *testing.T) { + w := newScene(1) + const samples = 21 + heights := make([]float32, samples*samples) + angle := 30 * math.Pi / 180 + for x := 0; x < samples; x++ { + for z := 0; z < samples; z++ { + heights[x*samples+z] = float32(math.Abs(float64(x)-10) * 0.25 * math.Tan(angle)) + } + } + addBody(w, mgl64.Vec3{}, mgl64.QuatIdent(), actor.NewHeightfield(samples, samples, heights, mgl64.Vec3{0.25, 1, 0.25}), actor.BodyTypeStatic, 0.6, 0) + box := addBody(w, mgl64.Vec3{0, 0.8, 0.1}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{0.3, 0.2, 0.3}}, actor.BodyTypeDynamic, 0.6, 0) + simulate(w, 0.5, nil) + patches := 0 + for _, m := range w.Contacts() { + if m.BodyA == box || m.BodyB == box { + patches++ + } + } + simulate(w, 2.5, nil) + // resting on both slopes: the bottom edges at 0.3 m from the middle, so the center at 0.3*tan + 0.2 + want := 0.3*math.Tan(angle) + 0.2 + t.Logf("%d patches, height %.4f m (want %.4f), asleep %v", patches, box.Transform.Position.Y(), want, box.IsSleeping) + if patches != 2 { + t.Errorf("%d patches, want 2", patches) + } + if !box.IsSleeping { + t.Error("the box is not asleep") + } + if math.Abs(box.Transform.Position.Y()-want) > 0.002 { + t.Errorf("the box rests at %.4f m, want %.4f", box.Transform.Position.Y(), want) + } +} + +// A moved and turned terrain: the bodies rest on its surface +func TestHeightfieldTransform(t *testing.T) { + w := newScene(1) + terrain := bumpyTerrain(w, 3) + terrain.Transform = actor.Transform{Position: mgl64.Vec3{5, -2, 3}, Rotation: mgl64.QuatRotate(0.6, mgl64.Vec3{0, 1, 0})} + terrain.UpdateAABB() + field := terrain.Shape.(*actor.Heightfield) + var spheres []*actor.RigidBody + for i := 0; i < 5; i++ { + local := mgl64.Vec3{float64(i)*2 - 4, 0, float64(i) - 2} + height, _ := field.HeightAt(local.X(), local.Z()) + local[1] = height + 1 + sphere := addBody(w, terrain.Transform.ToWorld(local), mgl64.QuatIdent(), &actor.Sphere{Radius: 0.2}, actor.BodyTypeDynamic, 0.8, 0) + sphere.Material.RollingResistance = 0.3 + spheres = append(spheres, sphere) + } + simulate(w, 4, nil) + for i, sphere := range spheres { + local := terrain.Transform.ToLocal(sphere.Transform.Position) + height, ok := field.HeightAt(local.X(), local.Z()) + if !ok || local.Y() < height || local.Y() > height+0.3 { + t.Errorf("sphere %d: %.3f m above the terrain", i, local.Y()-height) + } + } +} + +// The pair keeps its order: the normal goes from A to B, the terrain can be either +func TestHeightfieldPairOrder(t *testing.T) { + w := newScene(1) + terrain := slopeTerrain(w, 0, 0.5) + sphere := actor.NewRigidBody(actor.Transform{Position: mgl64.Vec3{0.1, 0.29, 0.2}, Rotation: mgl64.QuatIdent()}, &actor.Sphere{Radius: 0.3}, actor.BodyTypeDynamic, 1) + var terrainFirst, sphereFirst [MaxManifoldsPerPair]constraint.Manifold + if CollideAll(terrain, sphere, 0.02, terrainFirst[:]) != 1 || CollideAll(sphere, terrain, 0.02, sphereFirst[:]) != 1 { + t.Fatal("no contact") + } + if terrainFirst[0].Normal.Sub(mgl64.Vec3{0, 1, 0}).Len() > 1e-9 || sphereFirst[0].Normal.Sub(mgl64.Vec3{0, -1, 0}).Len() > 1e-9 { + t.Errorf("normals %v and %v", terrainFirst[0].Normal, sphereFirst[0].Normal) + } + if sphereFirst[0].BodyA != sphere || math.Abs(sphereFirst[0].Points[0].Separation+0.01) > 1e-9 { + t.Errorf("sphere first: %v", sphereFirst[0]) + } + var single constraint.Manifold + if !Collide(sphere, terrain, 0.02, &single) || single.Count != 1 { + t.Error("Collide: no contact") + } +} + +// terrainScene: bodies on hills, for the determinism & the allocations. The parallel paths run under 256 bodies +func terrainScene(workers int) *World { + w := newScene(workers) + w.parallelFrom = 1 + bumpyTerrain(w, 4) + r := rand.New(rand.NewSource(5)) + for i := 0; i < 200; i++ { + var shape actor.ShapeInterface = &actor.Box{HalfExtents: mgl64.Vec3{0.2, 0.15, 0.25}} + switch i % 3 { + case 1: + shape = &actor.Sphere{Radius: 0.2} + case 2: + shape = &actor.Capsule{HalfHeight: 0.25, Radius: 0.12} + } + position := mgl64.Vec3{r.Float64()*16 - 8, 5 + r.Float64()*6, r.Float64()*16 - 8} + addBody(w, position, mgl64.QuatRotate(r.Float64()*6, mgl64.Vec3{0, 1, 0}), shape, actor.BodyTypeDynamic, 0.6, 0) + } + return w +} + +// The same steps with 1 and 8 workers +func TestHeightfieldDeterminism(t *testing.T) { + single, parallel := terrainScene(1), terrainScene(8) + defer single.Close() + defer parallel.Close() + for step := 0; step < 120; step++ { + single.Step(sceneDt) + parallel.Step(sceneDt) + } + for i := range single.Bodies { + if single.Bodies[i].Transform != parallel.Bodies[i].Transform { + t.Fatalf("body %d: %v with 1 worker, %v with 8", i, single.Bodies[i].Transform, parallel.Bodies[i].Transform) + } + } +} + +func TestHeightfieldDoesNotAllocate(t *testing.T) { + if raceEnabled { + t.Skip("sync.Pool allocates under the race detector") + } + w := terrainScene(4) + defer w.Close() + // the buffers grow while the bodies land + for step := 0; step < 300; step++ { + w.Step(sceneDt) + } + allocations := testing.AllocsPerRun(20, func() { w.Step(sceneDt) }) + if allocations > 0 { + t.Errorf("%.1f allocations per step", allocations) + } +} diff --git a/invariants_test.go b/invariants_test.go new file mode 100644 index 0000000..792ee52 --- /dev/null +++ b/invariants_test.go @@ -0,0 +1,292 @@ +package feather + +import ( + "fmt" + "math" + "math/rand" + "testing" + + "github.com/akmonengine/feather/actor" + "github.com/go-gl/mathgl/mgl64" +) + +// ========== INVARIANTS ========== +// Random scenes, each from a fixed seed, checked at every step against the laws every step must keep: +// - finite positions, velocities & rotations, unit quaternions +// - 1 and 8 workers give the same bits +// - no body stays deeper than LinearSlop in a plane: a hit can push it deeper (the contacts are springs, as in Box2D +// & Box3D: ~3 % of the random piles), then it gets out, less deep at each step. In a terrain, the depth is only +// logged: the face contact of a triangle comes from the corners above it, a corner just beside it can stay a few +// mm deep (#819, see ARCHITECTURE.md) +// - a closed system never gains energy, with a restitution up to 0.5 (over it, a body spinning fast can bounce +// higher than it fell, see ARCHITECTURE.md) +// - in free flight, the momentum & the angular momentum are kept (the angular momentum during the steps without +// contact) +// A scene is a pile on a plane, a pile on a terrain, or bodies & joints colliding in free flight. + +const ( + // invariantScenes: the count of random scenes + invariantScenes = 60 + + // invariantSeconds: the simulated time of a scene (s) + invariantSeconds = 3 + + // unitTolerance of a quaternion: it is normalized at every sub-step, only the rounding remains + unitTolerance = 1e-9 + + // momentumTolerance: the impulses are applied to both bodies, opposite: only the rounding changes the momentum + momentumTolerance = 1e-9 + + // maxRestitution of the random scenes: the energy never grows up to it + maxRestitution = 0.5 +) + +// sceneKind of a random scene +type sceneKind int + +const ( + pileOnPlane sceneKind = iota + pileOnTerrain + freeFlight + sceneKinds +) + +func (k sceneKind) String() string { + return [...]string{"pile on a plane", "pile on a terrain", "free flight"}[k] +} + +// randomShape: a box, a sphere or a capsule, of 10 to 40 cm +func randomShape(r *rand.Rand) actor.ShapeInterface { + switch r.Intn(3) { + case 0: + return &actor.Box{HalfExtents: mgl64.Vec3{0.05 + 0.25*r.Float64(), 0.02 + 0.2*r.Float64(), 0.05 + 0.25*r.Float64()}} + case 1: + return &actor.Sphere{Radius: 0.05 + 0.2*r.Float64()} + } + return &actor.Capsule{HalfHeight: 0.05 + 0.25*r.Float64(), Radius: 0.05 + 0.15*r.Float64()} +} + +func randomRotation(r *rand.Rand) mgl64.Quat { + return mgl64.QuatRotate(r.Float64()*2*math.Pi, mgl64.Vec3{r.Float64() - 0.5, r.Float64() - 0.5, r.Float64() - 0.5}.Normalize()) +} + +// randomVector of length up to size +func randomVector(r *rand.Rand, size float64) mgl64.Vec3 { + return mgl64.Vec3{r.Float64() - 0.5, r.Float64() - 0.5, r.Float64() - 0.5}.Mul(2 * size) +} + +// randomScene: the same seed & kind give the same scene, whatever the workers +func randomScene(seed int64, workers int) (*World, sceneKind) { + r := rand.New(rand.NewSource(seed)) + kind := sceneKind(seed % int64(sceneKinds)) + w := newScene(workers) + w.parallelFrom = 1 + count := 4 + r.Intn(9) + // the same restitution for all the bodies & the ground (it is averaged between 2 bodies): the highest in a third + // of the scenes + restitution := r.Float64() * maxRestitution + if r.Intn(3) == 0 { + restitution = maxRestitution + } + + if kind == freeFlight { + // bodies thrown at each other around the origin, spinning up to 100 rad/s, some linked by joints + w.Gravity = mgl64.Vec3{} + var bodies []*actor.RigidBody + for i := 0; i < count; i++ { + position := mgl64.Vec3{float64(i%3) - 1, float64(i/3%3) - 1, float64(i/9) - 0.5}.Mul(1.2).Add(randomVector(r, 0.1)) + body := addBody(w, position, randomRotation(r), randomShape(r), actor.BodyTypeDynamic, r.Float64(), restitution) + body.Velocity = position.Mul(-2).Add(randomVector(r, 2)) + body.AngularVelocity = randomVector(r, 100/math.Sqrt(3)) + bodies = append(bodies, body) + } + for i := 0; i+1 < len(bodies); i += 3 { + a, b := bodies[i], bodies[i+1] + middle := a.Transform.Position.Add(b.Transform.Position).Mul(0.5) + if r.Intn(2) == 0 { + w.AddJoint(NewBallJoint(a, b, middle, mgl64.Vec3{0, 1, 0})) + } else { + w.AddJoint(NewDistanceJoint(a, b, a.Transform.Position, b.Transform.Position)) + } + } + return w, kind + } + + // a pile falling from 0.5 to 3 m, turned, some thrown down & spinning + var field *actor.Heightfield + if kind == pileOnTerrain { + terrain := bumpyTerrain(w, seed) + terrain.Material.Restitution = restitution + field = terrain.Shape.(*actor.Heightfield) + } else { + addGround(w, r.Float64()).Material.Restitution = restitution + } + for i := 0; i < count; i++ { + x, z := r.Float64()*3-1.5, r.Float64()*3-1.5 + ground := 0.0 + if field != nil { + ground, _ = field.HeightAt(x, z) + } + position := mgl64.Vec3{x, ground + 0.5 + 0.4*float64(i) + r.Float64()*0.2, z} + body := addBody(w, position, randomRotation(r), randomShape(r), actor.BodyTypeDynamic, r.Float64(), restitution) + body.Velocity = randomVector(r, 3) + body.AngularVelocity = randomVector(r, 20) + } + return w, kind +} + +// mechanics: the quantities a closed system keeps +type mechanics struct { + energy float64 + momentum mgl64.Vec3 + angularMomentum mgl64.Vec3 + // scale of the momentum & of the angular momentum, for the tolerances: the sums of their norms + momentumScale float64 + angularMomentumScale float64 + // potentialSlop: the energy gained if every body rose by LinearSlop + potentialSlop float64 + // gyroscopicError: the change of the angular momentum allowed by the implicit gyroscopic torque during a step. + // It is a first order method (Catto, GDC 2015): each sub-step changes the angular momentum I ω of a body by + // less than |I ω| (|ω| h)² + gyroscopicError float64 + // jointCouple: the change of the angular momentum allowed by the joints during a step. A soft joint pulls its 2 + // anchors, apart by its gap, with opposite impulses: a couple of gap × impulse at each sub-step + jointCouple float64 + // depths of the bodies in the planes + depths []float64 +} + +func measure(w *World) mechanics { + var m mechanics + gravity := w.Gravity.Len() + h := sceneDt / float64(w.Substeps) + for _, body := range w.Bodies { + if body.BodyType != actor.BodyTypeDynamic { + continue + } + mass := body.Material.GetMass() + spin := body.GetInertiaWorld().Mul3x1(body.AngularVelocity) + m.energy += 0.5*mass*body.Velocity.LenSqr() + 0.5*body.AngularVelocity.Dot(spin) - mass*w.Gravity.Dot(body.Transform.Position) + m.momentum = m.momentum.Add(body.Velocity.Mul(mass)) + m.momentumScale += mass * body.Velocity.Len() + orbital := body.Transform.Position.Cross(body.Velocity.Mul(mass)) + m.angularMomentum = m.angularMomentum.Add(orbital).Add(spin) + m.angularMomentumScale += orbital.Len() + spin.Len() + m.potentialSlop += mass * gravity * LinearSlop + turn := body.AngularVelocity.Len() * h + m.gyroscopicError += spin.Len() * float64(w.Substeps) * turn * turn + } + m.depths = planeDepths(w) + for _, joint := range w.Joints { + j := joint.base() + gap := j.BodyA.Transform.ToWorld(j.LocalFrameA.Position).Sub(j.BodyB.Transform.ToWorld(j.LocalFrameB.Position)).Len() + m.jointCouple += float64(w.Substeps) * gap * j.linearImpulse.Len() + } + return m +} + +// planeDepths: the depth of each dynamic body in the static bodies which are not terrains (0 for the others) +func planeDepths(w *World) []float64 { + depths := make([]float64, len(w.Bodies)) + for _, surface := range w.Bodies { + if surface.BodyType != actor.BodyTypeStatic { + continue + } + if _, terrain := surface.Shape.(*actor.Heightfield); terrain { + continue + } + for i, body := range w.Bodies { + if body.BodyType == actor.BodyTypeDynamic { + depths[i] = math.Max(depths[i], surfaceDepth(surface, body)) + } + } + } + return depths +} + +// checkInvariants of the world after a step, against its state at the start of the scene (start) and of the step +// (before), and the same scene run with other workers (twin). Returns the first broken invariant +func checkInvariants(w, twin *World, kind sceneKind, start, before mechanics) error { + for i, body := range w.Bodies { + transform := body.Transform + if !finite(transform.Position) || !finite(body.Velocity) || !finite(body.AngularVelocity) { + return fmt.Errorf("body %d is not finite: %v %v %v", i, transform.Position, body.Velocity, body.AngularVelocity) + } + if length := transform.Rotation.Len(); math.Abs(length-1) > unitTolerance { + return fmt.Errorf("body %d: the rotation is not a unit quaternion (%v)", i, length) + } + other := twin.Bodies[i] + if transform != other.Transform || body.Velocity != other.Velocity || body.AngularVelocity != other.AngularVelocity { + return fmt.Errorf("body %d: %v with %d workers, %v with %d", i, transform, w.Workers, other.Transform, twin.Workers) + } + } + + for i, depth := range planeDepths(w) { + if depth > LinearSlop && before.depths[i] > LinearSlop && depth >= before.depths[i] { + return fmt.Errorf("body %d (%T) stays %.2f mm in the ground (%.2f mm the step before)", i, w.Bodies[i].Shape, + depth*1000, before.depths[i]*1000) + } + } + + now := measure(w) + // the free fall integrated by the symplectic Euler loses m g² h² / 2 per sub-step, the restitution is at most 1: + // the energy grows only when a contact pushes a body out of the ground, by LinearSlop at most + if gain := now.energy - start.energy; gain > start.potentialSlop+1e-9*math.Abs(start.energy) { + return fmt.Errorf("the energy grew by %.4f J (from %.4f J)", gain, start.energy) + } + if kind == freeFlight { + if drift := now.momentum.Sub(start.momentum).Len(); drift > momentumTolerance*start.momentumScale { + return fmt.Errorf("the momentum changed by %.3g kg·m/s (of %.3f)", drift, start.momentumScale) + } + // the gyroscopic error and the couple of the joints, before or after the step, whichever is larger + allowed := math.Max(before.gyroscopicError, now.gyroscopicError) + math.Max(before.jointCouple, now.jointCouple) + if drift := now.angularMomentum.Sub(before.angularMomentum).Len(); len(w.Contacts()) == 0 && drift > allowed { + return fmt.Errorf("the angular momentum changed by %.4f kg·m²/s during the step (%.4f allowed)", drift, allowed) + } + } + return nil +} + +// worldTerrainDepth: the deepest body in a terrain of the world, 0 without terrain +func worldTerrainDepth(w *World) float64 { + worst := 0.0 + for _, surface := range w.Bodies { + if _, terrain := surface.Shape.(*actor.Heightfield); terrain { + for _, body := range w.Bodies { + if body.BodyType == actor.BodyTypeDynamic { + worst = math.Max(worst, surfaceDepth(surface, body)) + } + } + } + } + return worst +} + +// Every random scene keeps the invariants at every step. The scenes run in parallel +func TestInvariants(t *testing.T) { + for seed := int64(0); seed < invariantScenes; seed++ { + t.Run(fmt.Sprint(seed), func(t *testing.T) { + t.Parallel() + single, kind := randomScene(seed, 1) + parallel, _ := randomScene(seed, 8) + defer single.Close() + defer parallel.Close() + start := measure(single) + before := start + terrainDepth := 0.0 + for step := 0; step < int(math.Round(invariantSeconds/sceneDt)); step++ { + single.Step(sceneDt) + parallel.Step(sceneDt) + err := checkInvariants(single, parallel, kind, start, before) + before = measure(single) + if err != nil { + t.Fatalf("seed %d, %s, step %d: %v", seed, kind, step, err) + } + terrainDepth = math.Max(terrainDepth, worldTerrainDepth(single)) + } + if kind == pileOnTerrain { + t.Logf("seed %d: deepest in the terrain %.2f mm", seed, terrainDepth*1000) + } + }) + } +} diff --git a/island.go b/island.go new file mode 100644 index 0000000..42ef33c --- /dev/null +++ b/island.go @@ -0,0 +1,143 @@ +package feather + +import ( + "math" + + "github.com/akmonengine/feather/actor" +) + +// sleepIslands: the bodies touching each other form an island (as in Box2D). An island falls asleep when all +// its bodies are resting, and wakes up entirely when one of its bodies wakes up. +// A body can't sleep under a moving body anymore, and the bodies above a removed body wake up. +type sleepIslands struct { + // union-find over the dynamic bodies of the solver + parent []int + minTimer []float64 + island []int + + // the sleeping islands, and the island of each sleeping body + islands [][]*actor.RigidBody + free []int + islandOf map[*actor.RigidBody]int +} + +func (si *sleepIslands) find(i int) int { + for si.parent[i] != i { + si.parent[i] = si.parent[si.parent[i]] + i = si.parent[i] + } + return i +} + +func (si *sleepIslands) union(a, b int) { + rootA, rootB := si.find(a), si.find(b) + // the smallest index is the root: the islands never depend on the order of the contacts + if rootA < rootB { + si.parent[rootB] = rootA + } else if rootB < rootA { + si.parent[rootA] = rootB + } +} + +// update the sleep timers of the bodies, and puts to sleep the islands resting long enough +func (si *sleepIslands) update(s *solver, dt float64) { + count := len(s.states) + if cap(si.parent) < count { + si.parent = make([]int, count) + si.minTimer = make([]float64, count) + si.island = make([]int, count) + } + si.parent, si.minTimer, si.island = si.parent[:count], si.minTimer[:count], si.island[:count] + + // ========== 1. Timers ========== + for i := range s.states { + body := s.states[i].body + if body.Velocity.Len() < actor.DefaultSleepSpeed && body.AngularVelocity.Len() < actor.DefaultSleepSpeed { + body.SleepTimer += dt + } else { + body.SleepTimer = 0 + } + si.parent[i] = i + si.minTimer[i] = math.Inf(1) + si.island[i] = -1 + } + + // ========== 2. Islands: the dynamic bodies linked by a contact or a joint ========== + for i := range s.constraints { + c := &s.constraints[i] + if c.indexA >= 0 && c.indexB >= 0 && c.pointsCount > 0 { + si.union(c.indexA, c.indexB) + } + } + for _, joint := range s.joints { + base := joint.base() + if base.indexA >= 0 && base.indexB >= 0 { + si.union(base.indexA, base.indexB) + } + } + for i := range s.states { + root := si.find(i) + si.minTimer[root] = math.Min(si.minTimer[root], s.states[i].body.SleepTimer) + } + + // ========== 3. Sleep ========== + for i := range s.states { + root := si.find(i) + if si.minTimer[root] < actor.DefaultTimeToSleep { + continue + } + if si.island[root] < 0 { + si.island[root] = si.newIsland() + } + body := s.states[i].body + body.Sleep() + k := si.island[root] + si.islands[k] = append(si.islands[k], body) + si.islandOf[body] = k + } +} + +func (si *sleepIslands) newIsland() int { + if si.islandOf == nil { + si.islandOf = make(map[*actor.RigidBody]int) + } + if n := len(si.free); n > 0 { + k := si.free[n-1] + si.free = si.free[:n-1] + return k + } + si.islands = append(si.islands, nil) + return len(si.islands) - 1 +} + +// wake the island of the body (if it is sleeping) +func (si *sleepIslands) wake(body *actor.RigidBody) { + k, ok := si.islandOf[body] + if !ok { + body.WakeUp() + return + } + for _, member := range si.islands[k] { + member.WakeUp() + delete(si.islandOf, member) + } + si.islands[k] = si.islands[k][:0] + si.free = append(si.free, k) +} + +// wakeWoken: a body woken up from outside (AddForce, WakeUp) wakes up its whole island +func (si *sleepIslands) wakeWoken() { + for k := range si.islands { + for _, member := range si.islands[k] { + if !member.IsSleeping { + si.wake(member) + break + } + } + } +} + +// remove a body from its island: the island wakes up, the bodies it was holding must fall +func (si *sleepIslands) remove(body *actor.RigidBody) { + si.wake(body) +} diff --git a/joint.go b/joint.go new file mode 100644 index 0000000..f71a54a --- /dev/null +++ b/joint.go @@ -0,0 +1,641 @@ +package feather + +import ( + "math" + + "github.com/akmonengine/feather/actor" + "github.com/go-gl/mathgl/mgl64" +) + +// Joints are solved like the contacts (TGS Soft, as in Box2D v3): warm starting, soft constraints in the push, +// then rigid constraints in the relax. Each joint links 2 bodies with a frame on each body. +// The X axis of the frames is the axis of the hinge, and the twist axis of the ball joint (as in PhysX). +const ( + // DefaultJointHertz is the stiffness of the joints + DefaultJointHertz = 60.0 + + // DefaultJointDampingRatio of the joints + DefaultJointDampingRatio = 2.0 + + // the joint hertz can't exceed 1/4 of the sub-steps rate + jointHertzPerSubstepRate = 0.25 + + // under this length, the axis of a distance joint is not reliable (m) + jointMinLength = 1e-9 +) + +// Joint links 2 bodies (see DistanceJoint, BallJoint, HingeJoint, FixedJoint) +type Joint interface { + base() *JointBase + prepare(s *solver) + warmStart(s *solver) + solve(s *solver, useBias bool) +} + +// JointBase holds the settings common to all the joints +type JointBase struct { + BodyA *actor.RigidBody // can be static: the joint is attached to the world + BodyB *actor.RigidBody + // LocalFrameA & LocalFrameB: the anchor and the orientation of the joint in the local space of each body + LocalFrameA actor.Transform + LocalFrameB actor.Transform + // CollideConnected: if false, the 2 bodies don't collide with each other + CollideConnected bool + // Hertz & DampingRatio: the softness of the joint. 0 hertz = DefaultJointHertz + Hertz float64 + DampingRatio float64 + + // ========== solver ========== + indexA, indexB int + frameA, frameB mgl64.Quat // world rotation of the frames, at the beginning of the step + anchorA mgl64.Vec3 // anchors from the centers of mass, world orientation, at the beginning of the step + anchorB mgl64.Vec3 + deltaCenter mgl64.Vec3 + spring spring + + linearImpulse mgl64.Vec3 + // inArticulation: the point constraint is solved with the other joints of its tree (articulation.go) + inArticulation bool +} + +func (j *JointBase) base() *JointBase { return j } + +// prepareBase computes the world frames of the joint, at the beginning of the step +func (j *JointBase) prepareBase(s *solver) { + j.indexA, j.indexB = s.indexOf(j.BodyA), s.indexOf(j.BodyB) + transformA, transformB := j.BodyA.Transform, j.BodyB.Transform + j.frameA = transformA.Rotation.Mul(j.LocalFrameA.Rotation).Normalize() + j.frameB = transformB.Rotation.Mul(j.LocalFrameB.Rotation).Normalize() + j.anchorA = transformA.Rotation.Rotate(j.LocalFrameA.Position) + j.anchorB = transformB.Rotation.Rotate(j.LocalFrameB.Position) + j.deltaCenter = transformB.Position.Sub(transformA.Position) + + hertz := j.Hertz + if hertz <= 0 { + hertz = DefaultJointHertz + } + j.spring = newSpring(math.Min(hertz, jointHertzPerSubstepRate*s.invH), j.DampingRatio, s.h) +} + +// currentAnchors during the substeps +func (j *JointBase) currentAnchors(stateA, stateB *bodyState) (mgl64.Vec3, mgl64.Vec3) { + return actor.MulMat3(&stateA.deltaMatrix, j.anchorA), actor.MulMat3(&stateB.deltaMatrix, j.anchorB) +} + +// currentFrames: world rotation of both frames during the substeps +func (j *JointBase) currentFrames(stateA, stateB *bodyState) (mgl64.Quat, mgl64.Quat) { + return actor.MulQuat(&stateA.deltaRotation, &j.frameA), actor.MulQuat(&stateB.deltaRotation, &j.frameB) +} + +// ========== Point constraint ========== +// The anchors of both bodies stay at the same place (3 rows) + +func (j *JointBase) solvePoint(s *solver, stateA, stateB *bodyState, useBias bool) { + if j.inArticulation { + return + } + rA, rB := j.currentAnchors(stateA, stateB) + cdot := relativeVelocity(stateA, stateB, rA, rB) + + bias, row := mgl64.Vec3{}, rigid + if useBias { + separation := stateB.deltaPosition.Sub(stateA.deltaPosition).Add(rB.Sub(rA)).Add(j.deltaCenter) + row = j.spring + bias = separation.Mul(row.biasRate) + } + + // K = (mA + mB) I - [rA]x IA [rA]x - [rB]x IB [rB]x + m := stateA.invMass + stateB.invMass + identity := mgl64.Mat3{1 * m, 0 * m, 0 * m, 0 * m, 1 * m, 0 * m, 0 * m, 0 * m, 1 * m} + termA, termB := skewTerm(&stateA.inverseInertia, rA, rA), skewTerm(&stateB.inverseInertia, rB, rB) + k := actor.Sub3(&identity, &termA) + k = actor.Sub3(&k, &termB) + if math.Abs(actor.Det3(&k)) < 1e-30 { + return + } + inverse := actor.Inv3(&k) + impulse := row.impulse3(&inverse, cdot, bias, j.linearImpulse) + j.linearImpulse = j.linearImpulse.Add(impulse) + applyLinear(stateA, stateB, rA, rB, impulse) +} + +// applyLinear: -impulse at rA on A, +impulse at rB on B +func applyLinear(stateA, stateB *bodyState, rA, rB, impulse mgl64.Vec3) { + if stateA.body != nil { + v, w, m := &stateA.velocity, &stateA.angularVelocity, stateA.invMass + v[0], v[1], v[2] = v[0]-impulse[0]*m, v[1]-impulse[1]*m, v[2]-impulse[2]*m + torque := actor.MulMat3(&stateA.inverseInertia, rA.Cross(impulse)) + w[0], w[1], w[2] = w[0]-torque[0], w[1]-torque[1], w[2]-torque[2] + } + if stateB.body != nil { + v, w, m := &stateB.velocity, &stateB.angularVelocity, stateB.invMass + v[0], v[1], v[2] = v[0]+impulse[0]*m, v[1]+impulse[1]*m, v[2]+impulse[2]*m + torque := actor.MulMat3(&stateB.inverseInertia, rB.Cross(impulse)) + w[0], w[1], w[2] = w[0]+torque[0], w[1]+torque[1], w[2]+torque[2] + } +} + +// applyAngular: -impulse on A, +impulse on B +func applyAngular(stateA, stateB *bodyState, impulse mgl64.Vec3) { + if stateA.body != nil { + w, t := &stateA.angularVelocity, actor.MulMat3(&stateA.inverseInertia, impulse) + w[0], w[1], w[2] = w[0]-t[0], w[1]-t[1], w[2]-t[2] + } + if stateB.body != nil { + w, t := &stateB.angularVelocity, actor.MulMat3(&stateB.inverseInertia, impulse) + w[0], w[1], w[2] = w[0]+t[0], w[1]+t[1], w[2]+t[2] + } +} + +// axialMass for an angular impulse around the axis +func axialMass(stateA, stateB *bodyState, axis mgl64.Vec3) float64 { + k := axis.Dot(actor.MulMat3(&stateA.inverseInertia, axis)) + axis.Dot(actor.MulMat3(&stateB.inverseInertia, axis)) + if k <= 0 { + return 0 + } + return 1 / k +} + +// solveAngularLimit: C >= 0 around the axis, with C = direction * (angle of B around the axis) + offset. +// Returns the new accumulated impulse. Speculative when C > 0, soft when useBias. +func (j *JointBase) solveAngularLimit(s *solver, stateA, stateB *bodyState, axis mgl64.Vec3, c float64, direction float64, accumulated float64, useBias bool) float64 { + bias, row := j.limitRow(s, c, useBias) + cdot := direction * axis.Dot(stateB.angularVelocity.Sub(stateA.angularVelocity)) + impulse := row.impulse(axialMass(stateA, stateB, axis), cdot, bias, accumulated) + newImpulse := math.Max(accumulated+impulse, 0) + applyAngular(stateA, stateB, axis.Mul(direction*(newImpulse-accumulated))) + return newImpulse +} + +// limitRow: the bias and the spring of a limit C >= 0. Speculative when C > 0 (the bodies can get closer by C during +// the substep), soft when useBias, rigid otherwise +func (j *JointBase) limitRow(s *solver, c float64, useBias bool) (float64, spring) { + switch { + case c > 0: + return c * s.invH, rigid + case useBias: + return j.spring.biasRate * c, j.spring + } + return 0, rigid +} + +// rotationError is the rotation vector (world space) from the target to the current rotation, for small errors +func rotationError(current, target mgl64.Quat) mgl64.Vec3 { + conjugate := mgl64.Quat{W: target.W, V: target.V.Mul(-1)} + q := actor.MulQuat(¤t, &conjugate) + if q.W < 0 { + q = q.Scale(-1) + } + return q.V.Mul(2) +} + +// twistAngle: the rotation of B relative to A around the X axis, after the swing (swing-twist decomposition) +func twistAngle(frameA, frameB mgl64.Quat) float64 { + conjugate := mgl64.Quat{W: frameA.W, V: frameA.V.Mul(-1)} + relative := actor.MulQuat(&conjugate, &frameB) + if relative.W < 0 { + relative = relative.Scale(-1) + } + return 2 * math.Atan2(relative.V.X(), relative.W) +} + +// angleRow: a rotation constraint on a function f(p) of the X axis of the frame B seen in the frame A (p, unit vector). +// p moves as dp/dt = ω × p, so the rate of f is ω · (p × ∇f): returns f(p), the unit rotation axis p × ∇f +// (orthogonal to p: no twist), and the rate of f per unit of angular velocity around this axis +func angleRow(p mgl64.Vec3, f func(p mgl64.Vec3) float64) (float64, mgl64.Vec3, float64, bool) { + // gradient of f in the tangent plane of p + const epsilon = 1e-6 + tangent1 := anyPerpendicular(p) + tangent2 := p.Cross(tangent1) + d1 := (f(p.Add(tangent1.Mul(epsilon)).Normalize()) - f(p.Sub(tangent1.Mul(epsilon)).Normalize())) / (2 * epsilon) + d2 := (f(p.Add(tangent2.Mul(epsilon)).Normalize()) - f(p.Sub(tangent2.Mul(epsilon)).Normalize())) / (2 * epsilon) + axis := p.Cross(tangent1.Mul(d1).Add(tangent2.Mul(d2))) + rate := axis.Len() + if rate < 1e-12 { + return 0, mgl64.Vec3{}, 0, false + } + return f(p), axis.Mul(1 / rate), rate, true +} + +// swingLimit: p must stay in the elliptic cone of the 2 half angles. +// Returns the distance to the cone (rad, > 0 inside) and the rotation axis moving p out of the cone +func swingLimit(p mgl64.Vec3, limitY, limitZ float64) (float64, mgl64.Vec3, bool) { + cone := func(p mgl64.Vec3) float64 { + r := math.Hypot(p.Y(), p.Z()) + if r < 1e-12 { + return 0 + } + // rotation around Z moves X towards Y, rotation around Y moves X towards -Z + return math.Atan2(r, p.X()) / r * math.Hypot(p.Y()/limitZ, p.Z()/limitY) + } + if cone(p) == 0 { + return 0, mgl64.Vec3{}, false + } + f, axis, rate, ok := angleRow(p, cone) + if !ok { + return 0, mgl64.Vec3{}, false + } + return (1 - f) / rate, axis, true +} + +// twistRow: the twist angle, and its exact rate: d(twist)/dt = rate * (axis · (wB - wA)). +// The rate of the twist is not along the axes X when B swings: it is measured by turning B a little around each axis. +// A rotation of both frames together doesn't change the twist, so the rate depends on wB - wA only +func twistRow(frameA, frameB mgl64.Quat) (float64, mgl64.Vec3, float64) { + const epsilon = 1e-6 + twist := twistAngle(frameA, frameB) + var gradient mgl64.Vec3 + for k := 0; k < 3; k++ { + var delta mgl64.Vec3 + delta[k] = epsilon + plus := twistAngle(frameA, integrateRotation(&frameB, delta)) + minus := twistAngle(frameA, integrateRotation(&frameB, delta.Mul(-1))) + gradient[k] = math.Remainder(plus-minus, 2*math.Pi) / (2 * epsilon) + } + rate := gradient.Len() + if rate < 1e-12 { + return twist, actor.Rotate(&frameA, mgl64.Vec3{1, 0, 0}), 1 + } + return twist, gradient.Mul(1 / rate), rate +} + +// frameFromAxis returns a rotation turning X onto the axis +func frameFromAxis(axis mgl64.Vec3) mgl64.Quat { + return mgl64.QuatBetweenVectors(mgl64.Vec3{1, 0, 0}, axis.Normalize()) +} + +// newJointBase: the joint frames from an anchor and an orientation in world space +func newJointBase(bodyA, bodyB *actor.RigidBody, anchor mgl64.Vec3, frame mgl64.Quat) JointBase { + local := func(body *actor.RigidBody) actor.Transform { + return actor.Transform{Position: body.Transform.ToLocal(anchor), Rotation: body.Transform.Rotation.Conjugate().Mul(frame).Normalize()} + } + return JointBase{ + BodyA: bodyA, + BodyB: bodyB, + LocalFrameA: local(bodyA), + LocalFrameB: local(bodyB), + Hertz: DefaultJointHertz, + DampingRatio: DefaultJointDampingRatio, + } +} + +// ========== Distance ========== + +// DistanceJoint keeps the anchors at a distance: fixed (Length), in a range [MinLength, MaxLength] (a rope), +// or with a spring +type DistanceJoint struct { + JointBase + Length float64 + + EnableLimit bool + MinLength float64 + MaxLength float64 + + EnableSpring bool + SpringHertz float64 + SpringDampingRatio float64 + + springRow spring + impulse float64 + lowerImpulse float64 + upperImpulse float64 +} + +// NewDistanceJoint links 2 anchors (world space), at their current distance +func NewDistanceJoint(bodyA, bodyB *actor.RigidBody, anchorA, anchorB mgl64.Vec3) *DistanceJoint { + j := &DistanceJoint{JointBase: newJointBase(bodyA, bodyB, anchorA, mgl64.QuatIdent())} + j.LocalFrameB.Position = bodyB.Transform.ToLocal(anchorB) + j.Length = anchorB.Sub(anchorA).Len() + j.MinLength, j.MaxLength = j.Length, j.Length + return j +} + +func (j *DistanceJoint) prepare(s *solver) { + j.prepareBase(s) + if j.SpringHertz > 0 { + j.springRow = newSpring(j.SpringHertz, j.SpringDampingRatio, s.h) + } +} + +func (j *DistanceJoint) axis(stateA, stateB *bodyState) (mgl64.Vec3, mgl64.Vec3, mgl64.Vec3, float64) { + rA, rB := j.currentAnchors(stateA, stateB) + separation := j.deltaCenter.Add(stateB.deltaPosition.Sub(stateA.deltaPosition)).Add(rB.Sub(rA)) + length := separation.Len() + if length < jointMinLength { + return rA, rB, mgl64.Vec3{0, 1, 0}, length + } + return rA, rB, separation.Mul(1 / length), length +} + +func (j *DistanceJoint) warmStart(s *solver) { + stateA, stateB := s.state(j.indexA), s.state(j.indexB) + rA, rB, axis, _ := j.axis(stateA, stateB) + applyLinear(stateA, stateB, rA, rB, axis.Mul(j.impulse+j.lowerImpulse-j.upperImpulse)) +} + +// solveLinearAxis solves an impulse along the axis, applied at rA on A and rB on B: returns the new accumulated impulse +func solveLinearAxis(stateA, stateB *bodyState, rA, rB, axis mgl64.Vec3, bias float64, row spring, accumulated, low, high float64) float64 { + cdot := axis.Dot(relativeVelocity(stateA, stateB, rA, rB)) + rnA, rnB := rA.Cross(axis), rB.Cross(axis) + k := stateA.invMass + stateB.invMass + rnA.Dot(actor.MulMat3(&stateA.inverseInertia, rnA)) + rnB.Dot(actor.MulMat3(&stateB.inverseInertia, rnB)) + if k <= 0 { + return accumulated + } + impulse := row.impulse(1/k, cdot, bias, accumulated) + newImpulse := math.Max(low, math.Min(high, accumulated+impulse)) + applyLinear(stateA, stateB, rA, rB, axis.Mul(newImpulse-accumulated)) + return newImpulse +} + +func (j *DistanceJoint) solve(s *solver, useBias bool) { + stateA, stateB := s.state(j.indexA), s.state(j.indexB) + rA, rB, axis, length := j.axis(stateA, stateB) + infinite := math.Inf(1) + + if j.EnableSpring && (j.MinLength < j.MaxLength || !j.EnableLimit) { + // ========== SPRING ========== + if j.SpringHertz > 0 { + bias := j.springRow.biasRate * (length - j.Length) + j.impulse = solveLinearAxis(stateA, stateB, rA, rB, axis, bias, j.springRow, j.impulse, -infinite, infinite) + } + + // ========== LIMITS ========== + if j.EnableLimit { + j.lowerImpulse = j.solveLimit(s, stateA, stateB, rA, rB, axis, length-j.MinLength, j.lowerImpulse, useBias) + j.upperImpulse = j.solveLimit(s, stateA, stateB, rA, rB, axis.Mul(-1), j.MaxLength-length, j.upperImpulse, useBias) + } + return + } + + // ========== RIGID ========== + bias, row := 0.0, rigid + if useBias { + row = j.spring + bias = row.biasRate * (length - j.Length) + } + j.impulse = solveLinearAxis(stateA, stateB, rA, rB, axis, bias, row, j.impulse, -infinite, infinite) +} + +// solveLimit: C >= 0 along the axis +func (j *DistanceJoint) solveLimit(s *solver, stateA, stateB *bodyState, rA, rB, axis mgl64.Vec3, c, accumulated float64, useBias bool) float64 { + bias, row := j.limitRow(s, c, useBias) + return solveLinearAxis(stateA, stateB, rA, rB, axis, bias, row, accumulated, 0, math.Inf(1)) +} + +// ========== Ball ========== + +// BallJoint (ball and socket) keeps the anchors together, the bodies rotate freely. +// Optional limits: a cone for the swing of the X axis (half angles around Y and Z), and a range for the twist +// around X. Optional drive: a spring towards a target rotation of the frame B relative to the frame A. +type BallJoint struct { + JointBase + + EnableSwingLimit bool + SwingLimitY float64 // rad, rotation of the X axis around Y + SwingLimitZ float64 // rad, rotation of the X axis around Z + + EnableTwistLimit bool + TwistMin float64 // rad + TwistMax float64 // rad + + EnableDrive bool + DriveTarget mgl64.Quat // rotation of the frame B relative to the frame A + DriveHertz float64 + DriveDampingRatio float64 + + driveRow spring + swingImpulse float64 + twistLowerImpulse float64 + twistUpperImpulse float64 + driveImpulse mgl64.Vec3 + swingAxis mgl64.Vec3 + twistAxis mgl64.Vec3 +} + +// NewBallJoint links 2 bodies at an anchor (world space). The twist axis is X of the frames: twistAxis in world space +func NewBallJoint(bodyA, bodyB *actor.RigidBody, anchor, twistAxis mgl64.Vec3) *BallJoint { + return &BallJoint{JointBase: newJointBase(bodyA, bodyB, anchor, frameFromAxis(twistAxis)), DriveTarget: mgl64.QuatIdent()} +} + +func (j *BallJoint) prepare(s *solver) { + j.prepareBase(s) + if j.DriveHertz > 0 { + j.driveRow = newSpring(j.DriveHertz, j.DriveDampingRatio, s.h) + } +} + +func (j *BallJoint) warmStart(s *solver) { + stateA, stateB := s.state(j.indexA), s.state(j.indexB) + rA, rB := j.currentAnchors(stateA, stateB) + applyLinear(stateA, stateB, rA, rB, j.linearImpulse) + applyAngular(stateA, stateB, j.driveImpulse.Add(j.twistAxis.Mul(j.twistLowerImpulse-j.twistUpperImpulse)).Sub(j.swingAxis.Mul(j.swingImpulse))) +} + +func (j *BallJoint) solve(s *solver, useBias bool) { + stateA, stateB := s.state(j.indexA), s.state(j.indexB) + frameA, frameB := j.currentFrames(stateA, stateB) + + // ========== DRIVE ========== + if j.EnableDrive && j.DriveHertz > 0 { + c := rotationError(frameB, actor.MulQuat(&frameA, &j.DriveTarget)) + cdot := stateB.angularVelocity.Sub(stateA.angularVelocity) + k := actor.Add3(&stateA.inverseInertia, &stateB.inverseInertia) + if math.Abs(actor.Det3(&k)) > 1e-30 { + inverse := actor.Inv3(&k) + impulse := j.driveRow.impulse3(&inverse, cdot, c.Mul(j.driveRow.biasRate), j.driveImpulse) + j.driveImpulse = j.driveImpulse.Add(impulse) + applyAngular(stateA, stateB, impulse) + } + } + + // ========== TWIST LIMITS ========== + if j.EnableTwistLimit { + twist, axis, rate := twistRow(frameA, frameB) + j.twistAxis = axis + j.twistLowerImpulse = j.solveAngularLimit(s, stateA, stateB, j.twistAxis, (twist-j.TwistMin)/rate, 1, j.twistLowerImpulse, useBias) + j.twistUpperImpulse = j.solveAngularLimit(s, stateA, stateB, j.twistAxis, (j.TwistMax-twist)/rate, -1, j.twistUpperImpulse, useBias) + } + + // ========== SWING LIMIT (elliptic cone) ========== + if j.EnableSwingLimit { + p := actor.RotateInverse(&frameA, actor.Rotate(&frameB, mgl64.Vec3{1, 0, 0})) + if c, axis, ok := swingLimit(p, j.SwingLimitY, j.SwingLimitZ); ok { + j.swingAxis = actor.Rotate(&frameA, axis) + j.swingImpulse = j.solveAngularLimit(s, stateA, stateB, j.swingAxis, c, -1, j.swingImpulse, useBias) + } + } + + // ========== POINT ========== + j.solvePoint(s, stateA, stateB, useBias) +} + +// ========== Hinge ========== + +// HingeJoint keeps the anchors together, the bodies rotate around the X axis of the frames only (a door, a wheel). +// Optional: an angle range, a motor (speed and max torque), a spring towards a target angle. +type HingeJoint struct { + JointBase + + EnableLimit bool + LowerAngle float64 // rad + UpperAngle float64 // rad + + EnableMotor bool + MotorSpeed float64 // rad/s + MaxMotorTorque float64 // N·m + + EnableSpring bool + TargetAngle float64 // rad + SpringHertz float64 + SpringDampingRatio float64 + + springRow spring + angularImpulse mgl64.Vec3 // keeps the axes aligned + lowerImpulse float64 + upperImpulse float64 + motorImpulse float64 + springImpulse float64 + axis mgl64.Vec3 +} + +// NewHingeJoint links 2 bodies at an anchor, rotating around an axis (world space) +func NewHingeJoint(bodyA, bodyB *actor.RigidBody, anchor, axis mgl64.Vec3) *HingeJoint { + return &HingeJoint{JointBase: newJointBase(bodyA, bodyB, anchor, frameFromAxis(axis))} +} + +func (j *HingeJoint) prepare(s *solver) { + j.prepareBase(s) + if j.SpringHertz > 0 { + j.springRow = newSpring(j.SpringHertz, j.SpringDampingRatio, s.h) + } + j.axis = j.frameA.Rotate(mgl64.Vec3{1, 0, 0}) +} + +// Angle of the frame B around the axis, relative to the frame A +func (j *HingeJoint) Angle() float64 { + frameA := j.BodyA.Transform.Rotation.Mul(j.LocalFrameA.Rotation) + frameB := j.BodyB.Transform.Rotation.Mul(j.LocalFrameB.Rotation) + return hingeAngle(frameA, frameB) +} + +func hingeAngle(frameA, frameB mgl64.Quat) float64 { + axis := frameA.Rotate(mgl64.Vec3{1, 0, 0}) + yA, yB := frameA.Rotate(mgl64.Vec3{0, 1, 0}), frameB.Rotate(mgl64.Vec3{0, 1, 0}) + return math.Atan2(axis.Dot(yA.Cross(yB)), yA.Dot(yB)) +} + +func (j *HingeJoint) warmStart(s *solver) { + stateA, stateB := s.state(j.indexA), s.state(j.indexB) + rA, rB := j.currentAnchors(stateA, stateB) + applyLinear(stateA, stateB, rA, rB, j.linearImpulse) + axial := j.springImpulse + j.motorImpulse + j.lowerImpulse - j.upperImpulse + applyAngular(stateA, stateB, j.angularImpulse.Add(j.axis.Mul(axial))) +} + +func (j *HingeJoint) solve(s *solver, useBias bool) { + stateA, stateB := s.state(j.indexA), s.state(j.indexB) + frameA, frameB := j.currentFrames(stateA, stateB) + j.axis = actor.Rotate(&frameA, mgl64.Vec3{1, 0, 0}) + angle := hingeAngle(frameA, frameB) + mass := axialMass(stateA, stateB, j.axis) + cdot := func() float64 { return j.axis.Dot(stateB.angularVelocity.Sub(stateA.angularVelocity)) } + + // ========== SPRING ========== + if j.EnableSpring && j.SpringHertz > 0 { + c := math.Remainder(angle-j.TargetAngle, 2*math.Pi) + impulse := j.springRow.impulse(mass, cdot(), j.springRow.biasRate*c, j.springImpulse) + j.springImpulse += impulse + applyAngular(stateA, stateB, j.axis.Mul(impulse)) + } + + // ========== MOTOR ========== + if j.EnableMotor { + impulse := -mass * (cdot() - j.MotorSpeed) + maxImpulse := s.h * j.MaxMotorTorque + old := j.motorImpulse + j.motorImpulse = math.Max(-maxImpulse, math.Min(maxImpulse, old+impulse)) + applyAngular(stateA, stateB, j.axis.Mul(j.motorImpulse-old)) + } + + // ========== LIMITS ========== + if j.EnableLimit { + j.lowerImpulse = j.solveAngularLimit(s, stateA, stateB, j.axis, angle-j.LowerAngle, 1, j.lowerImpulse, useBias) + j.upperImpulse = j.solveAngularLimit(s, stateA, stateB, j.axis, j.UpperAngle-angle, -1, j.upperImpulse, useBias) + } + + // ========== AXIS: B turns only around the axis of A ========== + { + u1, u2 := actor.Rotate(&frameA, mgl64.Vec3{0, 1, 0}), actor.Rotate(&frameA, mgl64.Vec3{0, 0, 1}) + axisError := j.axis.Cross(actor.Rotate(&frameB, mgl64.Vec3{1, 0, 0})) + relative := stateB.angularVelocity.Sub(stateA.angularVelocity) + k := actor.Add3(&stateA.inverseInertia, &stateB.inverseInertia) + k11, k12, k22 := u1.Dot(actor.MulMat3(&k, u1)), u1.Dot(actor.MulMat3(&k, u2)), u2.Dot(actor.MulMat3(&k, u2)) + det := k11*k22 - k12*k12 + if det > 1e-30 { + bias1, bias2, row := 0.0, 0.0, rigid + if useBias { + row = j.spring + bias1, bias2 = row.biasRate*u1.Dot(axisError), row.biasRate*u2.Dot(axisError) + } + b1, b2 := u1.Dot(relative)+bias1, u2.Dot(relative)+bias2 + // solve the 2x2 system + l1 := (k22*b1 - k12*b2) / det + l2 := (k11*b2 - k12*b1) / det + accumulated1, accumulated2 := j.angularImpulse.Dot(u1), j.angularImpulse.Dot(u2) + // λ = -(K⁻¹ (v + b) + gamma * accumulated) / (1 + gamma), on both axes + scale := -1 / (1 + row.gamma) + impulse := u1.Mul(scale * (l1 + row.gamma*accumulated1)).Add(u2.Mul(scale * (l2 + row.gamma*accumulated2))) + j.angularImpulse = j.angularImpulse.Add(impulse) + applyAngular(stateA, stateB, impulse) + } + } + + // ========== POINT ========== + j.solvePoint(s, stateA, stateB, useBias) +} + +// ========== Fixed ========== + +// FixedJoint freezes the position and the rotation of B relative to A +type FixedJoint struct { + JointBase + angularImpulse mgl64.Vec3 +} + +// NewFixedJoint welds 2 bodies at an anchor (world space) +func NewFixedJoint(bodyA, bodyB *actor.RigidBody, anchor mgl64.Vec3) *FixedJoint { + return &FixedJoint{JointBase: newJointBase(bodyA, bodyB, anchor, mgl64.QuatIdent())} +} + +func (j *FixedJoint) prepare(s *solver) { j.prepareBase(s) } + +func (j *FixedJoint) warmStart(s *solver) { + stateA, stateB := s.state(j.indexA), s.state(j.indexB) + rA, rB := j.currentAnchors(stateA, stateB) + applyLinear(stateA, stateB, rA, rB, j.linearImpulse) + applyAngular(stateA, stateB, j.angularImpulse) +} + +func (j *FixedJoint) solve(s *solver, useBias bool) { + stateA, stateB := s.state(j.indexA), s.state(j.indexB) + + // ========== ANGULAR ========== + frameA, frameB := j.currentFrames(stateA, stateB) + cdot := stateB.angularVelocity.Sub(stateA.angularVelocity) + bias := mgl64.Vec3{} + row := rigid + if useBias { + row = j.spring + bias = rotationError(frameB, frameA).Mul(row.biasRate) + } + k := actor.Add3(&stateA.inverseInertia, &stateB.inverseInertia) + if math.Abs(actor.Det3(&k)) > 1e-30 { + inverse := actor.Inv3(&k) + impulse := row.impulse3(&inverse, cdot, bias, j.angularImpulse) + j.angularImpulse = j.angularImpulse.Add(impulse) + applyAngular(stateA, stateB, impulse) + } + + // ========== POINT ========== + j.solvePoint(s, stateA, stateB, useBias) +} diff --git a/joint_configurable.go b/joint_configurable.go new file mode 100644 index 0000000..39476c1 --- /dev/null +++ b/joint_configurable.go @@ -0,0 +1,267 @@ +package feather + +import ( + "math" + + "github.com/akmonengine/feather/actor" + "github.com/go-gl/mathgl/mgl64" +) + +// Motion of an axis of a ConfigurableJoint +type Motion int + +const ( + MotionLocked Motion = iota + MotionLimited + MotionFree +) + +// ConfigurableJoint sets each of the 6 axes of the frame A (like the D6 joint of PhysX): locked, limited or free. +// - linear X, Y, Z: the position of the anchor B along the axes of the frame A +// - twist: rotation around X, swing Y & swing Z: rotation of the X axis around Y and Z. +// If both swings are limited, they form an elliptic cone. +// Optional drives: towards a target position (in the frame A) and a target rotation (of the frame B relative to A). +// With everything locked it is a fixed joint, with only the twist free a hinge, with the linear locked a ball joint... +type ConfigurableJoint struct { + JointBase + + LinearMotion [3]Motion + LinearMin mgl64.Vec3 // m, along X, Y, Z of the frame A + LinearMax mgl64.Vec3 + + TwistMotion Motion + TwistMin float64 // rad + TwistMax float64 + + SwingYMotion Motion + SwingZMotion Motion + SwingLimitY float64 // rad, half angle of the rotation of X around Y + SwingLimitZ float64 // rad, half angle of the rotation of X around Z + + EnableLinearDrive bool + DriveTargetPosition mgl64.Vec3 // in the frame A + LinearDriveHertz float64 + LinearDriveDampingRatio float64 + + EnableAngularDrive bool + DriveTargetRotation mgl64.Quat // rotation of the frame B relative to the frame A + AngularDriveHertz float64 + AngularDriveDampingRatio float64 + + linearDriveRow spring + angularDriveRow spring + // accumulated impulses, per axis: [0] for a locked axis or a lower limit, [1] for an upper limit + linearImpulses [3][2]float64 + linearDriveImpulses [3]float64 + twistImpulses [2]float64 + swingImpulses [2][2]float64 + coneImpulse float64 + angularImpulse mgl64.Vec3 + angularDriveImpulse mgl64.Vec3 + // axes of the last solve, for the warm starting + linearAxes [3]mgl64.Vec3 + twistAxis mgl64.Vec3 + swingAxes [2]mgl64.Vec3 + coneAxis mgl64.Vec3 +} + +// NewConfigurableJoint links 2 bodies at an anchor (world space), X of the frames along the axis. +// Everything is locked by default: set the motion of each axis +func NewConfigurableJoint(bodyA, bodyB *actor.RigidBody, anchor, axis mgl64.Vec3) *ConfigurableJoint { + return &ConfigurableJoint{ + JointBase: newJointBase(bodyA, bodyB, anchor, frameFromAxis(axis)), + DriveTargetRotation: mgl64.QuatIdent(), + } +} + +func (j *ConfigurableJoint) prepare(s *solver) { + j.prepareBase(s) + if j.EnableLinearDrive && j.LinearDriveHertz > 0 { + j.linearDriveRow = newSpring(j.LinearDriveHertz, j.LinearDriveDampingRatio, s.h) + } + if j.EnableAngularDrive && j.AngularDriveHertz > 0 { + j.angularDriveRow = newSpring(j.AngularDriveHertz, j.AngularDriveDampingRatio, s.h) + } +} + +func (j *ConfigurableJoint) allLinearLocked() bool { + return j.LinearMotion[0] == MotionLocked && j.LinearMotion[1] == MotionLocked && j.LinearMotion[2] == MotionLocked +} + +func (j *ConfigurableJoint) allAngularLocked() bool { + return j.TwistMotion == MotionLocked && j.SwingYMotion == MotionLocked && j.SwingZMotion == MotionLocked +} + +// linearState: the anchors, and the offset of the anchor B from the anchor A (world space) +func (j *ConfigurableJoint) linearState(stateA, stateB *bodyState) (mgl64.Vec3, mgl64.Vec3, mgl64.Vec3) { + rA, rB := j.currentAnchors(stateA, stateB) + offset := j.deltaCenter.Add(stateB.deltaPosition.Sub(stateA.deltaPosition)).Add(rB.Sub(rA)) + return rA, rB, offset +} + +func (j *ConfigurableJoint) warmStart(s *solver) { + stateA, stateB := s.state(j.indexA), s.state(j.indexB) + rA, rB, offset := j.linearState(stateA, stateB) + + // ========== LINEAR ========== + if j.allLinearLocked() { + applyLinear(stateA, stateB, rA, rB, j.linearImpulse) + } else { + for k := range j.linearAxes { + impulse := j.linearImpulses[k][0] - j.linearImpulses[k][1] + j.linearDriveImpulses[k] + applyLinear(stateA, stateB, rA.Add(offset), rB, j.linearAxes[k].Mul(impulse)) + } + } + + // ========== ANGULAR ========== + angular := j.angularDriveImpulse.Add(j.angularImpulse).Add(j.twistAxis.Mul(j.twistImpulses[0] - j.twistImpulses[1])) + angular = angular.Sub(j.coneAxis.Mul(j.coneImpulse)) + for k := range j.swingAxes { + angular = angular.Add(j.swingAxes[k].Mul(j.swingImpulses[k][0] - j.swingImpulses[k][1])) + } + applyAngular(stateA, stateB, angular) +} + +func (j *ConfigurableJoint) solve(s *solver, useBias bool) { + stateA, stateB := s.state(j.indexA), s.state(j.indexB) + frameA, frameB := j.currentFrames(stateA, stateB) + + // ========== ANGULAR DRIVE ========== + if j.EnableAngularDrive && j.AngularDriveHertz > 0 { + c := rotationError(frameB, actor.MulQuat(&frameA, &j.DriveTargetRotation)) + j.angularDriveImpulse = solveAngular3(stateA, stateB, c, j.angularDriveRow, true, j.angularDriveImpulse) + } + + // ========== ANGULAR ========== + if j.allAngularLocked() { + j.angularImpulse = solveAngular3(stateA, stateB, rotationError(frameB, frameA), j.spring, useBias, j.angularImpulse) + } else { + j.solveTwist(s, stateA, stateB, frameA, frameB, useBias) + j.solveSwing(s, stateA, stateB, frameA, frameB, useBias) + } + + // ========== LINEAR DRIVE ========== + rA, rB, offset := j.linearState(stateA, stateB) + if j.EnableLinearDrive && j.LinearDriveHertz > 0 { + for k := 0; k < 3; k++ { + if j.LinearMotion[k] == MotionLocked { + continue + } + axis := actor.Rotate(&frameA, unitAxes[k]) + c := offset.Dot(axis) - j.DriveTargetPosition[k] + drive := j.linearDriveRow + j.linearDriveImpulses[k] = solveLinearAxis(stateA, stateB, rA.Add(offset), rB, axis, drive.biasRate*c, drive, j.linearDriveImpulses[k], math.Inf(-1), math.Inf(1)) + } + } + + // ========== LINEAR ========== + if j.allLinearLocked() { + j.solvePoint(s, stateA, stateB, useBias) + return + } + for k := 0; k < 3; k++ { + axis := actor.Rotate(&frameA, unitAxes[k]) + j.linearAxes[k] = axis + position := offset.Dot(axis) + switch j.LinearMotion[k] { + case MotionLocked: + bias, row := j.equalityRow(useBias, position) + j.linearImpulses[k][0] = solveLinearAxis(stateA, stateB, rA.Add(offset), rB, axis, bias, row, j.linearImpulses[k][0], math.Inf(-1), math.Inf(1)) + case MotionLimited: + // lower: position - min >= 0, upper: max - position >= 0 (along -axis) + bias, row := j.limitRow(s, position-j.LinearMin[k], useBias) + j.linearImpulses[k][0] = solveLinearAxis(stateA, stateB, rA.Add(offset), rB, axis, bias, row, j.linearImpulses[k][0], 0, math.Inf(1)) + bias, row = j.limitRow(s, j.LinearMax[k]-position, useBias) + j.linearImpulses[k][1] = solveLinearAxis(stateA, stateB, rA.Add(offset), rB, axis.Mul(-1), bias, row, j.linearImpulses[k][1], 0, math.Inf(1)) + } + } +} + +var unitAxes = [3]mgl64.Vec3{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}} + +// equalityRow: the bias and the spring of an equality constraint C = 0, soft when useBias +func (j *JointBase) equalityRow(useBias bool, c float64) (float64, spring) { + if !useBias { + return 0, rigid + } + return j.spring.biasRate * c, j.spring +} + +func (j *ConfigurableJoint) solveTwist(s *solver, stateA, stateB *bodyState, frameA, frameB mgl64.Quat, useBias bool) { + if j.TwistMotion == MotionFree { + return + } + twist, axis, rate := twistRow(frameA, frameB) + j.twistAxis = axis + if j.TwistMotion == MotionLocked { + j.twistImpulses[0] = j.solveAngularEquality(stateA, stateB, j.twistAxis, twist/rate, 1, j.twistImpulses[0], useBias) + return + } + j.twistImpulses[0] = j.solveAngularLimit(s, stateA, stateB, j.twistAxis, (twist-j.TwistMin)/rate, 1, j.twistImpulses[0], useBias) + j.twistImpulses[1] = j.solveAngularLimit(s, stateA, stateB, j.twistAxis, (j.TwistMax-twist)/rate, -1, j.twistImpulses[1], useBias) +} + +// swingAngles: rotation of the X axis of B around Y and around Z, from p (X of B seen in the frame A) +var swingAngles = [2]func(p mgl64.Vec3) float64{ + func(p mgl64.Vec3) float64 { return math.Atan2(-p.Z(), p.X()) }, + func(p mgl64.Vec3) float64 { return math.Atan2(p.Y(), p.X()) }, +} + +func (j *ConfigurableJoint) solveSwing(s *solver, stateA, stateB *bodyState, frameA, frameB mgl64.Quat, useBias bool) { + p := actor.RotateInverse(&frameA, actor.Rotate(&frameB, mgl64.Vec3{1, 0, 0})) + + // both limited: elliptic cone + if j.SwingYMotion == MotionLimited && j.SwingZMotion == MotionLimited { + if c, axis, ok := swingLimit(p, j.SwingLimitY, j.SwingLimitZ); ok { + j.coneAxis = actor.Rotate(&frameA, axis) + j.coneImpulse = j.solveAngularLimit(s, stateA, stateB, j.coneAxis, c, -1, j.coneImpulse, useBias) + } + return + } + + motions := [2]Motion{j.SwingYMotion, j.SwingZMotion} + limits := [2]float64{j.SwingLimitY, j.SwingLimitZ} + for k := 0; k < 2; k++ { + if motions[k] == MotionFree { + continue + } + angle, axis, rate, ok := angleRow(p, swingAngles[k]) + if !ok { + continue + } + j.swingAxes[k] = actor.Rotate(&frameA, axis) + // the angle changes by rate per unit of angular velocity around the axis: the constraints are in angle / rate + if motions[k] == MotionLocked { + j.swingImpulses[k][0] = j.solveAngularEquality(stateA, stateB, j.swingAxes[k], angle/rate, 1, j.swingImpulses[k][0], useBias) + continue + } + j.swingImpulses[k][0] = j.solveAngularLimit(s, stateA, stateB, j.swingAxes[k], (angle+limits[k])/rate, 1, j.swingImpulses[k][0], useBias) + j.swingImpulses[k][1] = j.solveAngularLimit(s, stateA, stateB, j.swingAxes[k], (limits[k]-angle)/rate, -1, j.swingImpulses[k][1], useBias) + } +} + +// solveAngularEquality: C = 0 around the axis (a locked rotation), soft when useBias +func (j *JointBase) solveAngularEquality(stateA, stateB *bodyState, axis mgl64.Vec3, c float64, direction float64, accumulated float64, useBias bool) float64 { + bias, row := j.equalityRow(useBias, c) + cdot := direction * axis.Dot(stateB.angularVelocity.Sub(stateA.angularVelocity)) + impulse := row.impulse(axialMass(stateA, stateB, axis), cdot, bias, accumulated) + applyAngular(stateA, stateB, axis.Mul(direction*impulse)) + return accumulated + impulse +} + +// solveAngular3: the 3 rotations together (K = IA + IB), towards the error c (rotation vector, world space) +func solveAngular3(stateA, stateB *bodyState, c mgl64.Vec3, soft spring, useBias bool, accumulated mgl64.Vec3) mgl64.Vec3 { + cdot := stateB.angularVelocity.Sub(stateA.angularVelocity) + bias, row := mgl64.Vec3{}, rigid + if useBias { + bias, row = c.Mul(soft.biasRate), soft + } + k := actor.Add3(&stateA.inverseInertia, &stateB.inverseInertia) + if math.Abs(actor.Det3(&k)) < 1e-30 { + return accumulated + } + inverse := actor.Inv3(&k) + impulse := row.impulse3(&inverse, cdot, bias, accumulated) + applyAngular(stateA, stateB, impulse) + return accumulated.Add(impulse) +} diff --git a/joint_configurable_test.go b/joint_configurable_test.go new file mode 100644 index 0000000..90bd62d --- /dev/null +++ b/joint_configurable_test.go @@ -0,0 +1,184 @@ +package feather + +import ( + "math" + "testing" + + "github.com/akmonengine/feather/actor" + "github.com/go-gl/mathgl/mgl64" +) + +// A slider: the block only slides along X of the frame (on a 30° slope, pulled by gravity), and stops at the limits +func TestConfigurableSlider(t *testing.T) { + w := newScene(1) + rail := anchorBody(w, mgl64.Vec3{0, 3, 0}) + slope := mgl64.Vec3{math.Cos(math.Pi / 6), -math.Sin(math.Pi / 6), 0} + block := addBody(w, mgl64.Vec3{0, 3, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.5, 0) + slider := NewConfigurableJoint(rail, block, mgl64.Vec3{0, 3, 0}, slope) + slider.LinearMotion = [3]Motion{MotionLimited, MotionLocked, MotionLocked} + slider.LinearMin, slider.LinearMax = mgl64.Vec3{-0.5, 0, 0}, mgl64.Vec3{1.5, 0, 0} + w.AddJoint(slider) + + worstOff, worstBeyond, worstTurn := 0.0, 0.0, 0.0 + simulate(w, 3, func() { + offset := block.Transform.Position.Sub(mgl64.Vec3{0, 3, 0}) + along := offset.Dot(slope) + worstOff = math.Max(worstOff, offset.Sub(slope.Mul(along)).Len()) + worstBeyond = math.Max(worstBeyond, along-1.5) + worstTurn = math.Max(worstTurn, 2*math.Acos(math.Min(1, math.Abs(block.Transform.Rotation.W)))) + }) + along := block.Transform.Position.Sub(mgl64.Vec3{0, 3, 0}).Dot(slope) + t.Logf("stopped at %.4f m (limit 1.5), off the axis %.3f mm, beyond the limit %.3f mm, turned %.3f°", along, worstOff*1000, worstBeyond*1000, degrees(worstTurn)) + if math.Abs(along-1.5) > 0.002 || worstOff > 0.001 || worstBeyond > 0.002 || degrees(worstTurn) > 0.5 { + t.Error("the slider did not hold") + } +} + +// The configurable joint set as a hinge (only the twist free) keeps its axis like HingeJoint +func TestConfigurableHinge(t *testing.T) { + w := newScene(1) + w.Gravity = mgl64.Vec3{} + frame := anchorBody(w, mgl64.Vec3{0, 1, 0}) + door := addBody(w, mgl64.Vec3{0.5, 1, 0}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{0.5, 1, 0.05}}, actor.BodyTypeDynamic, 0.5, 0) + hinge := NewConfigurableJoint(frame, door, mgl64.Vec3{0, 1, 0}, mgl64.Vec3{0, 1, 0}) + hinge.TwistMotion = MotionFree + w.AddJoint(hinge) + + worstAxis := 0.0 + simulate(w, 2, func() { + door.AddTorque(mgl64.Vec3{100, 60, 100}) + axis := door.Transform.Rotation.Mul(hinge.LocalFrameB.Rotation).Rotate(mgl64.Vec3{1, 0, 0}) + worstAxis = math.Max(worstAxis, math.Acos(math.Min(1, axis.Dot(mgl64.Vec3{0, 1, 0})))) + }) + t.Logf("axis tilt %.3f°, spin %.2f rad/s", degrees(worstAxis), door.AngularVelocity.Y()) + if degrees(worstAxis) > 0.5 || door.AngularVelocity.Y() < 1 { + t.Errorf("axis tilt %.2f°, spin %.2f rad/s", degrees(worstAxis), door.AngularVelocity.Y()) + } +} + +// One swing limited, the other locked, the twist locked: the arm only swings around Y, within its limit +func TestConfigurableSwingAxes(t *testing.T) { + w := newScene(1) + w.Gravity = mgl64.Vec3{} + pivot := mgl64.Vec3{0, 2, 0} + anchor := anchorBody(w, pivot) + arm := addBody(w, pivot.Add(mgl64.Vec3{0, -0.5, 0}), mgl64.QuatIdent(), &actor.Capsule{HalfHeight: 0.4, Radius: 0.05}, actor.BodyTypeDynamic, 0.5, 0) + joint := NewConfigurableJoint(anchor, arm, pivot, mgl64.Vec3{0, -1, 0}) + joint.SwingYMotion, joint.SwingLimitY = MotionLimited, 20*math.Pi/180 + w.AddJoint(joint) + + worstY, worstZ, worstTwist := 0.0, 0.0, 0.0 + for _, push := range []mgl64.Vec3{{40, 0, 0}, {-40, 0, 0}, {0, 0, 40}, {0, 0, -40}, {30, 0, 30}} { + simulate(w, 1, func() { + arm.AddForceAtPoint(push, arm.Transform.ToWorld(mgl64.Vec3{0, -0.45, 0})) + arm.AddTorque(arm.Transform.Rotation.Rotate(mgl64.Vec3{0, 2, 0})) + frameA := anchor.Transform.Rotation.Mul(joint.LocalFrameA.Rotation) + frameB := arm.Transform.Rotation.Mul(joint.LocalFrameB.Rotation) + p := frameA.Conjugate().Rotate(frameB.Rotate(mgl64.Vec3{1, 0, 0})) + worstY = math.Max(worstY, math.Abs(swingAngles[0](p))-joint.SwingLimitY) + worstZ = math.Max(worstZ, math.Abs(swingAngles[1](p))) + worstTwist = math.Max(worstTwist, math.Abs(twistAngle(frameA, frameB))) + }) + } + t.Logf("swing Y beyond its limit %.3f°, swing Z %.3f° (locked), twist %.3f° (locked)", degrees(worstY), degrees(worstZ), degrees(worstTwist)) + if degrees(worstY) > 0.5 || degrees(worstZ) > 0.5 || degrees(worstTwist) > 0.5 { + t.Error("the swing axes did not hold") + } +} + +// Both swings limited: the elliptic cone, like the ball joint +func TestConfigurableCone(t *testing.T) { + w := newScene(1) + w.Gravity = mgl64.Vec3{} + pivot := mgl64.Vec3{0, 2, 0} + anchor := anchorBody(w, pivot) + arm := addBody(w, pivot.Add(mgl64.Vec3{0, -0.5, 0}), mgl64.QuatIdent(), &actor.Capsule{HalfHeight: 0.4, Radius: 0.05}, actor.BodyTypeDynamic, 0.5, 0) + joint := NewConfigurableJoint(anchor, arm, pivot, mgl64.Vec3{0, -1, 0}) + joint.SwingYMotion, joint.SwingZMotion, joint.SwingLimitY, joint.SwingLimitZ = MotionLimited, MotionLimited, 20*math.Pi/180, 40*math.Pi/180 + joint.TwistMotion, joint.TwistMin, joint.TwistMax = MotionLimited, -0.2, 0.2 + w.AddJoint(joint) + + // 25 N at the tip is about 5 g: harder pushes bend the soft limits further + worstSwing, worstTwist := 0.0, 0.0 + for _, push := range []mgl64.Vec3{{25, 0, 0}, {-25, 0, 0}, {0, 0, 25}, {18, 0, 18}} { + simulate(w, 1, func() { + arm.AddForceAtPoint(push, arm.Transform.ToWorld(mgl64.Vec3{0, -0.45, 0})) + arm.AddTorque(arm.Transform.Rotation.Rotate(mgl64.Vec3{0, 3, 0})) + frameA := anchor.Transform.Rotation.Mul(joint.LocalFrameA.Rotation) + frameB := arm.Transform.Rotation.Mul(joint.LocalFrameB.Rotation) + p := frameA.Conjugate().Rotate(frameB.Rotate(mgl64.Vec3{1, 0, 0})) + if r := math.Hypot(p.Y(), p.Z()); r > 1e-9 { + worstSwing = math.Max(worstSwing, math.Atan2(r, p.X())-1/math.Hypot(p.Y()/r/joint.SwingLimitZ, p.Z()/r/joint.SwingLimitY)) + } + twist := twistAngle(frameA, frameB) + worstTwist = math.Max(worstTwist, math.Max(joint.TwistMin-twist, twist-joint.TwistMax)) + }) + } + t.Logf("cone overshoot %.3f°, twist overshoot %.3f°", degrees(worstSwing), degrees(worstTwist)) + if degrees(worstSwing) > 0.5 || degrees(worstTwist) > 0.5 { + t.Error("the cone did not hold") + } +} + +// Everything locked: a fixed joint +func TestConfigurableLocked(t *testing.T) { + w := newScene(1) + w.Gravity = mgl64.Vec3{} + a := addBody(w, mgl64.Vec3{0, 0, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.5, 0) + b := addBody(w, mgl64.Vec3{0.5, 0, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.5, 0) + w.AddJoint(NewConfigurableJoint(a, b, mgl64.Vec3{0.25, 0, 0}, mgl64.Vec3{1, 0, 0})) + a.AddImpulseAtPoint(mgl64.Vec3{0, 20, 0}, mgl64.Vec3{-0.25, 0, 0.2}) + worstPosition, worstAngle := 0.0, 0.0 + simulate(w, 3, func() { + worstPosition = math.Max(worstPosition, a.Transform.ToLocal(b.Transform.Position).Sub(mgl64.Vec3{0.5, 0, 0}).Len()) + rotation := a.Transform.Rotation.Conjugate().Mul(b.Transform.Rotation) + worstAngle = math.Max(worstAngle, 2*math.Acos(math.Min(1, math.Abs(rotation.W)))) + }) + t.Logf("worst %.3f mm, %.3f°", worstPosition*1000, degrees(worstAngle)) + if worstPosition > 0.001 || degrees(worstAngle) > 0.5 { + t.Errorf("moved by %.2f mm and %.2f°", worstPosition*1000, degrees(worstAngle)) + } +} + +// Drives: the linear drive brings the body to its target position, the angular drive to its target rotation +func TestConfigurableDrives(t *testing.T) { + w := newScene(1) + w.Gravity = mgl64.Vec3{} + base := anchorBody(w, mgl64.Vec3{}) + body := addBody(w, mgl64.Vec3{}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.5, 0) + joint := NewConfigurableJoint(base, body, mgl64.Vec3{}, mgl64.Vec3{1, 0, 0}) + joint.LinearMotion = [3]Motion{MotionFree, MotionFree, MotionFree} + joint.TwistMotion, joint.SwingYMotion, joint.SwingZMotion = MotionFree, MotionFree, MotionFree + joint.EnableLinearDrive, joint.DriveTargetPosition, joint.LinearDriveHertz, joint.LinearDriveDampingRatio = true, mgl64.Vec3{0.3, -0.2, 0.1}, 2, 1 + target := mgl64.QuatRotate(1, mgl64.Vec3{1, 1, 0}.Normalize()) + joint.EnableAngularDrive, joint.DriveTargetRotation, joint.AngularDriveHertz, joint.AngularDriveDampingRatio = true, target, 2, 1 + w.AddJoint(joint) + simulate(w, 3, nil) + positionError := body.Transform.Position.Sub(mgl64.Vec3{0.3, -0.2, 0.1}).Len() + angleError := 2 * math.Acos(math.Min(1, math.Abs(body.Transform.Rotation.Dot(target)))) + t.Logf("position error %.3f mm, rotation error %.3f°", positionError*1000, degrees(angleError)) + if positionError > 0.001 || degrees(angleError) > 0.5 { + t.Error("the drives did not reach their targets") + } +} + +func TestConfigurableDoesNotAllocate(t *testing.T) { + if raceEnabled { + t.Skip("sync.Pool drops its items with the race detector") + } + w := newScene(1) + pivot := mgl64.Vec3{0, 2, 0} + previous := anchorBody(w, pivot) + for i := 0; i < 5; i++ { + link := addBody(w, pivot.Sub(mgl64.Vec3{0, float64(i)*0.5 + 0.25, 0}), mgl64.QuatIdent(), &actor.Capsule{HalfHeight: 0.2, Radius: 0.05}, actor.BodyTypeDynamic, 0.5, 0) + joint := NewConfigurableJoint(previous, link, pivot.Sub(mgl64.Vec3{0, float64(i) * 0.5, 0}), mgl64.Vec3{0, -1, 0}) + joint.SwingYMotion, joint.SwingZMotion, joint.SwingLimitY, joint.SwingLimitZ = MotionLimited, MotionLimited, 0.5, 0.5 + joint.TwistMotion, joint.TwistMin, joint.TwistMax = MotionLimited, -0.2, 0.2 + w.AddJoint(joint) + previous = link + } + simulate(w, 0.2, nil) + if allocs := testing.AllocsPerRun(10, func() { w.Step(sceneDt) }); allocs > 0 { + t.Errorf("%.1f allocations per step", allocs) + } +} diff --git a/joint_test.go b/joint_test.go new file mode 100644 index 0000000..915b2eb --- /dev/null +++ b/joint_test.go @@ -0,0 +1,387 @@ +package feather + +import ( + "math" + "math/rand" + "testing" + + "github.com/akmonengine/feather/actor" + "github.com/go-gl/mathgl/mgl64" +) + +func anchorBody(w *World, position mgl64.Vec3) *actor.RigidBody { + return addBody(w, position, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.02}, actor.BodyTypeStatic, 0, 0) +} + +func degrees(radians float64) float64 { return radians * 180 / math.Pi } + +// A pendulum: the period of a physical pendulum, 2π sqrt(I / (m g L)), within 1% (small angles) +func TestJointPendulumPeriod(t *testing.T) { + const length, radius, angle = 1.0, 0.1, 0.1 + w := newScene(1) + pivot := mgl64.Vec3{0, 2, 0} + anchor := anchorBody(w, pivot) + bob := addBody(w, pivot.Add(mgl64.Vec3{length * math.Sin(angle), -length * math.Cos(angle), 0}), mgl64.QuatIdent(), &actor.Sphere{Radius: radius}, actor.BodyTypeDynamic, 0, 0) + w.AddJoint(NewBallJoint(anchor, bob, pivot, mgl64.Vec3{0, -1, 0})) + + // zero crossings of x, going to the right + var crossings []float64 + elapsed, previous := 0.0, bob.Transform.Position.X() + simulate(w, 12, func() { + elapsed += sceneDt + x := bob.Transform.Position.X() + if previous < 0 && x >= 0 { + crossings = append(crossings, elapsed-sceneDt*x/(x-previous)) + } + previous = x + }) + if len(crossings) < 4 { + t.Fatalf("only %d crossings", len(crossings)) + } + period := (crossings[len(crossings)-1] - crossings[0]) / float64(len(crossings)-1) + inertia := 0.4*radius*radius + length*length + want := 2 * math.Pi * math.Sqrt(inertia/(sceneGravity*length)) * (1 + angle*angle/16) + t.Logf("period %.4f s, theory %.4f s", period, want) + if math.Abs(period-want) > 0.01*want { + t.Errorf("period %.4f s, want %.4f s (1%%)", period, want) + } + if d := bob.Transform.Position.Sub(pivot).Len(); math.Abs(d-length) > 0.001 { + t.Errorf("the bob is %.4f m from the pivot, want %.4f", d, length) + } +} + +// chain of capsules hanging from an anchor, linked by ball joints +func hangingChain(w *World, links int) (*actor.RigidBody, []*actor.RigidBody, []*BallJoint) { + const halfHeight, radius = 0.2, 0.05 + top := mgl64.Vec3{0, 5, 0} + anchor := anchorBody(w, top) + previous := anchor + var capsules []*actor.RigidBody + var joints []*BallJoint + for i := 0; i < links; i++ { + joint := top.Sub(mgl64.Vec3{0, float64(i) * 2 * halfHeight, 0}) + capsule := addBody(w, joint.Sub(mgl64.Vec3{0, halfHeight, 0}), mgl64.QuatIdent(), &actor.Capsule{HalfHeight: halfHeight, Radius: radius}, actor.BodyTypeDynamic, 0.5, 0) + ball := NewBallJoint(previous, capsule, joint, mgl64.Vec3{0, -1, 0}) + w.AddJoint(ball) + joints = append(joints, ball) + capsules = append(capsules, capsule) + previous = capsule + } + return anchor, capsules, joints +} + +// jointGap: distance between the anchors of both bodies +func jointGap(j *JointBase) float64 { + return j.BodyA.Transform.ToWorld(j.LocalFrameA.Position).Sub(j.BodyB.Transform.ToWorld(j.LocalFrameB.Position)).Len() +} + +// A chain of 5 capsules stays at rest, and doesn't explode when it is shaken +func TestJointChain(t *testing.T) { + w := newScene(1) + _, capsules, joints := hangingChain(w, 5) + simulate(w, 2, nil) + start := make([]mgl64.Vec3, len(capsules)) + for i, c := range capsules { + start[i] = c.Transform.Position + } + simulate(w, 10, nil) + for i, c := range capsules { + if d := c.Transform.Position.Sub(start[i]).Len(); d > 0.0001 { + t.Errorf("capsule %d drifted %.3f mm at rest", i, d*1000) + } + } + + // shaking: random impulses on the whole chain + r := rand.New(rand.NewSource(3)) + worstGap := 0.0 + simulate(w, 3, func() { + for _, c := range capsules { + c.AddImpulse(mgl64.Vec3{r.NormFloat64(), r.NormFloat64(), r.NormFloat64()}.Mul(0.3)) + } + for _, j := range joints { + worstGap = math.Max(worstGap, jointGap(&j.JointBase)) + } + }) + for i, c := range capsules { + if !finite(c.Transform.Position) || c.Velocity.Len() > 50 { + t.Fatalf("capsule %d exploded: position %v velocity %v", i, c.Transform.Position, c.Velocity) + } + } + t.Logf("worst gap in the joints while shaking: %.2f mm", worstGap*1000) + if worstGap > 0.01 { + t.Errorf("the joints opened by %.1f mm while shaking", worstGap*1000) + } + // after the shaking, no damping: the chain keeps swinging, but its energy must not grow + energy := func() float64 { + e := 0.0 + for _, c := range capsules { + m := c.Material.GetMass() + e += 0.5*m*c.Velocity.LenSqr() + 0.5*c.AngularVelocity.Dot(c.GetInertiaWorld().Mul3x1(c.AngularVelocity)) + m*sceneGravity*c.Transform.Position.Y() + } + return e + } + start0 := energy() + worstEnergy := start0 + simulate(w, 5, func() { worstEnergy = math.Max(worstEnergy, energy()) }) + t.Logf("energy after the shaking %.3f J, highest in the next 5 s %.3f J", start0, worstEnergy) + if worstEnergy > start0+0.01*math.Abs(start0) { + t.Errorf("the chain gained energy: %.3f J -> %.3f J", start0, worstEnergy) + } +} + +// A chain of 10 links carrying a ball 100 times heavier, released horizontal: while it swings, no joint opens by more +// than 1 % of a link. Solved one by one, the spring of each joint acts on the mass of a link, and the ball stretches the +// chain (articulation.go) +func TestHeavyChainDoesNotStretch(t *testing.T) { + const links, halfHeight, radius, ratio = 10, 0.2, 0.05, 100 + w := newScene(1) + top := mgl64.Vec3{0, 6, 0} + previous := anchorBody(w, top) + lying := mgl64.QuatRotate(math.Pi/2, mgl64.Vec3{0, 0, 1}) + var joints []*BallJoint + for i := 0; i < links; i++ { + joint := top.Add(mgl64.Vec3{float64(i) * 2 * halfHeight, 0, 0}) + link := addBody(w, joint.Add(mgl64.Vec3{halfHeight, 0, 0}), lying, &actor.Capsule{HalfHeight: halfHeight, Radius: radius}, actor.BodyTypeDynamic, 0.5, 0) + ball := NewBallJoint(previous, link, joint, mgl64.Vec3{1, 0, 0}) + w.AddJoint(ball) + joints = append(joints, ball) + previous = link + } + end := top.Add(mgl64.Vec3{float64(links) * 2 * halfHeight, 0, 0}) + linkMass := previous.Material.GetMass() + const ballRadius = 0.2 + density := ratio * linkMass / (4.0 / 3 * math.Pi * ballRadius * ballRadius * ballRadius) + ball := actor.NewRigidBody(actor.Transform{Position: end.Add(mgl64.Vec3{ballRadius, 0, 0}), Rotation: mgl64.QuatIdent()}, &actor.Sphere{Radius: ballRadius}, actor.BodyTypeDynamic, density) + w.AddBody(ball) + w.AddJoint(NewBallJoint(previous, ball, end, mgl64.Vec3{1, 0, 0})) + joints = append(joints, w.Joints[len(w.Joints)-1].(*BallJoint)) + + worstGap := 0.0 + simulate(w, 4, func() { + for _, j := range joints { + worstGap = math.Max(worstGap, jointGap(&j.JointBase)) + } + }) + t.Logf("ball %.1f kg, link %.2f kg: worst gap %.3f mm", ball.Material.GetMass(), linkMass, worstGap*1000) + if worstGap > 0.01*2*halfHeight { + t.Errorf("a joint opened by %.2f mm, more than 1 %% of a link (%.1f mm)", worstGap*1000, 0.01*2*halfHeight*1000) + } +} + +// A door on a hinge: it only turns around its axis, and stays within its limits, even pushed hard +func TestJointHingeLimits(t *testing.T) { + w := newScene(1) + w.Gravity = mgl64.Vec3{} + frame := anchorBody(w, mgl64.Vec3{0, 1, 0}) + door := addBody(w, mgl64.Vec3{0.5, 1, 0}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{0.5, 1, 0.05}}, actor.BodyTypeDynamic, 0.5, 0) + hinge := NewHingeJoint(frame, door, mgl64.Vec3{0, 1, 0}, mgl64.Vec3{0, 1, 0}) + hinge.EnableLimit, hinge.LowerAngle, hinge.UpperAngle = true, -0.5, 1.0 + w.AddJoint(hinge) + + worstLimit, worstAxis := 0.0, 0.0 + check := func() { + angle := hinge.Angle() + worstLimit = math.Max(worstLimit, math.Max(hinge.LowerAngle-angle, angle-hinge.UpperAngle)) + axis := door.Transform.Rotation.Mul(hinge.LocalFrameB.Rotation).Rotate(mgl64.Vec3{1, 0, 0}) + worstAxis = math.Max(worstAxis, math.Acos(math.Min(1, axis.Dot(mgl64.Vec3{0, 1, 0})))) + } + for _, torque := range []mgl64.Vec3{{0, 200, 0}, {0, -200, 0}, {150, 0, 150}} { + simulate(w, 1.5, func() { + door.AddTorque(torque) + check() + }) + } + t.Logf("worst limit overshoot %.3f°, worst axis tilt %.3f°", degrees(worstLimit), degrees(worstAxis)) + if degrees(worstLimit) > 0.5 { + t.Errorf("the door went %.2f° beyond its limits", degrees(worstLimit)) + } + if degrees(worstAxis) > 0.5 { + t.Errorf("the hinge axis tilted by %.2f°", degrees(worstAxis)) + } +} + +// A ball joint with an elliptic cone and a twist range: never exceeded by more than 0.5° +func TestJointBallLimits(t *testing.T) { + w := newScene(1) + w.Gravity = mgl64.Vec3{} + pivot := mgl64.Vec3{0, 2, 0} + anchor := anchorBody(w, pivot) + arm := addBody(w, pivot.Add(mgl64.Vec3{0, -0.5, 0}), mgl64.QuatIdent(), &actor.Capsule{HalfHeight: 0.4, Radius: 0.05}, actor.BodyTypeDynamic, 0.5, 0) + ball := NewBallJoint(anchor, arm, pivot, mgl64.Vec3{0, -1, 0}) + ball.EnableSwingLimit, ball.SwingLimitY, ball.SwingLimitZ = true, 20*math.Pi/180, 40*math.Pi/180 + ball.EnableTwistLimit, ball.TwistMin, ball.TwistMax = true, -10*math.Pi/180, 10*math.Pi/180 + w.AddJoint(ball) + + worstSwing, worstTwist := 0.0, 0.0 + check := func() { + frameA := anchor.Transform.Rotation.Mul(ball.LocalFrameA.Rotation) + frameB := arm.Transform.Rotation.Mul(ball.LocalFrameB.Rotation) + twist := twistAngle(frameA, frameB) + // the X axis of B, in the frame A: its angle beyond the ellipse, at its direction + p := frameA.Conjugate().Rotate(frameB.Rotate(mgl64.Vec3{1, 0, 0})) + if r := math.Hypot(p.Y(), p.Z()); r > 1e-9 { + angle := math.Atan2(r, p.X()) + limit := 1 / math.Hypot(p.Y()/r/ball.SwingLimitZ, p.Z()/r/ball.SwingLimitY) + worstSwing = math.Max(worstSwing, angle-limit) + } + worstTwist = math.Max(worstTwist, math.Max(ball.TwistMin-twist, twist-ball.TwistMax)) + } + bottom := func() mgl64.Vec3 { return arm.Transform.ToWorld(mgl64.Vec3{0, -0.45, 0}) } + for _, push := range []mgl64.Vec3{{50, 0, 0}, {-50, 0, 0}, {0, 0, 50}, {0, 0, -50}, {35, 0, 35}} { + simulate(w, 1, func() { + arm.AddForceAtPoint(push, bottom()) + arm.AddTorque(arm.Transform.Rotation.Rotate(mgl64.Vec3{0, 3, 0})) + check() + }) + } + simulate(w, 1, func() { + arm.AddTorque(arm.Transform.Rotation.Rotate(mgl64.Vec3{0, -3, 0})) + check() + }) + t.Logf("worst swing overshoot %.3f°, worst twist overshoot %.3f°", degrees(worstSwing), degrees(worstTwist)) + if degrees(worstSwing) > 0.5 || degrees(worstTwist) > 0.5 { + t.Errorf("limits exceeded: swing %.2f°, twist %.2f°", degrees(worstSwing), degrees(worstTwist)) + } +} + +// A distance joint with a spring: the body oscillates at the frequency of the spring +func TestJointSpringFrequency(t *testing.T) { + const hertz = 2.0 + w := newScene(1) + w.Gravity = mgl64.Vec3{} + anchor := anchorBody(w, mgl64.Vec3{0, 0, 0}) + body := addBody(w, mgl64.Vec3{1, 0, 0}, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.1}, actor.BodyTypeDynamic, 0, 0) + spring := NewDistanceJoint(anchor, body, mgl64.Vec3{0, 0, 0}, mgl64.Vec3{1, 0, 0}) + spring.EnableSpring, spring.SpringHertz, spring.SpringDampingRatio = true, hertz, 0 + w.AddJoint(spring) + body.Transform.Position = mgl64.Vec3{1.2, 0, 0} // stretched by 20 cm + + var crossings []float64 + elapsed, previous := 0.0, body.Transform.Position.X()-1 + simulate(w, 5, func() { + elapsed += sceneDt + x := body.Transform.Position.X() - 1 + if previous > 0 && x <= 0 { + crossings = append(crossings, elapsed-sceneDt*x/(x-previous)) + } + previous = x + }) + if len(crossings) < 3 { + t.Fatalf("only %d crossings", len(crossings)) + } + frequency := float64(len(crossings)-1) / (crossings[len(crossings)-1] - crossings[0]) + t.Logf("frequency %.4f Hz, spring %.1f Hz", frequency, hertz) + if math.Abs(frequency-hertz) > 0.03*hertz { + t.Errorf("frequency %.3f Hz, want %.1f Hz", frequency, hertz) + } +} + +// A rope (distance in [0, L]): the ball falls freely, then hangs at L +func TestJointRope(t *testing.T) { + w := newScene(1) + anchor := anchorBody(w, mgl64.Vec3{0, 5, 0}) + ball := addBody(w, mgl64.Vec3{0.5, 5, 0}, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.1}, actor.BodyTypeDynamic, 0, 0) + rope := NewDistanceJoint(anchor, ball, mgl64.Vec3{0, 5, 0}, mgl64.Vec3{0.5, 5, 0}) + rope.EnableSpring, rope.EnableLimit, rope.MinLength, rope.MaxLength = true, true, 0, 2 + w.AddJoint(rope) + simulate(w, 0.2, nil) + if y := ball.Transform.Position.Y(); math.Abs(y-(5-0.5*sceneGravity*0.04)) > 0.01 { + t.Errorf("free fall: y=%.3f, want %.3f", y, 5-0.5*sceneGravity*0.04) + } + simulate(w, 8, nil) + if d := ball.Transform.Position.Sub(mgl64.Vec3{0, 5, 0}).Len(); math.Abs(d-2) > 0.002 { + t.Errorf("the rope is %.4f m long, want 2", d) + } +} + +// A fixed joint: 2 boxes stay welded, even hit +func TestJointFixed(t *testing.T) { + w := newScene(1) + w.Gravity = mgl64.Vec3{} + a := addBody(w, mgl64.Vec3{0, 0, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.5, 0) + b := addBody(w, mgl64.Vec3{0.5, 0, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.5, 0) + w.AddJoint(NewFixedJoint(a, b, mgl64.Vec3{0.25, 0, 0})) + a.AddImpulseAtPoint(mgl64.Vec3{0, 20, 0}, mgl64.Vec3{-0.25, 0, 0.2}) + worstPosition, worstAngle := 0.0, 0.0 + simulate(w, 3, func() { + relative := a.Transform.ToLocal(b.Transform.Position) + worstPosition = math.Max(worstPosition, relative.Sub(mgl64.Vec3{0.5, 0, 0}).Len()) + rotation := a.Transform.Rotation.Conjugate().Mul(b.Transform.Rotation) + worstAngle = math.Max(worstAngle, 2*math.Acos(math.Min(1, math.Abs(rotation.W)))) + }) + t.Logf("worst %.3f mm, %.3f°", worstPosition*1000, degrees(worstAngle)) + if worstPosition > 0.001 || degrees(worstAngle) > 0.5 { + t.Errorf("the weld moved by %.2f mm and %.2f°", worstPosition*1000, degrees(worstAngle)) + } + if a.AngularVelocity.Len() < 1 { + t.Error("the welded pair did not spin") + } +} + +// Motors: the hinge motor reaches its speed, the ball drive brings the body to its target without overshoot +// (critical damping) +func TestJointMotors(t *testing.T) { + w := newScene(1) + w.Gravity = mgl64.Vec3{} + axle := anchorBody(w, mgl64.Vec3{}) + wheel := addBody(w, mgl64.Vec3{}, mgl64.QuatIdent(), &actor.Capsule{HalfHeight: 0.3, Radius: 0.2}, actor.BodyTypeDynamic, 0.5, 0) + hinge := NewHingeJoint(axle, wheel, mgl64.Vec3{}, mgl64.Vec3{0, 1, 0}) + hinge.EnableMotor, hinge.MotorSpeed, hinge.MaxMotorTorque = true, 3, 100 + w.AddJoint(hinge) + simulate(w, 1, nil) + if speed := wheel.AngularVelocity.Y(); math.Abs(speed-3) > 0.01 { + t.Errorf("motor speed %.3f rad/s, want 3", speed) + } + + w2 := newScene(1) + w2.Gravity = mgl64.Vec3{} + pivot := anchorBody(w2, mgl64.Vec3{}) + arm := addBody(w2, mgl64.Vec3{0, 0, 0}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{0.5, 0.1, 0.1}}, actor.BodyTypeDynamic, 0.5, 0) + drive := NewBallJoint(pivot, arm, mgl64.Vec3{}, mgl64.Vec3{1, 0, 0}) + target := mgl64.QuatRotate(math.Pi/2, mgl64.Vec3{0, 0, 1}) + drive.EnableDrive, drive.DriveTarget, drive.DriveHertz, drive.DriveDampingRatio = true, target, 2, 1 + w2.AddJoint(drive) + overshoot := 0.0 + simulate(w2, 3, func() { + angle := 2 * math.Atan2(arm.Transform.Rotation.V.Z(), arm.Transform.Rotation.W) + overshoot = math.Max(overshoot, angle-math.Pi/2) + }) + finalError := degrees(2 * math.Acos(math.Min(1, math.Abs(arm.Transform.Rotation.Dot(target))))) + t.Logf("drive: final error %.3f°, overshoot %.3f°", finalError, degrees(overshoot)) + if finalError > 0.5 { + t.Errorf("the drive did not reach its target: %.2f° away", finalError) + } + if degrees(overshoot) > 1 { + t.Errorf("critically damped drive overshot by %.2f°", degrees(overshoot)) + } +} + +// 2 bodies linked by a joint don't collide by default; with CollideConnected they do +func TestJointCollideConnected(t *testing.T) { + for _, collide := range []bool{false, true} { + w := newScene(1) + w.Gravity = mgl64.Vec3{} + a := addBody(w, mgl64.Vec3{0, 0, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.5, 0) + b := addBody(w, mgl64.Vec3{0.4, 0, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.5, 0) + joint := NewDistanceJoint(a, b, a.Transform.Position, b.Transform.Position) + joint.CollideConnected = collide + w.AddJoint(joint) + w.Step(sceneDt) + if contacts := len(w.Contacts()); (contacts > 0) != collide { + t.Errorf("CollideConnected=%v: %d contacts", collide, contacts) + } + } +} + +// Joints don't allocate after the first steps +func TestJointsDoNotAllocate(t *testing.T) { + if raceEnabled { + t.Skip("sync.Pool drops its items with the race detector") + } + w := newScene(1) + hangingChain(w, 10) + simulate(w, 0.2, nil) + if allocs := testing.AllocsPerRun(10, func() { w.Step(sceneDt) }); allocs > 0 { + t.Errorf("%.1f allocations per step", allocs) + } +} diff --git a/measure_test.go b/measure_test.go new file mode 100644 index 0000000..e8fddcc --- /dev/null +++ b/measure_test.go @@ -0,0 +1,169 @@ +package feather + +import ( + "math" + + "github.com/akmonengine/feather/actor" + "github.com/go-gl/mathgl/mgl64" +) + +// ========== MEASURES ========== +// Exact measures of the overlaps, independent of the collision detection of the engine + +// surfaceDepth: how deep the body is in the static body (plane, terrain or box), 0 outside it +func surfaceDepth(surface, body *actor.RigidBody) float64 { + switch shape := surface.Shape.(type) { + case *actor.Plane: + lowest := body.SupportWorld(shape.Normal.Mul(-1)) + return math.Max(0, -(lowest.Dot(shape.Normal) + shape.Distance)) + case *actor.Heightfield: + return terrainDepth(shape, surface.Transform, body) + } + return math.Max(0, satOverlap(surface, body)) +} + +// terrainDepth: the deepest point of the body under the terrain, 0 above it +func terrainDepth(field *actor.Heightfield, transform actor.Transform, body *actor.RigidBody) float64 { + // the distance of a world point to the terrain, negative under it + distance := func(p mgl64.Vec3) float64 { + return terrainDistance(field, transform.Rotation.Conjugate().Rotate(p.Sub(transform.Position))) + } + depth := 0.0 + switch shape := body.Shape.(type) { + case *actor.Box: + for c := 0; c < 8; c++ { + corner := shape.HalfExtents + for k := 0; k < 3; k++ { + if c&(1< 1e-9 { + axes[count] = axis.Mul(1 / length) + count++ + } + } + directions := func(body *actor.RigidBody) []mgl64.Vec3 { + if _, ok := body.Shape.(*actor.Box); !ok { + return nil + } + r := body.Transform.Rotation + return []mgl64.Vec3{r.Rotate(mgl64.Vec3{1, 0, 0}), r.Rotate(mgl64.Vec3{0, 1, 0}), r.Rotate(mgl64.Vec3{0, 0, 1})} + } + axesA, axesB := directions(a), directions(b) + for _, axis := range axesA { + add(axis) + } + for _, axis := range axesB { + add(axis) + } + for _, x := range axesA { + for _, y := range axesB { + add(x.Cross(y)) + } + } + for _, pair := range [2][2]*actor.RigidBody{{a, b}, {b, a}} { + if _, ok := pair[0].Shape.(*actor.Sphere); ok { + add(pair[0].Transform.Position.Sub(closestOnBox(pair[1], pair[0].Transform.Position))) + } + } + + overlap := math.Inf(1) + for _, n := range axes[:count] { + maxA, minA := a.SupportWorld(n).Dot(n), a.SupportWorld(n.Mul(-1)).Dot(n) + maxB, minB := b.SupportWorld(n).Dot(n), b.SupportWorld(n.Mul(-1)).Dot(n) + overlap = math.Min(overlap, math.Min(maxA-minB, maxB-minA)) + } + return overlap +} + +// closestOnBox: the point of the box closest to p +func closestOnBox(body *actor.RigidBody, p mgl64.Vec3) mgl64.Vec3 { + box, ok := body.Shape.(*actor.Box) + if !ok { + return body.Transform.Position + } + local := body.Transform.Rotation.Conjugate().Rotate(p.Sub(body.Transform.Position)) + for k := 0; k < 3; k++ { + local[k] = math.Max(-box.HalfExtents[k], math.Min(box.HalfExtents[k], local[k])) + } + return body.Transform.ToWorld(local) +} + +// boxOverlap: how much 2 boxes overlap, 0 apart +func boxOverlap(a, b *actor.RigidBody) float64 { + return math.Max(0, satOverlap(a, b)) +} + +// terrainDistance: the distance from the point to the triangles, negative under the terrain +func terrainDistance(field *actor.Heightfield, p mgl64.Vec3) float64 { + best := math.Inf(1) + around := mgl64.Vec3{1, 100, 1} + for _, cell := range field.OverlapCells(actor.AABB{Min: p.Sub(around), Max: p.Add(around)}, nil) { + x, z := int(cell)/(field.ZSamples-1), int(cell)%(field.ZSamples-1) + for t := 0; t < 2; t++ { + triangle, _ := field.Triangle(x, z, t) + best = math.Min(best, p.Sub(closestOnTriangle(p, triangle)).Len()) + } + } + if height, ok := field.HeightAt(p.X(), p.Z()); ok && p.Y() < height { + return -best + } + return best +} + +// closestOnTriangle: the closest point of the triangle (Ericson 5.1.5) +func closestOnTriangle(p mgl64.Vec3, triangle [3]mgl64.Vec3) mgl64.Vec3 { + a, b, c := triangle[0], triangle[1], triangle[2] + ab, ac, ap := b.Sub(a), c.Sub(a), p.Sub(a) + d1, d2 := ab.Dot(ap), ac.Dot(ap) + if d1 <= 0 && d2 <= 0 { + return a + } + bp := p.Sub(b) + d3, d4 := ab.Dot(bp), ac.Dot(bp) + if d3 >= 0 && d4 <= d3 { + return b + } + vc := d1*d4 - d3*d2 + if vc <= 0 && d1 >= 0 && d3 <= 0 { + return a.Add(ab.Mul(d1 / (d1 - d3))) + } + cp := p.Sub(c) + d5, d6 := ab.Dot(cp), ac.Dot(cp) + if d6 >= 0 && d5 <= d6 { + return c + } + vb := d5*d2 - d1*d6 + if vb <= 0 && d2 >= 0 && d6 <= 0 { + return a.Add(ac.Mul(d2 / (d2 - d6))) + } + va := d3*d6 - d5*d4 + if va <= 0 && d4-d3 >= 0 && d5-d6 >= 0 { + return b.Add(c.Sub(b).Mul((d4 - d3) / ((d4 - d3) + (d5 - d6)))) + } + denominator := 1 / (va + vb + vc) + return a.Add(ab.Mul(vb * denominator)).Add(ac.Mul(vc * denominator)) +} diff --git a/norace_test.go b/norace_test.go new file mode 100644 index 0000000..7a0eff9 --- /dev/null +++ b/norace_test.go @@ -0,0 +1,5 @@ +//go:build !race + +package feather + +const raceEnabled = false diff --git a/pipeline.go b/pipeline.go deleted file mode 100644 index fcda286..0000000 --- a/pipeline.go +++ /dev/null @@ -1,20 +0,0 @@ -package feather - -import "sync" - -func task[T any](workersCount int, data []T, fn func(data T)) { - var wg sync.WaitGroup - dataSize := len(data) - chunkSize := (dataSize + workersCount - 1) / workersCount - - for workerID := 0; workerID < workersCount; workerID++ { - wg.Add(1) - go func(start, end int) { - defer wg.Done() - for i := start; i < end; i++ { - fn(data[i]) - } - }(workerID*chunkSize, min((workerID+1)*chunkSize, dataSize)) - } - wg.Wait() -} diff --git a/pool.go b/pool.go new file mode 100644 index 0000000..a0014f3 --- /dev/null +++ b/pool.go @@ -0,0 +1,131 @@ +package feather + +import ( + "runtime" + "sync/atomic" +) + +// spinsBeforeYield: an idle worker checks for new work this many times before letting other goroutines run +const spinsBeforeYield = 64 + +// workerPool runs the stages of a step in parallel. +// The workers are created once, and sleep between the steps (waiting on a channel, no CPU used). +// During a step they wait for the stages by spinning: a stage is often too short to wake up a goroutine. +// Each index is processed exactly once and writes only its own result: the order doesn't matter. +type workerPool struct { + helpers int // workers - 1, the caller is the last worker + wake chan struct{} + generation atomic.Uint64 + parking atomic.Bool + awake atomic.Int64 // helpers not sleeping + pending atomic.Int64 // helpers still working on the current job + next atomic.Int64 // next index to process + job func(i int) + count int64 + chunkSize int64 + start uint64 // generation when the helpers are woken up +} + +// begin wakes the helpers up for a step. They are created the first time (or if the count of workers changes) +func (p *workerPool) begin(workers int) { + if p.helpers != workers-1 { + p.close() + p.helpers = workers - 1 + p.wake = make(chan struct{}, p.helpers) + for range p.helpers { + go p.loop() + } + } + + p.parking.Store(false) + p.start = p.generation.Load() + p.awake.Store(int64(p.helpers)) + for range p.helpers { + p.wake <- struct{}{} + } +} + +// end puts the helpers to sleep until the next step +func (p *workerPool) end() { + if p.helpers == 0 { + return + } + p.parking.Store(true) + for spins := 0; p.awake.Load() != 0; spins++ { + if spins > spinsBeforeYield { + runtime.Gosched() + } + } +} + +// close stops the helpers +func (p *workerPool) close() { + if p.wake != nil { + close(p.wake) + p.wake = nil + } + p.helpers = 0 +} + +func (p *workerPool) loop() { + wake := p.wake + for { + if _, ok := <-wake; !ok { + return + } + + seen, spins := p.start, 0 + for !p.parking.Load() { + generation := p.generation.Load() + if generation == seen { + spins++ + if spins > spinsBeforeYield { + runtime.Gosched() + } + continue + } + + seen, spins = generation, 0 + p.work() + p.pending.Add(-1) + } + p.awake.Add(-1) + } +} + +// run calls job(i) for each i in [0, count), on all the workers (if they are awake) +func (p *workerPool) run(count int, chunkSize int, job func(i int)) { + if p.helpers == 0 || p.parking.Load() || count <= chunkSize { + for i := 0; i < count; i++ { + job(i) + } + return + } + + p.job, p.count, p.chunkSize = job, int64(count), int64(chunkSize) + p.next.Store(0) + p.pending.Store(int64(p.helpers)) + p.generation.Add(1) + + p.work() + for spins := 0; p.pending.Load() != 0; spins++ { + if spins > spinsBeforeYield { + runtime.Gosched() + } + } + // the job references the World: the workers must not keep it alive + p.job = nil +} + +func (p *workerPool) work() { + for { + start := p.next.Add(p.chunkSize) - p.chunkSize + if start >= p.count { + return + } + end := min(start+p.chunkSize, p.count) + for i := start; i < end; i++ { + p.job(int(i)) + } + } +} diff --git a/profile.go b/profile.go new file mode 100644 index 0000000..57ace62 --- /dev/null +++ b/profile.go @@ -0,0 +1,30 @@ +package feather + +import "time" + +// ========== PROFILE ========== + +// Profile is the time spent in each phase of the last step, as the b2Profile of Box2D. The phases follow each other: +// their sum is the step, but for the bookkeeping between them +type Profile struct { + Step time.Duration + // BroadPhase: the AABBs of the bodies and the pairs of the spatial grid + BroadPhase time.Duration + // NarrowPhase: the contacts of the pairs + NarrowPhase time.Duration + // Prepare: the events of the contacts, the warm start and the constraints of the solver + Prepare time.Duration + // Substeps: the velocities, the constraints and the positions, for all the substeps + Substeps time.Duration + // Restitution: the bounces and the impulses stored for the next step + Restitution time.Duration + // Continuous: the continuous collision of the fast bodies + Continuous time.Duration + // Islands: the sleep of the bodies, and the events + Islands time.Duration +} + +// Profile of the last step +func (w *World) Profile() Profile { + return w.profile +} diff --git a/profile_test.go b/profile_test.go new file mode 100644 index 0000000..263c2f4 --- /dev/null +++ b/profile_test.go @@ -0,0 +1,26 @@ +package feather + +import ( + "testing" + "time" +) + +// Every phase of a step with contacts takes some time, and the phases fit in the step +func TestProfile(t *testing.T) { + w := terrainScene(4) + defer w.Close() + simulate(w, 1, nil) + profile := w.Profile() + phases := []time.Duration{profile.BroadPhase, profile.NarrowPhase, profile.Prepare, profile.Substeps, profile.Restitution, profile.Continuous, profile.Islands} + var sum time.Duration + for i, phase := range phases { + if phase <= 0 { + t.Errorf("phase %d: %v", i, phase) + } + sum += phase + } + t.Logf("%+v", profile) + if sum > profile.Step { + t.Errorf("the phases take %v, the step %v", sum, profile.Step) + } +} diff --git a/race_test.go b/race_test.go new file mode 100644 index 0000000..5458bb0 --- /dev/null +++ b/race_test.go @@ -0,0 +1,6 @@ +//go:build race + +package feather + +// the race detector drops the items of sync.Pool on purpose: the allocations cannot be measured +const raceEnabled = true diff --git a/scenes_test.go b/scenes_test.go new file mode 100644 index 0000000..db4e646 --- /dev/null +++ b/scenes_test.go @@ -0,0 +1,216 @@ +package feather + +import ( + "math" + "testing" + + "github.com/akmonengine/feather/actor" + "github.com/go-gl/mathgl/mgl64" +) + +// ========== MINIMAL SCENES ========== +// Each scene isolates one mechanism, with one body (or two) and no chaos: the result doesn't depend on a draw. +// The bound of every scene comes from a quantity of the engine, never from a measure: +// landingDepth = LinearSlop, the length tolerance of the collision detection. A speculative contact stops a point before +// it touches: a point goes deeper only if the contact misses it (reduction, rotation during the step, no contact) +const landingDepth = LinearSlop + +// cornerDown: the rotation putting the corner (1, 1, 1) of a box at the bottom +func cornerDown() mgl64.Quat { + return mgl64.QuatBetweenVectors(mgl64.Vec3{1, 1, 1}.Normalize(), mgl64.Vec3{0, -1, 0}) +} + +// ridgeTerrain: a terrain folded along z at x = 0, 41x41 samples every 0.25 m, sloped by angle on both sides: +// a ridge (convex fold) or a V (concave fold) +func ridgeTerrain(w *World, angle float64, ridge bool) *actor.RigidBody { + const samples, spacing = 41, 0.25 + heights := make([]float32, samples*samples) + for x := 0; x < samples; x++ { + for z := 0; z < samples; z++ { + height := math.Abs(float64(x)-(samples-1)/2.0) * spacing * math.Tan(angle) + if ridge { + height = -height + } + heights[x*samples+z] = float32(height) + } + } + field := actor.NewHeightfield(samples, samples, heights, mgl64.Vec3{spacing, 1, spacing}) + return addBody(w, mgl64.Vec3{}, mgl64.QuatIdent(), field, actor.BodyTypeStatic, 0.6, 0) +} + +// worstDepth runs the scene and returns the deepest point of the body under the static body, over all the steps +func worstDepth(w *World, surface, body *actor.RigidBody, seconds float64) float64 { + worst := 0.0 + simulate(w, seconds, func() { worst = math.Max(worst, surfaceDepth(surface, body)) }) + return worst +} + +// A box falls on a corner while spinning, on a plane, on a sloped terrain (across the diagonals of the cells) and on a +// static box. A thin plate falls fast and tilted: its 8 corners are in the speculative margin, only its bottom face +// touches. Turned by 45° over a box, the clipping of its face gives 8 points: the reduction to 4 must keep the deepest +func TestBoxLandsOnCorner(t *testing.T) { + plate := &actor.Box{HalfExtents: mgl64.Vec3{0.3, 0.02, 0.3}} + tilted := mgl64.QuatRotate(0.15, mgl64.Vec3{1, 0, 1}.Normalize()) + cases := []struct { + name string + surface string + shape actor.ShapeInterface + rotation mgl64.Quat + velocity mgl64.Vec3 + spin mgl64.Vec3 + }{ + {"box on a plane", "plane", cube(), cornerDown(), mgl64.Vec3{}, mgl64.Vec3{3, 8, -2}}, + {"box on a terrain", "terrain", cube(), cornerDown(), mgl64.Vec3{}, mgl64.Vec3{3, 8, -2}}, + {"box on a box", "box", cube(), cornerDown(), mgl64.Vec3{}, mgl64.Vec3{3, 8, -2}}, + {"plate on a plane", "plane", plate, tilted, mgl64.Vec3{0, -12, 0}, mgl64.Vec3{0, 4, 0}}, + {"plate on a terrain", "terrain", plate, tilted, mgl64.Vec3{0, -12, 0}, mgl64.Vec3{0, 4, 0}}, + {"plate on a box", "box", plate, mgl64.QuatRotate(math.Pi/4, mgl64.Vec3{0, 1, 0}).Mul(tilted), mgl64.Vec3{0, -12, 0}, mgl64.Vec3{}}, + } + for _, c := range cases { + w := newScene(1) + var surface *actor.RigidBody + start := mgl64.Vec3{0.3, 1.2, 0.4} + switch c.surface { + case "terrain": + surface = slopeTerrain(w, 15*math.Pi/180, 0.6) + start[1] += 0.3 * math.Tan(15*math.Pi/180) + case "box": + surface = addBody(w, mgl64.Vec3{0.3, -0.25, 0.4}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{0.3, 0.25, 0.3}}, actor.BodyTypeStatic, 0.6, 0) + default: + surface = addGround(w, 0.6) + } + body := addBody(w, start, c.rotation, c.shape, actor.BodyTypeDynamic, 0.6, 0) + body.Velocity, body.AngularVelocity = c.velocity, c.spin + depth := worstDepth(w, surface, body, 2) + t.Logf("%s: %.2f mm", c.name, depth*1000) + if depth > landingDepth { + t.Errorf("%s: %.2f mm under the surface", c.name, depth*1000) + } + } +} + +// A capsule tumbles at 30 rad/s while sliding on a plane, and spins like a top on its end at 20 rad/s: its contact +// points turn with it during the step, and its separation follows its rounded ends (cores) +func TestCapsuleTumbles(t *testing.T) { + shape := &actor.Capsule{HalfHeight: 0.22, Radius: 0.15} + tilted := mgl64.QuatRotate(0.6, mgl64.Vec3{0, 0, 1}) + axis := tilted.Rotate(mgl64.Vec3{0, 1, 0}) + cases := []struct { + name string + position mgl64.Vec3 + rotation mgl64.Quat + velocity mgl64.Vec3 + spin mgl64.Vec3 + }{ + {"tumbling", mgl64.Vec3{0, 0.5, 0}, mgl64.QuatRotate(math.Pi/2, mgl64.Vec3{0, 0, 1}), mgl64.Vec3{4, 0, 0}, mgl64.Vec3{0, 0, -30}}, + // resting on its lower end, spinning around its axis & precessing + {"spinning top", axis.Mul(shape.HalfHeight).Add(mgl64.Vec3{0, shape.Radius, 0}), tilted, mgl64.Vec3{}, axis.Mul(20).Add(mgl64.Vec3{0, 5, 0})}, + } + for _, c := range cases { + w := newScene(1) + ground := addGround(w, 0.15) + capsule := addBody(w, c.position, c.rotation, shape, actor.BodyTypeDynamic, 0.15, 0) + capsule.Velocity, capsule.AngularVelocity = c.velocity, c.spin + depth := worstDepth(w, ground, capsule, 3) + t.Logf("%s: %.2f mm", c.name, depth*1000) + if depth > landingDepth { + t.Errorf("%s: the capsule went %.2f mm under the ground", c.name, depth*1000) + } + } +} + +// A sphere rolls fast into a static wall: it stops on it +func TestSphereRollsIntoWall(t *testing.T) { + for _, speed := range []float64{3, 6, 10} { + w := newScene(1) + addGround(w, 0.6) + wall := addBody(w, mgl64.Vec3{3.5, 1, 0}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{0.5, 1, 2}}, actor.BodyTypeStatic, 0.6, 0) + sphere := addBody(w, mgl64.Vec3{0, 0.2, 0}, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.2}, actor.BodyTypeDynamic, 0.6, 0) + sphere.Velocity = mgl64.Vec3{speed, 0, 0} + sphere.AngularVelocity = mgl64.Vec3{0, 0, -speed / 0.2} + depth := worstDepth(w, wall, sphere, 1) + t.Logf("%.0f m/s: %.2f mm in the wall", speed, depth*1000) + if depth > landingDepth { + t.Errorf("%.0f m/s: the sphere went %.2f mm in the wall", speed, depth*1000) + } + } +} + +// A box lands astride a ridge of the terrain: the active edge of the ridge holds it, it tips over on one side and rests +// on the slope (tan 20° < friction) +func TestBoxOnRidge(t *testing.T) { + w := newScene(1) + terrain := ridgeTerrain(w, 20*math.Pi/180, true) + box := addBody(w, mgl64.Vec3{0.05, 0.6, 0.1}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{0.3, 0.1, 0.3}}, actor.BodyTypeDynamic, 0.6, 0) + depth := worstDepth(w, terrain, box, 3) + // the slope under the box + side := math.Copysign(1, box.Transform.Position.X()) + slope := mgl64.Vec3{side * math.Sin(20*math.Pi/180), math.Cos(20 * math.Pi / 180), 0} + tilt := math.Acos(math.Min(1, math.Abs(box.Transform.Rotation.Rotate(mgl64.Vec3{0, 1, 0}).Dot(slope)))) + t.Logf("%.2f mm, at x=%.3f, %.2f° from the slope, asleep %v", depth*1000, box.Transform.Position.X(), degrees(tilt), box.IsSleeping) + if depth > landingDepth { + t.Errorf("the box went %.2f mm under the ridge", depth*1000) + } + // flat on the slope: its corners at the same depth, within the tolerance of the detection, across its width + if !box.IsSleeping || tilt > math.Atan(LinearSlop/0.6) { + t.Error("the box doesn't rest on the slope") + } +} + +// A sphere dropped in a V rests on both slopes: its center at r / cos(angle) above the fold +func TestSphereInV(t *testing.T) { + const radius = 0.3 + angle := 30 * math.Pi / 180 + w := newScene(1) + terrain := ridgeTerrain(w, angle, false) + sphere := addBody(w, mgl64.Vec3{0.02, 1, 0.1}, mgl64.QuatIdent(), &actor.Sphere{Radius: radius}, actor.BodyTypeDynamic, 0.6, 0) + depth := worstDepth(w, terrain, sphere, 3) + want := radius / math.Cos(angle) + height := sphere.Transform.Position.Y() + t.Logf("%.2f mm, height %.4f m (want %.4f), asleep %v", depth*1000, height, want, sphere.IsSleeping) + if depth > landingDepth { + t.Errorf("the sphere went %.2f mm under the terrain", depth*1000) + } + // at rest, no point is deeper than the tolerance of the detection + if math.Abs(height-want) > LinearSlop || !sphere.IsSleeping { + t.Errorf("the sphere rests at %.4f m, want %.4f", height, want) + } +} + +// A box falls on a corner on another box resting on the ground. Between 2 dynamic bodies the contact is speculative +// only within SpeculativeDistance (as in Box2D): the box, landing at v, enters the other box by v dt - SpeculativeDistance +// at most during the step, then the spring of the contact pushes it out +func TestBoxLandsOnBox(t *testing.T) { + w := newScene(1) + addGround(w, 0.6) + lower := addBody(w, mgl64.Vec3{0, cubeHalf, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.6, 0) + upper := addBody(w, mgl64.Vec3{0.05, 1.5, 0.08}, cornerDown(), cube(), actor.BodyTypeDynamic, 0.6, 0) + upper.AngularVelocity = mgl64.Vec3{0, 5, 0} + // the lowest corner falls from 1.5 - √3 half sizes to the top of the lower box + fall := 1.5 - math.Sqrt(3)*cubeHalf - 2*cubeHalf + bound := math.Sqrt(2*sceneGravity*fall)*sceneDt - SpeculativeDistance + worst := 0.0 + simulate(w, 2, func() { worst = math.Max(worst, boxOverlap(lower, upper)) }) + t.Logf("%.2f mm (at most %.2f mm)", worst*1000, bound*1000) + if worst > bound { + t.Errorf("the boxes overlap by %.2f mm", worst*1000) + } +} + +// A small ball at 200 m/s (4 m per step, 400 times the wall thickness) against a wall of 1 cm: the continuous collision +// stops it on the wall +func TestFastBodyAgainstThinWall(t *testing.T) { + w := newScene(1) + w.Gravity = mgl64.Vec3{} + wall := addBody(w, mgl64.Vec3{}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{0.005, 2, 2}}, actor.BodyTypeStatic, 0, 0) + ball := addBody(w, mgl64.Vec3{-6, 0.3, 0.2}, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.05}, actor.BodyTypeDynamic, 0, 0) + ball.Velocity = mgl64.Vec3{200, 0, 0} + depth := worstDepth(w, wall, ball, 0.5) + t.Logf("%.2f mm, at x=%.3f", depth*1000, ball.Transform.Position.X()) + if ball.Transform.Position.X() > 0 { + t.Fatalf("the ball went through the wall: x=%.3f", ball.Transform.Position.X()) + } + if depth > landingDepth { + t.Errorf("the ball went %.2f mm in the wall", depth*1000) + } +} diff --git a/solver.go b/solver.go new file mode 100644 index 0000000..b23da88 --- /dev/null +++ b/solver.go @@ -0,0 +1,1162 @@ +package feather + +import ( + "math" + + "github.com/akmonengine/feather/actor" + "github.com/akmonengine/feather/constraint" + "github.com/go-gl/mathgl/mgl64" +) + +// The solver is a TGS Soft solver (sub-stepping + soft constraints + warm starting + relax). +// See ALGORITHMS.md - "Solver" section. +const ( + // LinearSlop is the length tolerance of the collision detection (m) + LinearSlop = 0.005 + + // SpeculativeDistance: contacts are created before the shapes touch, up to this distance (m), + // plus the distance the bodies can travel during the step + SpeculativeDistance = 4 * LinearSlop + + // DefaultContactHertz is the stiffness of the contacts between dynamic bodies, as in Box2D v3.1. + // Contacts with a static body are twice as stiff, with half the damping ratio (as Box3D). + // Higher values = less overlap under load, lower values = softer contacts + DefaultContactHertz = 30.0 + + // ContactDampingRatio of the contacts: > 1 means no oscillation + ContactDampingRatio = 10.0 + + // ContactSpeed is the maximum speed (m/s) used to push overlapping bodies apart + ContactSpeed = 3.0 + + // RestitutionThreshold: no bounce under this relative velocity (m/s) + RestitutionThreshold = 1.0 + + // MaxLinearSpeed of a body (m/s) + MaxLinearSpeed = 400.0 + + // MaxRotation of a body during one substep (rad). Box2D limits it per step: Feather lets the bodies turn faster + // (a wheel, a ball), the contacts follow the rotation during the step (turnAnchors) + MaxRotation = 0.25 * math.Pi + + // StaticFrictionSpeed: under this sliding speed (m/s), a contact point uses the static friction + StaticFrictionSpeed = 0.01 + + // turnAnchorsCos: the anchors of a body turn if it turned more than 0.01 rad since the beginning of the step, + // cos(0.01 / 2). Under it, the error of a lever arm of 50 cm is 0.5 mm + turnAnchorsCos = 0.99998750002604166 + + // minFrictionWeight: the weight of a point far from touching in the friction center (Box3D) + minFrictionWeight = 1e-10 + + // minFrictionDeterminant: under it, the tangents can't turn the bodies apart (no mass): no friction + minFrictionDeterminant = 1e-30 + + // the contact hertz can't exceed 1/8 of the sub-steps rate, otherwise it becomes unstable + hertzPerSubstepRate = 0.125 + + // a new contact point takes the impulses of an old point closer than this distance (m) + contactMatchDistance = 4 * LinearSlop +) + +// spring is a soft constraint (Erin Catto, "Soft Constraints", GDC 2011): the error of the constraint is a spring +// of frequency ω and damping ratio ζ, whatever the mass (k = m ω², c = 2 m ζ ω), integrated implicitly over h: +// +// biasRate = k / (c + h k) = ω / (2ζ + h ω) // the part of the error removed per second +// gamma = m / (h (c + h k)) = 1 / (h ω (2ζ + h ω)) // the softness γ of the paper, times the mass +// +// A row of effective mass m, relative velocity v and bias b (biasRate * error) gets the impulse +// +// λ = -(m (v + b) + gamma * accumulated) / (1 + gamma) +// +// A rigid row has gamma = 0: λ = -m (v + b) +type spring struct { + biasRate float64 + gamma float64 +} + +// rigid: a constraint without softness +var rigid = spring{} + +// newSpring for a frequency and a damping ratio, over the substep h. A spring of 0 hertz doesn't exist (it would be +// infinitely soft): the callers check the frequency first +func newSpring(hertz, dampingRatio, h float64) spring { + if hertz <= 0 { + panic("feather: a spring needs a frequency > 0") + } + omega := 2 * math.Pi * hertz + return spring{biasRate: omega / (2*dampingRatio + h*omega), gamma: 1 / (h * omega * (2*dampingRatio + h*omega))} +} + +// impulse of a row of effective mass m +func (s spring) impulse(mass, velocity, bias, accumulated float64) float64 { + return -(mass*(velocity+bias) + s.gamma*accumulated) / (1 + s.gamma) +} + +// impulse3 of 3 rows solved together, with the inverse of their mass matrix +func (s spring) impulse3(inverseMass *mgl64.Mat3, velocity, bias, accumulated mgl64.Vec3) mgl64.Vec3 { + v := mgl64.Vec3{velocity[0] + bias[0], velocity[1] + bias[1], velocity[2] + bias[2]} + u := actor.MulMat3(inverseMass, v) + scale := -1 / (1 + s.gamma) + return mgl64.Vec3{(u[0] + accumulated[0]*s.gamma) * scale, (u[1] + accumulated[1]*s.gamma) * scale, (u[2] + accumulated[2]*s.gamma) * scale} +} + +// skewTerm is [rX]x I [rY]x: the angular part of the mass matrix of a point constraint +func skewTerm(inertia *mgl64.Mat3, rX, rY mgl64.Vec3) mgl64.Mat3 { + sx, sy := skew(rX), skew(rY) + t := actor.Mul3(&sx, inertia) + return actor.Mul3(&t, &sy) +} + +// bodyState is the copy of a dynamic body used by the solver during a step +type bodyState struct { + // the impulses read and write these 64 bytes: a cache line + body *actor.RigidBody + velocity mgl64.Vec3 + angularVelocity mgl64.Vec3 + invMass float64 + + deltaPosition mgl64.Vec3 // since the beginning of the step + deltaMatrix mgl64.Mat3 // deltaRotation as a matrix, updated once per substep + deltaRotation mgl64.Quat // since the beginning of the step + inverseInertia mgl64.Mat3 // inverse inertia in world space, turned with the body during the step + anisotropic bool // false if the inertia is the same on all axes: no gyroscopic torque, it doesn't turn +} + +// bodyStart is what the solver keeps of a body at the beginning of the step, apart from its state: read by a few +// bodies per substep (the ones turning, with an anisotropic inertia), the states stay compact for the contacts +type bodyStart struct { + inertia mgl64.Mat3 // inverse inertia in world space + rotation mgl64.Quat +} + +// jacobian of a contact direction d: the angular part rA × d and rB × d, +// and the angular velocity given by a unit impulse, I⁻¹ * (r × d) +type jacobian struct { + angularA mgl64.Vec3 + angularB mgl64.Vec3 + impulseA mgl64.Vec3 + impulseB mgl64.Vec3 + mass float64 // effective mass +} + +type contactPoint struct { + rA mgl64.Vec3 // from the center of mass of A + rB mgl64.Vec3 // from the center of mass of B + // coreA & coreB: the contact point without the radius of the rounded shapes (the center of a sphere, the axis of + // a capsule). A rolling sphere turns its surface, not its center: the separation follows the cores + coreA mgl64.Vec3 + coreB mgl64.Vec3 + baseSeparation float64 + normal jacobian + normalImpulse float64 + totalNormalImpulse float64 // the normal impulse of the step: the impulse which stopped the point, for the restitution + normalVelocity float64 // before the solver, for the restitution + leverArm float64 // distance to the friction center: the twist friction it can hold +} + +type contactConstraint struct { + manifold *constraint.Manifold + indexA int // -1 for a static or sleeping body + indexB int + normal mgl64.Vec3 + tangents [2]mgl64.Vec3 + restitution float64 + spring spring + points [constraint.MaxContactPoints]contactPoint + pointsCount int + + // radius of the rounded shapes: the anchors turn with their cores + radiusA float64 + radiusB float64 + + // friction of the contact, at the friction center of its points (as Box3D, Jolt, the friction patch of PhysX): along + // both tangents, and the twist around the normal. The centers without the radius turn with the bodies (turnAnchors) + friction float64 + centerCoreA mgl64.Vec3 + centerCoreB mgl64.Vec3 + frictionRows [2]jacobian + frictionMass [3]float64 // the inverse of the 2x2 mass matrix of both tangents: xx, xy, yy + frictionImpulse [2]float64 + twistMass float64 + twistImpulse float64 + + // rolling resistance, around both tangents + rollingResistance float64 + rollingMass [2]float64 + rollingImpulse [2]float64 + rollingA [2]mgl64.Vec3 // angular velocity of A given by a unit impulse + rollingB [2]mgl64.Vec3 +} + +type solver struct { + states []bodyState + starts []bodyStart + constraints []contactConstraint + joints []Joint + // articulations: the trees of joints, solved together + articulations articulations + graph constraintGraph + pool *workerPool + jobs solverJobs + + // parameters of the current stage, for the jobs + manifolds []constraint.Manifold + contactSpring spring + staticSpring spring + gravity mgl64.Vec3 + maxAngularSpeed float64 + stage func(c *contactConstraint) + jointStage func(j Joint) + color []int + items []graphItem // the contacts then the joints, for the coloring + // stateIndex: the state of each body of the World (-1 if not awake); stateBody: the body of each state + stateIndex []int32 + stateBody []int32 + bodies []*actor.RigidBody + // indices: the states of the bodies of the joints only (a map, for the few bodies of the joints) + indices map[*actor.RigidBody]int + h float64 + invH float64 + + // static bodies (and sleeping ones) share this state: no mass, they never move + static bodyState +} + +// solverJobs are the functions run by the workers. They are created once: a closure created at each stage +// would allocate +type solverJobs struct { + integrateVelocity func(i int) + integratePosition func(i int) + warmStart func(c *contactConstraint) + push func(c *contactConstraint) + relax func(c *contactConstraint) + restitution func(c *contactConstraint) + warmStartJoint func(j Joint) + pushJoint func(j Joint) + relaxJoint func(j Joint) + color func(i int) + prepareConstraint func(i int) + storeImpulses func(i int) + finalize func(i int) + state func(i int) +} + +func (s *solver) initJobs() { + if s.jobs.color != nil { + return + } + s.jobs = solverJobs{ + integrateVelocity: s.integrateVelocity, + integratePosition: s.integratePosition, + warmStart: s.warmStartConstraint, + push: s.pushConstraint, + relax: s.relaxConstraint, + restitution: s.restitutionConstraint, + warmStartJoint: func(j Joint) { j.warmStart(s) }, + pushJoint: func(j Joint) { j.solve(s, true) }, + relaxJoint: func(j Joint) { j.solve(s, false) }, + prepareConstraint: s.prepareConstraint, + storeImpulses: s.storeImpulsesConstraint, + finalize: s.finalizeBody, + state: s.stateOf, + color: func(i int) { + s.solveItem(s.color[i]) + }, + } +} + +func (s *solver) state(index int) *bodyState { + if index < 0 { + return &s.static + } + return &s.states[index] +} + +func (s *solver) prepare(bodies []*actor.RigidBody, manifolds []constraint.Manifold, dt float64, substeps int, contactHertz float64, pool *workerPool) { + s.pool = pool + s.initJobs() + s.h = dt / float64(substeps) + s.invH = 1 / s.h + s.static = bodyState{deltaRotation: mgl64.QuatIdent(), deltaMatrix: mgl64.Ident3()} + + // ========== 1. Body states ========== + // the awake dynamic bodies get a state, numbered in the order of the World; the states are filled in parallel + if s.indices == nil { + s.indices = make(map[*actor.RigidBody]int) + } + clear(s.indices) + for _, joint := range s.joints { + base := joint.base() + s.indices[base.BodyA], s.indices[base.BodyB] = -1, -1 + } + if cap(s.stateIndex) < len(bodies) { + s.stateIndex = make([]int32, len(bodies)) + } + s.stateIndex = s.stateIndex[:len(bodies)] + s.stateBody = s.stateBody[:0] + for i, body := range bodies { + s.stateIndex[i] = -1 + if !isAwakeDynamic(body) { + continue + } + s.stateIndex[i] = int32(len(s.stateBody)) + if len(s.indices) > 0 { + if _, ok := s.indices[body]; ok { + s.indices[body] = len(s.stateBody) + } + } + s.stateBody = append(s.stateBody, int32(i)) + } + if cap(s.states) < len(s.stateBody) { + s.states = make([]bodyState, len(s.stateBody)) + s.starts = make([]bodyStart, len(s.stateBody)) + } + s.states, s.starts = s.states[:len(s.stateBody)], s.starts[:len(s.stateBody)] + s.bodies = bodies + s.pool.run(len(s.states), bodiesChunk, s.jobs.state) + s.bodies = nil + + // ========== 2. Contact constraints ========== + hertz := math.Min(contactHertz, hertzPerSubstepRate*s.invH) + s.contactSpring = newSpring(hertz, ContactDampingRatio, s.h) + s.staticSpring = newSpring(2*hertz, 0.5*ContactDampingRatio, s.h) + + s.manifolds = manifolds + if cap(s.constraints) < len(manifolds) { + s.constraints = make([]contactConstraint, len(manifolds)) + } + s.constraints = s.constraints[:len(manifolds)] + s.pool.run(len(manifolds), constraintsChunk, s.jobs.prepareConstraint) + s.manifolds = nil + + // ========== 3. Joints ========== + for _, joint := range s.joints { + joint.prepare(s) + } + s.buildArticulations() + + // ========== 4. Graph coloring: the contacts, then the joints ========== + s.items = s.items[:0] + for i := range s.constraints { + s.items = append(s.items, graphItem{s.constraints[i].indexA, s.constraints[i].indexB}) + } + for _, joint := range s.joints { + base := joint.base() + s.items = append(s.items, graphItem{base.indexA, base.indexB}) + } + s.graph.color(s.items, len(s.states)) +} + +// isIsotropic: the same inertia on all axes (sphere, cube), the gyroscopic torque ω × Iω is null +func isIsotropic(inertia mgl64.Mat3) bool { + return inertia[0] == inertia[4] && inertia[0] == inertia[8] && + inertia[1] == 0 && inertia[2] == 0 && inertia[3] == 0 && inertia[5] == 0 && inertia[6] == 0 && inertia[7] == 0 +} + +// prepareConstraint i, from the manifold i +func (s *solver) prepareConstraint(i int) { + manifold := &s.manifolds[i] + c := &s.constraints[i] + *c = contactConstraint{ + manifold: manifold, + indexA: int(s.stateIndex[manifold.IndexA]), + indexB: int(s.stateIndex[manifold.IndexB]), + normal: manifold.Normal, + pointsCount: manifold.Count, + } + if c.indexA < 0 && c.indexB < 0 { + // nothing to solve + c.pointsCount = 0 + return + } + + c.spring = s.contactSpring + if c.indexA < 0 || c.indexB < 0 { + c.spring = s.staticSpring + } + c.tangents[0], c.tangents[1] = tangentBasis(c.normal) + c.restitution = constraint.ComputeRestitution(manifold.BodyA.Material, manifold.BodyB.Material) + staticFriction := constraint.ComputeStaticFriction(manifold.BodyA.Material, manifold.BodyB.Material) + dynamicFriction := constraint.ComputeDynamicFriction(manifold.BodyA.Material, manifold.BodyB.Material) + + stateA, stateB := s.state(c.indexA), s.state(c.indexB) + radiusA, radiusB := shapeRadius(manifold.BodyA.Shape), shapeRadius(manifold.BodyB.Shape) + c.radiusA, c.radiusB = radiusA, radiusB + c.rollingResistance = constraint.ComputeRollingResistance(manifold.BodyA.Material, manifold.BodyB.Material, radiusA, radiusB) + if c.rollingResistance > 0 { + c.prepareRolling(stateA, stateB) + for k := range c.tangents { + c.rollingImpulse[k] = manifold.RollingImpulse.Dot(c.tangents[k]) + } + } + for j := 0; j < manifold.Count; j++ { + point := &manifold.Points[j] + cp := &c.points[j] + + cp.rA = point.Position.Sub(manifold.BodyA.Transform.Position) + cp.rB = point.Position.Sub(manifold.BodyB.Transform.Position) + // the point on the surface of each body (Position is halfway), then its core + half := c.normal.Mul(point.Separation / 2) + cp.coreA = cp.rA.Sub(half).Sub(c.normal.Mul(radiusA)) + cp.coreB = cp.rB.Add(half).Add(c.normal.Mul(radiusB)) + cp.baseSeparation = point.Separation - cp.coreB.Sub(cp.coreA).Dot(c.normal) + cp.normal = makeJacobian(stateA, stateB, cp.rA, cp.rB, c.normal) + + // Warm starting: the impulses of the previous step + cp.normalImpulse = point.NormalImpulse + cp.normalVelocity = relativeVelocity(stateA, stateB, cp.rA, cp.rB).Dot(c.normal) + } + c.prepareFriction(stateA, stateB, staticFriction, dynamicFriction) +} + +// prepareFriction: the friction center is the average of the points, weighted by their separation as in Box3D (a +// speculative point far from touching barely counts: 1 up to SpeculativeDistance, 0 at twice). The friction is the +// static one if the center slides slower than StaticFrictionSpeed +func (c *contactConstraint) prepareFriction(stateA, stateB *bodyState, staticFriction, dynamicFriction float64) { + manifold := c.manifold + var centerA, centerB mgl64.Vec3 + total := 0.0 + for j := 0; j < c.pointsCount; j++ { + cp := &c.points[j] + weight := 2 - manifold.Points[j].Separation/SpeculativeDistance + if weight < minFrictionWeight { + weight = minFrictionWeight + } + if weight > 1 { + weight = 1 + } + centerA = centerA.Add(cp.coreA.Mul(weight)) + centerB = centerB.Add(cp.coreB.Mul(weight)) + total += weight + } + c.centerCoreA, c.centerCoreB = centerA.Mul(1/total), centerB.Mul(1/total) + rA, rB := c.frictionArms(c.centerCoreA, c.centerCoreB) + for j := 0; j < c.pointsCount; j++ { + c.points[j].leverArm = c.points[j].rA.Sub(rA).Len() + } + c.makeFrictionRows(stateA, stateB, rA, rB) + + c.frictionImpulse = [2]float64{manifold.FrictionImpulse.Dot(c.tangents[0]), manifold.FrictionImpulse.Dot(c.tangents[1])} + c.twistImpulse = manifold.TwistImpulse + relativeVel := relativeVelocity(stateA, stateB, rA, rB) + c.friction = dynamicFriction + if relativeVel.Sub(c.normal.Mul(relativeVel.Dot(c.normal))).Len() < StaticFrictionSpeed { + c.friction = staticFriction + } +} + +// frictionArms: the lever arms of the friction center, from its cores (the radius of the rounded shapes along the normal) +func (c *contactConstraint) frictionArms(coreA, coreB mgl64.Vec3) (mgl64.Vec3, mgl64.Vec3) { + return coreA.Add(c.normal.Mul(c.radiusA)), coreB.Sub(c.normal.Mul(c.radiusB)) +} + +// makeFrictionRows: both tangents at the friction center, their 2x2 mass matrix (coupled by the rotation), and the +// mass of the twist around the normal +func (c *contactConstraint) makeFrictionRows(stateA, stateB *bodyState, rA, rB mgl64.Vec3) { + for k := range c.frictionRows { + c.frictionRows[k] = makeJacobian(stateA, stateB, rA, rB, c.tangents[k]) + } + t0, t1 := &c.frictionRows[0], &c.frictionRows[1] + linear := stateA.invMass + stateB.invMass + kxx := linear + t0.angularA.Dot(t0.impulseA) + t0.angularB.Dot(t0.impulseB) + kyy := linear + t1.angularA.Dot(t1.impulseA) + t1.angularB.Dot(t1.impulseB) + kxy := t0.angularA.Dot(t1.impulseA) + t0.angularB.Dot(t1.impulseB) + c.frictionMass = [3]float64{} + if det := kxx*kyy - kxy*kxy; det > minFrictionDeterminant { + c.frictionMass = [3]float64{kyy / det, -kxy / det, kxx / det} + } + c.twistMass = 0 + if k := c.normal.Dot(actor.MulMat3(&stateA.inverseInertia, c.normal)) + c.normal.Dot(actor.MulMat3(&stateB.inverseInertia, c.normal)); k > 0 { + c.twistMass = 1 / k + } +} + +// prepareRolling: the angular velocity given by a unit rolling impulse around both tangents, and its mass +func (c *contactConstraint) prepareRolling(stateA, stateB *bodyState) { + for k := range c.tangents { + c.rollingA[k] = actor.MulMat3(&stateA.inverseInertia, c.tangents[k]) + c.rollingB[k] = actor.MulMat3(&stateB.inverseInertia, c.tangents[k]) + c.rollingMass[k] = 0 + if mass := c.rollingA[k].Dot(c.tangents[k]) + c.rollingB[k].Dot(c.tangents[k]); mass > 0 { + c.rollingMass[k] = 1 / mass + } + } +} + +// shapeRadius is the radius of the rounded shapes, for the rolling resistance and the separation +func shapeRadius(shape actor.ShapeInterface) float64 { + switch shape := shape.(type) { + case *actor.Sphere: + return shape.Radius + case *actor.Capsule: + return shape.Radius + } + return 0 +} + +// applyRolling applies the rolling impulses λ around both tangents: -λ on A, +λ on B +func (c *contactConstraint) applyRolling(stateA, stateB *bodyState, lambda [2]float64) { + if stateA.body != nil { + stateA.angularVelocity = stateA.angularVelocity.Sub(c.rollingA[0].Mul(lambda[0])).Sub(c.rollingA[1].Mul(lambda[1])) + } + if stateB.body != nil { + stateB.angularVelocity = stateB.angularVelocity.Add(c.rollingB[0].Mul(lambda[0])).Add(c.rollingB[1].Mul(lambda[1])) + } +} + +// indexOf: the state of a body of a joint +func (s *solver) indexOf(body *actor.RigidBody) int { + if index, ok := s.indices[body]; ok { + return index + } + return -1 +} + +// stateOf fills the state of the body i +func (s *solver) stateOf(i int) { + body := s.bodies[s.stateBody[i]] + inverseInertia := body.GetInverseInertiaWorld() + s.states[i] = bodyState{ + body: body, + velocity: body.Velocity, + angularVelocity: body.AngularVelocity, + deltaRotation: mgl64.QuatIdent(), + deltaMatrix: mgl64.Ident3(), + invMass: body.InverseMass(), + inverseInertia: inverseInertia, + anisotropic: !isIsotropic(body.InertiaLocal), + } + s.starts[i] = bodyStart{inertia: inverseInertia, rotation: body.Transform.Rotation} +} + +// makeJacobian for an impulse along the direction, applied at rA and rB +func makeJacobian(stateA, stateB *bodyState, rA, rB, direction mgl64.Vec3) jacobian { + var j jacobian + j.turnA(stateA, rA, direction) + j.turnB(stateB, rB, direction) + j.updateMass(stateA, stateB) + return j +} + +// velocity of B relative to A along the direction of the jacobian +func (j *jacobian) velocity(stateA, stateB *bodyState, direction mgl64.Vec3) float64 { + vA, vB, wA, wB := &stateA.velocity, &stateB.velocity, &stateA.angularVelocity, &stateB.angularVelocity + return (vB[0]-vA[0])*direction[0] + (vB[1]-vA[1])*direction[1] + (vB[2]-vA[2])*direction[2] + + (wB[0]*j.angularB[0] + wB[1]*j.angularB[1] + wB[2]*j.angularB[2]) - + (wA[0]*j.angularA[0] + wA[1]*j.angularA[1] + wA[2]*j.angularA[2]) +} + +// apply the impulse λ along the direction: -λ on A, +λ on B. +// The static state is shared by all the static bodies: it is never written (it has no mass anyway). +// The components are written out: the vector methods go through the stack +func (j *jacobian) apply(stateA, stateB *bodyState, direction mgl64.Vec3, lambda float64) { + if stateA.body != nil { + v, w, m := &stateA.velocity, &stateA.angularVelocity, lambda*stateA.invMass + v[0], v[1], v[2] = v[0]-direction[0]*m, v[1]-direction[1]*m, v[2]-direction[2]*m + w[0], w[1], w[2] = w[0]-j.impulseA[0]*lambda, w[1]-j.impulseA[1]*lambda, w[2]-j.impulseA[2]*lambda + } + if stateB.body != nil { + v, w, m := &stateB.velocity, &stateB.angularVelocity, lambda*stateB.invMass + v[0], v[1], v[2] = v[0]+direction[0]*m, v[1]+direction[1]*m, v[2]+direction[2]*m + w[0], w[1], w[2] = w[0]+j.impulseB[0]*lambda, w[1]+j.impulseB[1]*lambda, w[2]+j.impulseB[2]*lambda + } +} + +// relativeVelocity of B relative to A, at the contact point +func relativeVelocity(stateA, stateB *bodyState, rA, rB mgl64.Vec3) mgl64.Vec3 { + vA, wA, vB, wB := &stateA.velocity, &stateA.angularVelocity, &stateB.velocity, &stateB.angularVelocity + return mgl64.Vec3{ + vB[0] + (wB[1]*rB[2] - wB[2]*rB[1]) - (vA[0] + (wA[1]*rA[2] - wA[2]*rA[1])), + vB[1] + (wB[2]*rB[0] - wB[0]*rB[2]) - (vA[1] + (wA[2]*rA[0] - wA[0]*rA[2])), + vB[2] + (wB[0]*rB[1] - wB[1]*rB[0]) - (vA[2] + (wA[0]*rA[1] - wA[1]*rA[0])), + } +} + +// currentSeparation: the contact points are not computed again during the sub-steps, +// the separation is updated from the motion of both bodies +func currentSeparation(stateA, stateB *bodyState, cp *contactPoint, normal mgl64.Vec3) float64 { + // a static body doesn't move: its core stays (its delta is the identity) + coreA := cp.coreA + if stateA.body != nil { + coreA = actor.MulMat3(&stateA.deltaMatrix, cp.coreA) + } + m, c := &stateB.deltaMatrix, cp.coreB + pA, pB := &stateA.deltaPosition, &stateB.deltaPosition + x := pB[0] - pA[0] + (m[0]*c[0] + m[3]*c[1] + m[6]*c[2]) - coreA[0] + y := pB[1] - pA[1] + (m[1]*c[0] + m[4]*c[1] + m[7]*c[2]) - coreA[1] + z := pB[2] - pA[2] + (m[2]*c[0] + m[5]*c[1] + m[8]*c[2]) - coreA[2] + return cp.baseSeparation + (x*normal[0] + y*normal[1] + z*normal[2]) +} + +// turnAnchors: the lever arms of the contacts turn with the bodies, once per substep (before Relax). +// A body turning fast (a tumbling capsule) would otherwise be pushed at the place its contact had at the beginning +// of the step: the solver would see the contact open while the body sinks. +// The core of a rounded shape turns, its radius stays along the normal. A static body doesn't turn +func (c *contactConstraint) turnAnchors(stateA, stateB *bodyState) { + turnA := stateA.body != nil && math.Abs(stateA.deltaRotation.W) < turnAnchorsCos + turnB := stateB.body != nil && math.Abs(stateB.deltaRotation.W) < turnAnchorsCos + if !turnA && !turnB { + return + } + for j := 0; j < c.pointsCount; j++ { + cp := &c.points[j] + if turnA { + cp.normal.turnA(stateA, actor.MulMat3(&stateA.deltaMatrix, cp.coreA).Add(c.normal.Mul(c.radiusA)), c.normal) + } + if turnB { + cp.normal.turnB(stateB, actor.MulMat3(&stateB.deltaMatrix, cp.coreB).Sub(c.normal.Mul(c.radiusB)), c.normal) + } + cp.normal.updateMass(stateA, stateB) + } + // the friction center turns with its bodies + coreA, coreB := c.centerCoreA, c.centerCoreB + if turnA { + coreA = actor.MulMat3(&stateA.deltaMatrix, coreA) + } + if turnB { + coreB = actor.MulMat3(&stateB.deltaMatrix, coreB) + } + rA, rB := c.frictionArms(coreA, coreB) + c.makeFrictionRows(stateA, stateB, rA, rB) + if c.rollingResistance > 0 { + c.prepareRolling(stateA, stateB) + } +} + +// turnA: the lever arm of A is rA +func (j *jacobian) turnA(stateA *bodyState, rA, direction mgl64.Vec3) { + a, m := &j.angularA, &stateA.inverseInertia + a[0], a[1], a[2] = rA[1]*direction[2]-rA[2]*direction[1], rA[2]*direction[0]-rA[0]*direction[2], rA[0]*direction[1]-rA[1]*direction[0] + j.impulseA = mgl64.Vec3{m[0]*a[0] + m[3]*a[1] + m[6]*a[2], m[1]*a[0] + m[4]*a[1] + m[7]*a[2], m[2]*a[0] + m[5]*a[1] + m[8]*a[2]} +} + +// turnB: the lever arm of B is rB +func (j *jacobian) turnB(stateB *bodyState, rB, direction mgl64.Vec3) { + b, m := &j.angularB, &stateB.inverseInertia + b[0], b[1], b[2] = rB[1]*direction[2]-rB[2]*direction[1], rB[2]*direction[0]-rB[0]*direction[2], rB[0]*direction[1]-rB[1]*direction[0] + j.impulseB = mgl64.Vec3{m[0]*b[0] + m[3]*b[1] + m[6]*b[2], m[1]*b[0] + m[4]*b[1] + m[7]*b[2], m[2]*b[0] + m[5]*b[1] + m[8]*b[2]} +} + +func (j *jacobian) updateMass(stateA, stateB *bodyState) { + j.mass = 0 + k := stateA.invMass + stateB.invMass + (j.impulseA[0]*j.angularA[0] + j.impulseA[1]*j.angularA[1] + j.impulseA[2]*j.angularA[2]) + + (j.impulseB[0]*j.angularB[0] + j.impulseB[1]*j.angularB[1] + j.impulseB[2]*j.angularB[2]) + if k > 0 { + j.mass = 1 / k + } +} + +func tangentBasis(normal mgl64.Vec3) (mgl64.Vec3, mgl64.Vec3) { + axis := mgl64.Vec3{1, 0, 0} + if math.Abs(normal.X()) > 0.57735 { + axis = mgl64.Vec3{0, 1, 0} + } + tangent1 := normal.Cross(axis).Normalize() + return tangent1, normal.Cross(tangent1) +} + +func (s *solver) integrateVelocities(gravity mgl64.Vec3) { + s.gravity = gravity + s.forEachBody(s.jobs.integrateVelocity) +} + +func (s *solver) integrateVelocity(i int) { + h, gravity := s.h, s.gravity + state := &s.states[i] + body := state.body + + linearDamping := 1 / (1 + h*body.Material.LinearDamping) + angularDamping := 1 / (1 + h*body.Material.AngularDamping) + + // ========== LINEAR ========== + v, force, invMass := &state.velocity, body.Force(), state.invMass + v[0] = v[0]*linearDamping + (gravity[0]+force[0]*invMass)*h + v[1] = v[1]*linearDamping + (gravity[1]+force[1]*invMass)*h + v[2] = v[2]*linearDamping + (gravity[2]+force[2]*invMass)*h + + // ========== ANGULAR ========== + angularVelocity := state.angularVelocity + if state.anisotropic { + angularVelocity = gyroscopic(angularVelocity, state.deltaRotation.Mul(s.starts[i].rotation).Normalize(), body.InertiaLocal, h) + } + state.angularVelocity = mgl64.Vec3{angularVelocity[0] * angularDamping, angularVelocity[1] * angularDamping, angularVelocity[2] * angularDamping} + if torque := body.Torque(); torque != (mgl64.Vec3{}) { + state.angularVelocity = state.angularVelocity.Add(actor.MulMat3(&state.inverseInertia, torque).Mul(h)) + } +} + +// gyroscopic applies the gyroscopic torque -ω × Iω, implicitly (1 Newton iteration in body space). +// Without it, a spinning body does not keep its angular momentum. +func gyroscopic(angularVelocity mgl64.Vec3, rotation mgl64.Quat, inertia mgl64.Mat3, h float64) mgl64.Vec3 { + omega := rotation.Conjugate().Rotate(angularVelocity) + inertiaOmega := inertia.Mul3x1(omega) + f := omega.Cross(inertiaOmega).Mul(h) + jacobian := inertia.Add(skew(omega).Mul3(inertia).Sub(skew(inertiaOmega)).Mul(h)) + if math.Abs(jacobian.Det()) < 1e-30 { + return angularVelocity + } + omega = omega.Sub(jacobian.Inv().Mul3x1(f)) + + return rotation.Rotate(omega) +} + +// skew returns the matrix of the cross product v × _ +func skew(v mgl64.Vec3) mgl64.Mat3 { + return mgl64.Mat3{0, v.Z(), -v.Y(), -v.Z(), 0, v.X(), v.Y(), -v.X(), 0} +} + +func (s *solver) integratePositions(dt float64) { + s.maxAngularSpeed = MaxRotation * s.invH + s.forEachBody(s.jobs.integratePosition) +} + +func (s *solver) integratePosition(i int) { + h, maxAngularSpeed := s.h, s.maxAngularSpeed + state := &s.states[i] + if state.velocity.LenSqr() > MaxLinearSpeed*MaxLinearSpeed { + state.velocity = state.velocity.Mul(MaxLinearSpeed / state.velocity.Len()) + } + if state.angularVelocity.LenSqr() > maxAngularSpeed*maxAngularSpeed { + state.angularVelocity = state.angularVelocity.Mul(maxAngularSpeed / state.angularVelocity.Len()) + } + + p, v, w := &state.deltaPosition, &state.velocity, &state.angularVelocity + p[0], p[1], p[2] = p[0]+v[0]*h, p[1]+v[1]*h, p[2]+v[2]*h + state.deltaRotation = integrateRotation(&state.deltaRotation, mgl64.Vec3{w[0] * h, w[1] * h, w[2] * h}) + state.deltaMatrix = rotationMatrix(&state.deltaRotation) + + // the inertia turns with the body, as the anchors of its contacts (turnAnchors): I⁻¹ = ΔR I⁻¹start ΔRᵀ + if state.anisotropic && math.Abs(state.deltaRotation.W) < turnAnchorsCos { + turned := actor.Mul3(&state.deltaMatrix, &s.starts[i].inertia) + transposed := actor.Transpose3(&state.deltaMatrix) + state.inverseInertia = actor.Mul3(&turned, &transposed) + } +} + +// rotationMatrix of a unit quaternion (column major) +func rotationMatrix(q *mgl64.Quat) mgl64.Mat3 { + w, x, y, z := q.W, q.V[0], q.V[1], q.V[2] + return mgl64.Mat3{ + 1 - 2*(y*y+z*z), 2 * (x*y + w*z), 2 * (x*z - w*y), + 2 * (x*y - w*z), 1 - 2*(x*x+z*z), 2 * (y*z + w*x), + 2 * (x*z + w*y), 2 * (y*z - w*x), 1 - 2*(x*x+y*y), + } +} + +// integrateRotation for a small rotation vector: q + 0.5 * θ * q, then normalized. The arithmetic of mgl64 +// (Quat.Mul, Scale, Add, Normalize), written out: the methods are not inlined +func integrateRotation(q *mgl64.Quat, theta mgl64.Vec3) mgl64.Quat { + // (0, θ) × q + qv, qw := &q.V, q.W + w := 0*qw - (theta[0]*qv[0] + theta[1]*qv[1] + theta[2]*qv[2]) + x := theta[1]*qv[2] - theta[2]*qv[1] + qv[0]*0 + theta[0]*qw + y := theta[2]*qv[0] - theta[0]*qv[2] + qv[1]*0 + theta[1]*qw + z := theta[0]*qv[1] - theta[1]*qv[0] + qv[2]*0 + theta[2]*qw + // q + 0.5 (0, θ) q + r := mgl64.Quat{W: qw + w*0.5, V: mgl64.Vec3{qv[0] + x*0.5, qv[1] + y*0.5, qv[2] + z*0.5}} + length := math.Sqrt(r.W*r.W + r.V[0]*r.V[0] + r.V[1]*r.V[1] + r.V[2]*r.V[2]) + if mgl64.FloatEqual(1, length) { + return r + } + if length == 0 { + return mgl64.QuatIdent() + } + if length == mgl64.InfPos { + length = mgl64.MaxValue + } + inverse := 1 / length + return mgl64.Quat{W: r.W * 1 / length, V: mgl64.Vec3{r.V[0] * inverse, r.V[1] * inverse, r.V[2] * inverse}} +} + +// The joints are colored with the contacts (as in Box2D v3): solved with them, in parallel; the articulations before +func (s *solver) warmStart() { + s.solveConstraints(s.jobs.warmStart, s.jobs.warmStartJoint) +} + +func (s *solver) warmStartConstraint(c *contactConstraint) { + stateA, stateB := s.state(c.indexA), s.state(c.indexB) + for j := 0; j < c.pointsCount; j++ { + cp := &c.points[j] + cp.totalNormalImpulse += cp.normalImpulse + cp.normal.apply(stateA, stateB, c.normal, cp.normalImpulse) + } + c.frictionRows[0].apply(stateA, stateB, c.tangents[0], c.frictionImpulse[0]) + c.frictionRows[1].apply(stateA, stateB, c.tangents[1], c.frictionImpulse[1]) + c.applyTwist(stateA, stateB, c.twistImpulse) + if c.rollingResistance > 0 { + c.applyRolling(stateA, stateB, c.rollingImpulse) + } +} + +// push solves the contacts with their spring, to push the overlap out. No friction (as Box3D): solved there, before +// the normals, it pushed the light bodies out from under a heavy one (at 4 substeps, 2 cubes 1 m out from under a slab +// 400 times heavier) +func (s *solver) push() { + s.solveArticulations(true) + s.solveConstraints(s.jobs.push, s.jobs.pushJoint) +} + +func (s *solver) pushConstraint(c *contactConstraint) { + stateA, stateB := s.state(c.indexA), s.state(c.indexB) + s.solveNormals(c, stateA, stateB, true) +} + +// relax solves the contacts again as rigid constraints (pushing the overlap out adds energy), then the friction +func (s *solver) relax() { + s.solveArticulations(false) + s.solveConstraints(s.jobs.relax, s.jobs.relaxJoint) +} + +func (s *solver) relaxConstraint(c *contactConstraint) { + stateA, stateB := s.state(c.indexA), s.state(c.indexB) + c.turnAnchors(stateA, stateB) + s.solveNormals(c, stateA, stateB, false) + c.solveRolling(stateA, stateB) + c.solveFriction(stateA, stateB) +} + +// ========== NORMAL ========== +// solveNormals: the points of the contact must not overlap. A speculative point (separation > 0) can get closer by its +// separation during the substep, not further. An overlapping point is pushed out by the spring of the contact (soft, +// at ContactSpeed at most), or only stopped (rigid). +// The points of a contact are solved together, exactly (block Gauss-Seidel: the block solver of Box2D v2, for 4 points). +// Solved one after the other, the first point takes more than its share and turns the body: a box landing flat starts +// to tip, and the rounding decides which way +func (s *solver) solveNormals(c *contactConstraint, stateA, stateB *bodyState, soft bool) { + n := c.pointsCount + if n == 1 { + s.solveNormal(c, stateA, stateB, soft) + return + } + var block normalBlock + block.count = n + for i := 0; i < n; i++ { + cp := &c.points[i] + separation := currentSeparation(stateA, stateB, cp, c.normal) + bias, gamma := 0.0, 0.0 + if separation > 0 { + bias = separation * s.invH + } else if soft { + bias = c.spring.biasRate * separation + if bias < -ContactSpeed { + bias = -ContactSpeed + } + gamma = c.spring.gamma + } + for j := 0; j <= i; j++ { + k := stateA.invMass + stateB.invMass + cp.normal.angularA.Dot(c.points[j].normal.impulseA) + + cp.normal.angularB.Dot(c.points[j].normal.impulseB) + block.matrix[i][j], block.matrix[j][i] = k, k + } + // the softness of the row: the fixed point of its own impulse is v + b + γ K_ii λ = 0 + block.softness[i] = gamma * block.matrix[i][i] + block.previous[i] = cp.normalImpulse + block.offset[i] = cp.normal.velocity(stateA, stateB, c.normal) + bias + } + impulses := block.solve() + for i := 0; i < n; i++ { + cp := &c.points[i] + cp.addNormalImpulse(stateA, stateB, c.normal, impulses[i]-cp.normalImpulse) + } +} + +// solveNormal: a single point (a sphere, a corner), the block of one row: λ = max(0, -r / a), the same arithmetic as +// the block solver, without its enumeration +func (s *solver) solveNormal(c *contactConstraint, stateA, stateB *bodyState, soft bool) { + cp := &c.points[0] + separation := currentSeparation(stateA, stateB, cp, c.normal) + bias, gamma := 0.0, 0.0 + if separation > 0 { + bias = separation * s.invH + } else if soft { + bias = c.spring.biasRate * separation + if bias < -ContactSpeed { + bias = -ContactSpeed + } + gamma = c.spring.gamma + } + k := stateA.invMass + stateB.invMass + cp.normal.angularA.Dot(cp.normal.impulseA) + cp.normal.angularB.Dot(cp.normal.impulseB) + previous := cp.normalImpulse + r := cp.normal.velocity(stateA, stateB, c.normal) + bias + r -= k * previous + a := k + (gamma*k + blockRegularization*k) + r -= blockRegularization * k * previous + impulse := 0.0 + if a > 0 { + if lambda := -r / a; lambda > 0 { + impulse = lambda + } + } + cp.addNormalImpulse(stateA, stateB, c.normal, impulse-previous) +} + +// normalBlock: the normal rows of a contact. Their accumulated impulses λ are the solution of the linear +// complementarity problem w = A λ + r, λ ≥ 0, w ≥ 0, λ w = 0, with A = K + D: +// - K the mass matrix of the rows (the relative velocity of the point i given by a unit impulse at the point j) +// - D the softness of the rows +// - r = v + b - K λ₀, from the velocities v and the biases b with the impulses λ₀ of the rows +type normalBlock struct { + count int + matrix [constraint.MaxContactPoints][constraint.MaxContactPoints]float64 + softness [constraint.MaxContactPoints]float64 + previous [constraint.MaxContactPoints]float64 + offset [constraint.MaxContactPoints]float64 +} + +const ( + // blockRegularization: 4 rigid points on a face give 3 independent rows only (a translation, 2 rotations): K is + // singular, the share of the load between the points is not defined. A proximal term ε W (λ - λ₀) chooses the share + // closest to the impulses the rows start from (the proximal point method: Rockafellar 1976; the proximal + // formulations of contact of Alart & Curnier 1991, Acary & Brogliato 2008). Repeated at each pass, its bias + // vanishes. ε = 1e-3 keeps A well conditioned (~1000, the bound of the block solver of Box2D v2) + blockRegularization = 1e-3 + + // blockTolerance: an impulse (N·s) or a relative velocity (m/s) within the rounding of 0 is 0 + blockTolerance = 1e-12 +) + +// solve the problem by enumerating the sets of active points, all of them first (Murty's total enumeration, as the +// block solver of Box2D v2). A is positive definite: the solution is unique, the first set found is the only one +func (b *normalBlock) solve() [constraint.MaxContactPoints]float64 { + n := b.count + var a [constraint.MaxContactPoints][constraint.MaxContactPoints]float64 + var r [constraint.MaxContactPoints]float64 + for i := 0; i < n; i++ { + r[i] = b.offset[i] + for j := 0; j < n; j++ { + a[i][j] = b.matrix[i][j] + r[i] -= b.matrix[i][j] * b.previous[j] + } + // the proximal term, towards the impulses λ₀ the rows start from + a[i][i] += b.softness[i] + blockRegularization*b.matrix[i][i] + r[i] -= blockRegularization * b.matrix[i][i] * b.previous[i] + } + + for set := (1 << n) - 1; set > 0; set-- { + if lambda, ok := solveActive(&a, &r, n, set); ok { + return lambda + } + } + // no point pushes + return [constraint.MaxContactPoints]float64{} +} + +// solveActive solves the rows of the set (a bit per point) with the others at 0, and checks the solution: the active +// impulses push, the inactive points don't get closer +func solveActive(a *[constraint.MaxContactPoints][constraint.MaxContactPoints]float64, r *[constraint.MaxContactPoints]float64, n, set int) ([constraint.MaxContactPoints]float64, bool) { + var index [constraint.MaxContactPoints]int + m := 0 + for i := 0; i < n; i++ { + if set&(1<= 0; i-- { + sum := sub[i][m] + for j := i + 1; j < m; j++ { + sum -= sub[i][j] * lambda[index[j]] + } + value := sum / sub[i][i] + if value < -blockTolerance { + return lambda, false + } + lambda[index[i]] = math.Max(value, 0) + } + for i := 0; i < n; i++ { + if set&(1< limit { + twist = limit + } else if twist < -limit { + twist = -limit + } + c.twistImpulse = twist + c.applyTwist(stateA, stateB, twist-previousTwist) + + // both tangents, together + v0 := c.frictionRows[0].velocity(stateA, stateB, c.tangents[0]) + v1 := c.frictionRows[1].velocity(stateA, stateB, c.tangents[1]) + m := c.frictionMass + previous := c.frictionImpulse + impulse := [2]float64{previous[0] - (m[0]*v0 + m[1]*v1), previous[1] - (m[1]*v0 + m[2]*v1)} + clampDisk(&impulse, c.friction*normalImpulse) + c.frictionImpulse = impulse + c.frictionRows[0].apply(stateA, stateB, c.tangents[0], impulse[0]-previous[0]) + c.frictionRows[1].apply(stateA, stateB, c.tangents[1], impulse[1]-previous[1]) +} + +// applyTwist: the angular impulse around the normal, -λ on A, +λ on B +func (c *contactConstraint) applyTwist(stateA, stateB *bodyState, lambda float64) { + t := [3]float64{c.normal[0] * lambda, c.normal[1] * lambda, c.normal[2] * lambda} + if stateA.body != nil { + w, m := &stateA.angularVelocity, &stateA.inverseInertia + w[0] -= m[0]*t[0] + m[3]*t[1] + m[6]*t[2] + w[1] -= m[1]*t[0] + m[4]*t[1] + m[7]*t[2] + w[2] -= m[2]*t[0] + m[5]*t[1] + m[8]*t[2] + } + if stateB.body != nil { + w, m := &stateB.angularVelocity, &stateB.inverseInertia + w[0] += m[0]*t[0] + m[3]*t[1] + m[6]*t[2] + w[1] += m[1]*t[0] + m[4]*t[1] + m[7]*t[2] + w[2] += m[2]*t[0] + m[5]*t[1] + m[8]*t[2] + } +} + +// clampDisk scales the 2D impulse down to the radius +func clampDisk(impulse *[2]float64, radius float64) { + length := math.Hypot(impulse[0], impulse[1]) + if length <= radius { + return + } + scale := 0.0 + if length > 0 { + scale = radius / length + } + impulse[0] *= scale + impulse[1] *= scale +} + +// ========== RESTITUTION ========== +// restitution, after the substeps: a point which hit faster than RestitutionThreshold bounces. Its impulse goes towards +// the velocity -restitution * its velocity before the step (Newton), and is at most restitution times the impulse which +// stopped it, its normal impulse of the step (Poisson's hypothesis, W. J. Stronge, Impact Mechanics): a pile of bodies +// doesn't give back more than it absorbed +func (s *solver) restitution() { + s.solveConstraints(s.jobs.restitution, nil) +} + +func (s *solver) restitutionConstraint(c *contactConstraint) { + if c.restitution == 0 { + return + } + stateA, stateB := s.state(c.indexA), s.state(c.indexB) + for j := 0; j < c.pointsCount; j++ { + cp := &c.points[j] + if cp.normalVelocity >= -RestitutionThreshold || cp.totalNormalImpulse <= 0 { + continue + } + velocity := cp.normal.velocity(stateA, stateB, c.normal) + newton := -cp.normal.mass * (velocity + c.restitution*cp.normalVelocity) + poisson := c.restitution * cp.totalNormalImpulse + if impulse := math.Min(newton, poisson); impulse > 0 { + cp.addNormalImpulse(stateA, stateB, c.normal, impulse) + } + } +} + +// storeImpulses in the manifolds, for the warm starting of the next step +func (s *solver) storeImpulses() { + s.pool.run(len(s.constraints), constraintsChunk, s.jobs.storeImpulses) +} + +func (s *solver) storeImpulsesConstraint(i int) { + c := &s.constraints[i] + c.manifold.RollingImpulse = c.tangents[0].Mul(c.rollingImpulse[0]).Add(c.tangents[1].Mul(c.rollingImpulse[1])) + for j := 0; j < c.pointsCount; j++ { + point := &c.manifold.Points[j] + cp := &c.points[j] + point.NormalImpulse = cp.normalImpulse + } + c.manifold.FrictionImpulse = c.tangents[0].Mul(c.frictionImpulse[0]).Add(c.tangents[1].Mul(c.frictionImpulse[1])) + c.manifold.TwistImpulse = c.twistImpulse +} + +// finalize writes the new transform and velocities into the bodies +func (s *solver) finalize() { + s.forEachBody(s.jobs.finalize) +} + +func (s *solver) finalizeBody(i int) { + state := &s.states[i] + body := state.body + body.Transform.Position = body.Transform.Position.Add(state.deltaPosition) + body.Transform.Rotation = state.deltaRotation.Mul(s.starts[i].rotation).Normalize() + body.Velocity = state.velocity + body.AngularVelocity = state.angularVelocity + body.ClearForces() + body.UpdateAABB() +} diff --git a/spatialgrid.go b/spatialgrid.go deleted file mode 100644 index 4b651f5..0000000 --- a/spatialgrid.go +++ /dev/null @@ -1,206 +0,0 @@ -package feather - -import ( - "math" - "sort" - "sync" - - "github.com/akmonengine/feather/actor" - "github.com/go-gl/mathgl/mgl64" -) - -// CellKey - Coordinates of a cell in 3D space -type CellKey struct { - X, Y, Z int -} - -// Cell - Container of body indices in a cell -type Cell struct { - bodyIndices []int -} - -// Pair - Pair of bodies potentially in collision -type Pair struct { - BodyA *actor.RigidBody - BodyB *actor.RigidBody -} - -// SpatialGrid - Uniform spatial grid with hashing for broad phase -type SpatialGrid struct { - cellSize float64 - cells []Cell - planes Cell -} - -// NewSpatialGrid - Creates a new spatial grid -func NewSpatialGrid(cellSize float64, numCells int) *SpatialGrid { - cells := make([]Cell, numCells) - for i := range cells { - cells[i].bodyIndices = make([]int, 0, 8) - } - - return &SpatialGrid{ - cellSize: cellSize, - cells: cells, - } -} - -// Insert - Inserts a body into all cells it occupies -func (sg *SpatialGrid) Insert(bodyIndex int, body *actor.RigidBody) { - if _, ok := body.Shape.(*actor.Plane); ok { - sg.planes.bodyIndices = append(sg.planes.bodyIndices, bodyIndex) - return - } - - aabb := body.Shape.GetAABB() - minCell := sg.worldToCell(aabb.Min) - maxCell := sg.worldToCell(aabb.Max) - - for x := minCell.X; x <= maxCell.X; x++ { - for y := minCell.Y; y <= maxCell.Y; y++ { - for z := minCell.Z; z <= maxCell.Z; z++ { - cellKey := CellKey{x, y, z} - cellIdx := sg.hashCell(cellKey) - - sg.cells[cellIdx].bodyIndices = append( - sg.cells[cellIdx].bodyIndices, - bodyIndex, - ) - } - } - } -} - -// Clear - Resets the spatial grid by clearing all body indices from cells and planes -func (sg *SpatialGrid) Clear() { - sg.planes.bodyIndices = sg.planes.bodyIndices[:0] - - for i := range sg.cells { - sg.cells[i].bodyIndices = sg.cells[i].bodyIndices[:0] - } -} - -// SortCells - Sorts body indices within each cell for optimized collision detection -func (sg *SpatialGrid) SortCells() { - for i := range sg.cells { - if len(sg.cells[i].bodyIndices) > 1 { - sort.Ints(sg.cells[i].bodyIndices) - } - } -} - -// FindPairsParallel - Parallel version returning a channel -func (sg *SpatialGrid) FindPairsParallel(bodies []*actor.RigidBody, workersCount int) <-chan Pair { - var wg sync.WaitGroup - pairsChan := make(chan Pair, workersCount*10) - clearSeen := make([]bool, len(bodies)) - - dataSize := len(bodies) - chunkSize := (dataSize + workersCount - 1) / workersCount - for workerID := 0; workerID < workersCount; workerID++ { - wg.Add(1) - - go func(start, end int) { - defer wg.Done() - - seen := make([]bool, len(bodies)) - for bodyIdx := start; bodyIdx < end; bodyIdx++ { - if _, isPlane := bodies[bodyIdx].Shape.(*actor.Plane); isPlane { - continue - } - bodyA := bodies[bodyIdx] - - // write all planes/body collisions - for _, planeId := range sg.planes.bodyIndices { - pairsChan <- Pair{BodyA: bodies[planeId], BodyB: bodyA} - } - - copy(seen, clearSeen) - - // Find cells occupied by bodyA - minCell := sg.worldToCell(bodyA.Shape.GetAABB().Min) - maxCell := sg.worldToCell(bodyA.Shape.GetAABB().Max) - - // Iterate through these cells - for x := minCell.X; x <= maxCell.X; x++ { - for y := minCell.Y; y <= maxCell.Y; y++ { - for z := minCell.Z; z <= maxCell.Z; z++ { - cellKey := CellKey{x, y, z} - cellIdx := sg.hashCell(cellKey) - - // Test against all bodies in this cell - for _, otherIdx := range sg.cells[cellIdx].bodyIndices { - // Avoid duplicates - if otherIdx <= bodyIdx || seen[otherIdx] { - continue - } - seen[otherIdx] = true - - bodyB := bodies[otherIdx] - if bodyA.BodyType == actor.BodyTypeStatic && bodyB.BodyType == actor.BodyTypeStatic { - continue - } - if bodyA.IsSleeping && bodyB.IsSleeping { - continue - } - - if bodyA.Shape.GetAABB().Overlaps(bodyB.Shape.GetAABB()) { - pairsChan <- Pair{BodyA: bodyA, BodyB: bodyB} - } - } - } - } - } - } - }(workerID*chunkSize, min((workerID+1)*chunkSize, dataSize)) - } - - go func() { - wg.Wait() - close(pairsChan) - }() - - return pairsChan -} - -// worldToCell - Converts a world position to cell coordinates -func (sg *SpatialGrid) worldToCell(pos mgl64.Vec3) CellKey { - return CellKey{ - X: int(math.Floor(pos.X() / sg.cellSize)), - Y: int(math.Floor(pos.Y() / sg.cellSize)), - Z: int(math.Floor(pos.Z() / sg.cellSize)), - } -} - -// hashCell - Hashes a cell to an index in the array -// Uses a hash function inspired by MurmurHash3 for better distribution -// and to reduce collisions. The constants used are known prime numbers -// for their good bit mixing properties. -func (sg *SpatialGrid) hashCell(key CellKey) int { - // Mixing constants inspired by MurmurHash3 - // These values were chosen empirically for their diffusion properties - const ( - prime1 = uint32(16777619) // First prime number for initial mixing - prime2 = uint32(2166136261) // Second prime number for mixing - prime3 = uint32(1681692777) // Third prime number for mixing - - // Constants for final mixing (avalanche effect) - mix1 = uint32(0x85ebca6b) // Mixing constant for bit diffusion - mix2 = uint32(0xc2b2ae35) // Second mixing constant - ) - - // Conversion to uint32 to avoid unexpected overflows - h := uint32(key.X) * prime1 - h = (h ^ uint32(key.Y)) * prime2 - h = (h ^ uint32(key.Z)) * prime3 - - // Final mixing to improve distribution (avalanche effect) - // This sequence creates complete bit diffusion to reduce collisions - h ^= h >> 16 - h *= mix1 - h ^= h >> 13 - h *= mix2 - h ^= h >> 16 - - return int(h) % len(sg.cells) -} diff --git a/spatialgrid_test.go b/spatialgrid_test.go deleted file mode 100644 index 8fe6771..0000000 --- a/spatialgrid_test.go +++ /dev/null @@ -1,574 +0,0 @@ -package feather - -import ( - "sort" - "testing" - - "github.com/akmonengine/feather/actor" - "github.com/go-gl/mathgl/mgl64" -) - -func TestWorldToCell(t *testing.T) { - grid := NewSpatialGrid(1.0, 16) - - tests := []struct { - name string - position mgl64.Vec3 - expected CellKey - }{ - {"origine", mgl64.Vec3{0, 0, 0}, CellKey{0, 0, 0}}, - {"positif", mgl64.Vec3{1.5, 2.3, 3.7}, CellKey{1, 2, 3}}, - {"negatif", mgl64.Vec3{-1.5, -2.3, -3.7}, CellKey{-2, -3, -4}}, - {"fractionnaire", mgl64.Vec3{0.5, 0.5, 0.5}, CellKey{0, 0, 0}}, - {"grand", mgl64.Vec3{100.7, -200.3, 50.1}, CellKey{100, -201, 50}}, - } - - for _, tt := range tests { - t.Run(tt.name, func(t *testing.T) { - result := grid.worldToCell(tt.position) - if result != tt.expected { - t.Errorf("worldToCell(%v) = %v, want %v", tt.position, result, tt.expected) - } - }) - } -} - -func TestHashCell(t *testing.T) { - grid := NewSpatialGrid(1.0, 16) // 16 cellules, mask = 15 - - tests := []struct { - name string - key CellKey - expected int - }{ - {"origine", CellKey{0, 0, 0}, 0}, - {"simple", CellKey{1, 2, 3}, 0}, - {"negatif", CellKey{-1, -2, -3}, 13}, - {"grand", CellKey{100, 200, 300}, 14}, - } - - for _, tt := range tests { - t.Run(tt.name, func(t *testing.T) { - result := grid.hashCell(tt.key) - // Vérifier que le résultat est dans la plage valide - if result < 0 || result >= len(grid.cells) { - t.Errorf("hashCell(%v) = %d, out of range [0, %d)", tt.key, result, len(grid.cells)) - } - // Vérifier la valeur exacte (tous les cas maintenant) - if true { - if result != tt.expected { - t.Errorf("hashCell(%v) = %d, want %d", tt.key, result, tt.expected) - } - } - }) - } -} - -func TestHashCellDistribution(t *testing.T) { - grid := NewSpatialGrid(1.0, 1024) // Grande grille pour tester la distribution - - // Créer beaucoup de clés aléatoires et vérifier la distribution - cellCounts := make(map[int]int) - for x := -100; x <= 100; x++ { - for y := -100; y <= 100; y++ { - for z := -100; z <= 100; z++ { - key := CellKey{x, y, z} - hash := grid.hashCell(key) - cellCounts[hash]++ - } - } - } - - // Vérifier que la distribution est raisonnable - minCount := int(^uint(0) >> 1) - maxCount := 0 - for _, count := range cellCounts { - if count < minCount { - minCount = count - } - if count > maxCount { - maxCount = count - } - } - - t.Logf("Hash distribution: min=%d, max=%d, avg=%.1f", minCount, maxCount, float64(201*201*201)/float64(len(cellCounts))) - - // La distribution devrait être relativement uniforme - // Le ratio max/min ne devrait pas être trop élevé - ratio := float64(maxCount) / float64(minCount) - if ratio > 2.0 { - t.Logf("Warning: hash distribution ratio is %.1f, expected < 2.0", ratio) - } -} - -func createTestBox(position mgl64.Vec3, halfExtents mgl64.Vec3) *actor.RigidBody { - return actor.NewRigidBody( - actor.Transform{Position: position, Rotation: mgl64.QuatIdent()}, - &actor.Box{HalfExtents: halfExtents}, - actor.BodyTypeDynamic, - 1.0, - ) -} - -func createTestPlane() *actor.RigidBody { - return actor.NewRigidBody( - actor.Transform{Position: mgl64.Vec3{0, 0, 0}, Rotation: mgl64.QuatIdent()}, - &actor.Plane{Normal: mgl64.Vec3{0, 1, 0}, Distance: 0}, - actor.BodyTypeStatic, - 0.0, - ) -} - -func TestInsertSingleBody(t *testing.T) { - grid := NewSpatialGrid(1.0, 16) - body := createTestBox(mgl64.Vec3{1.5, 2.5, 3.5}, mgl64.Vec3{0.4, 0.4, 0.4}) - - grid.Insert(0, body) - - // Vérifier que le body est dans la bonne cellule - minCell := grid.worldToCell(body.Shape.GetAABB().Min) - maxCell := grid.worldToCell(body.Shape.GetAABB().Max) - - found := false - for x := minCell.X; x <= maxCell.X; x++ { - for y := minCell.Y; y <= maxCell.Y; y++ { - for z := minCell.Z; z <= maxCell.Z; z++ { - cellKey := CellKey{x, y, z} - cellIdx := grid.hashCell(cellKey) - for _, idx := range grid.cells[cellIdx].bodyIndices { - if idx == 0 { - found = true - break - } - } - if found { - break - } - } - if found { - break - } - } - if found { - break - } - } - - if !found { - t.Error("Body not found in any cell after insertion") - } -} - -func TestInsertMultipleBodies(t *testing.T) { - grid := NewSpatialGrid(1.0, 16) - bodies := []*actor.RigidBody{ - createTestBox(mgl64.Vec3{1.0, 1.0, 1.0}, mgl64.Vec3{0.4, 0.4, 0.4}), - createTestBox(mgl64.Vec3{2.0, 2.0, 2.0}, mgl64.Vec3{0.4, 0.4, 0.4}), - createTestBox(mgl64.Vec3{3.0, 3.0, 3.0}, mgl64.Vec3{0.4, 0.4, 0.4}), - } - - for i, body := range bodies { - grid.Insert(i, body) - } - - // Vérifier que tous les bodies sont insérés - for i, body := range bodies { - found := false - minCell := grid.worldToCell(body.Shape.GetAABB().Min) - maxCell := grid.worldToCell(body.Shape.GetAABB().Max) - - for x := minCell.X; x <= maxCell.X; x++ { - for y := minCell.Y; y <= maxCell.Y; y++ { - for z := minCell.Z; z <= maxCell.Z; z++ { - cellKey := CellKey{x, y, z} - cellIdx := grid.hashCell(cellKey) - for _, idx := range grid.cells[cellIdx].bodyIndices { - if idx == i { - found = true - break - } - } - if found { - break - } - } - if found { - break - } - } - if found { - break - } - } - - if !found { - t.Errorf("Body %d not found in any cell after insertion", i) - } - } -} - -func TestInsertPlane(t *testing.T) { - grid := NewSpatialGrid(1.0, 16) - plane := createTestPlane() - - grid.Insert(0, plane) - - // Les planes devraient être dans la cellule spéciale planes - if len(grid.planes.bodyIndices) != 1 || grid.planes.bodyIndices[0] != 0 { - t.Error("Plane not correctly inserted into planes cell") - } - - // Vérifier qu'aucun body n'est dans les cellules régulières - for _, cell := range grid.cells { - if len(cell.bodyIndices) > 0 { - t.Error("Regular cells should be empty when inserting plane") - } - } -} - -func TestClear(t *testing.T) { - grid := NewSpatialGrid(1.0, 16) - bodies := []*actor.RigidBody{ - createTestBox(mgl64.Vec3{1.0, 1.0, 1.0}, mgl64.Vec3{0.4, 0.4, 0.4}), - createTestBox(mgl64.Vec3{2.0, 2.0, 2.0}, mgl64.Vec3{0.4, 0.4, 0.4}), - } - - // Insérer des bodies - for i, body := range bodies { - grid.Insert(i, body) - } - - // Vérifier que les bodies sont présents - if len(grid.cells[grid.hashCell(grid.worldToCell(bodies[0].Shape.GetAABB().Min))].bodyIndices) == 0 { - t.Error("Bodies should be present before clear") - } - - // Clear - grid.Clear() - - // Vérifier que tout est vide - if len(grid.planes.bodyIndices) != 0 { - t.Error("Planes cell should be empty after clear") - } - - for _, cell := range grid.cells { - if len(cell.bodyIndices) != 0 { - t.Error("Cells should be empty after clear") - } - } -} - -func TestSortCells(t *testing.T) { - grid := NewSpatialGrid(1.0, 16) - - // Insérer des bodies dans la même cellule dans un ordre aléatoire - bodyIndices := []int{5, 2, 8, 1, 9, 3} - cellIdx := 0 // Utiliser la première cellule - grid.cells[cellIdx].bodyIndices = append(grid.cells[cellIdx].bodyIndices, bodyIndices...) - - // Trier - grid.SortCells() - - // Vérifier que la cellule est triée - if !sort.IntsAreSorted(grid.cells[cellIdx].bodyIndices) { - t.Error("Cell indices should be sorted") - } - - // Vérifier que les indices sont corrects - expected := []int{1, 2, 3, 5, 8, 9} - for i, idx := range grid.cells[cellIdx].bodyIndices { - if idx != expected[i] { - t.Errorf("Expected index %d at position %d, got %d", expected[i], i, idx) - } - } -} - -func TestFindPairsParallelNoCollision(t *testing.T) { - grid := NewSpatialGrid(1.0, 16) - bodies := []*actor.RigidBody{ - createTestBox(mgl64.Vec3{0, 0, 0}, mgl64.Vec3{0.4, 0.4, 0.4}), - createTestBox(mgl64.Vec3{10, 10, 10}, mgl64.Vec3{0.4, 0.4, 0.4}), - } - - // Insérer les bodies - for i, body := range bodies { - grid.Insert(i, body) - } - - // Trouver les paires avec la version parallèle - pairs := make([]Pair, 0) - for pair := range grid.FindPairsParallel(bodies, 2) { - pairs = append(pairs, pair) - } - - // Ne devrait pas avoir de collision (pas de planes dans ce test) - if len(pairs) != 0 { - t.Errorf("Expected 0 pairs, got %d", len(pairs)) - } -} - -func TestFindPairsParallelWithCollision(t *testing.T) { - grid := NewSpatialGrid(1.0, 16) - bodies := []*actor.RigidBody{ - createTestBox(mgl64.Vec3{0, 0, 0}, mgl64.Vec3{0.4, 0.4, 0.4}), - createTestBox(mgl64.Vec3{0.5, 0.5, 0.5}, mgl64.Vec3{0.4, 0.4, 0.4}), - } - - // Insérer les bodies - for i, body := range bodies { - grid.Insert(i, body) - } - - // Trouver les paires avec la version parallèle - pairs := make([]Pair, 0) - for pair := range grid.FindPairsParallel(bodies, 2) { - pairs = append(pairs, pair) - } - - // Devrait avoir une collision - if len(pairs) != 1 { - t.Errorf("Expected 1 pair, got %d", len(pairs)) - } - - // Vérifier que c'est la bonne paire - foundCorrectPair := false - for _, pair := range pairs { - if (pair.BodyA == bodies[0] && pair.BodyB == bodies[1]) || (pair.BodyA == bodies[1] && pair.BodyB == bodies[0]) { - foundCorrectPair = true - break - } - } - if !foundCorrectPair { - t.Error("Correct pair not found") - } -} - -func TestFindPairsParallelWithPlane(t *testing.T) { - grid := NewSpatialGrid(1.0, 16) - plane := createTestPlane() - body := createTestBox(mgl64.Vec3{0, 5, 0}, mgl64.Vec3{0.4, 0.4, 0.4}) - - bodies := []*actor.RigidBody{plane, body} // Inclure le plane dans la liste des bodies - - // Insérer le plane et le body - grid.Insert(0, plane) - grid.Insert(1, body) - - // Trouver les paires avec la version parallèle - pairs := make([]Pair, 0) - for pair := range grid.FindPairsParallel(bodies, 2) { - pairs = append(pairs, pair) - } - - // Devrait détecter la paire plane-body (TOUJOURS ajoutée sans test de collision) - // Note: Le plane est dans la liste des bodies, donc il sera traité normalement - // mais aussi via la logique spéciale des planes - if len(pairs) != 1 { - t.Errorf("Expected 1 pair with plane, got %d", len(pairs)) - } - - // Vérifier que c'est la bonne paire - foundCorrectPair := false - for _, pair := range pairs { - if (pair.BodyA == plane && pair.BodyB == body) || (pair.BodyA == body && pair.BodyB == plane) { - foundCorrectPair = true - break - } - } - if !foundCorrectPair { - t.Error("Correct plane-body pair not found") - } -} - -func TestFindPairsParallelStaticBodies(t *testing.T) { - grid := NewSpatialGrid(1.0, 16) - staticBody1 := actor.NewRigidBody( - actor.Transform{Position: mgl64.Vec3{0, 0, 0}, Rotation: mgl64.QuatIdent()}, - &actor.Box{HalfExtents: mgl64.Vec3{0.4, 0.4, 0.4}}, - actor.BodyTypeStatic, - 0.0, - ) - staticBody2 := actor.NewRigidBody( - actor.Transform{Position: mgl64.Vec3{0.5, 0.5, 0.5}, Rotation: mgl64.QuatIdent()}, - &actor.Box{HalfExtents: mgl64.Vec3{0.4, 0.4, 0.4}}, - actor.BodyTypeStatic, - 0.0, - ) - - bodies := []*actor.RigidBody{staticBody1, staticBody2} - - // Insérer les bodies - for i, body := range bodies { - grid.Insert(i, body) - } - - // Trouver les paires avec la version parallèle - pairs := make([]Pair, 0) - for pair := range grid.FindPairsParallel(bodies, 2) { - pairs = append(pairs, pair) - } - - // Ne devrait pas détecter de collision entre bodies statiques - if len(pairs) != 0 { - t.Errorf("Expected 0 pairs for static bodies, got %d", len(pairs)) - } -} - -func TestFindPairsParallelSleepingBodies(t *testing.T) { - grid := NewSpatialGrid(1.0, 16) - body1 := createTestBox(mgl64.Vec3{0, 0, 0}, mgl64.Vec3{0.4, 0.4, 0.4}) - body2 := createTestBox(mgl64.Vec3{0.5, 0.5, 0.5}, mgl64.Vec3{0.4, 0.4, 0.4}) - - body1.IsSleeping = true - body2.IsSleeping = true - - bodies := []*actor.RigidBody{body1, body2} - - // Insérer les bodies - for i, body := range bodies { - grid.Insert(i, body) - } - - // Trouver les paires avec la version parallèle - pairs := make([]Pair, 0) - for pair := range grid.FindPairsParallel(bodies, 2) { - pairs = append(pairs, pair) - } - - // Ne devrait pas détecter de collision entre bodies endormis - if len(pairs) != 0 { - t.Errorf("Expected 0 pairs for sleeping bodies, got %d", len(pairs)) - } -} - -func TestFindPairsParallelMultiplePlanes(t *testing.T) { - grid := NewSpatialGrid(1.0, 16) - plane1 := createTestPlane() - plane2 := actor.NewRigidBody( - actor.Transform{Position: mgl64.Vec3{0, 0, 0}, Rotation: mgl64.QuatIdent()}, - &actor.Plane{Normal: mgl64.Vec3{0, 1, 0}, Distance: 0}, - actor.BodyTypeStatic, - 0.0, - ) - body := createTestBox(mgl64.Vec3{0, 5, 0}, mgl64.Vec3{0.4, 0.4, 0.4}) - - bodies := []*actor.RigidBody{plane1, plane2, body} // Inclure les planes dans la liste des bodies - - // Insérer les planes et le body - grid.Insert(0, plane1) - grid.Insert(1, plane2) - grid.Insert(2, body) - - // Trouver les paires avec la version parallèle - pairs := make([]Pair, 0) - for pair := range grid.FindPairsParallel(bodies, 2) { - pairs = append(pairs, pair) - } - - // Devrait détecter les paires avec les deux planes (TOUJOURS ajoutées sans test de collision) - // Note: Les planes sont dans la liste des bodies, donc ils seront traités normalement - // mais aussi via la logique spéciale des planes - if len(pairs) != 2 { - t.Errorf("Expected 2 pairs with planes, got %d", len(pairs)) - } - - // Vérifier que les deux paires plane-body sont présentes - foundPlane1 := false - foundPlane2 := false - for _, pair := range pairs { - if (pair.BodyA == plane1 && pair.BodyB == body) || (pair.BodyA == body && pair.BodyB == plane1) { - foundPlane1 = true - } - if (pair.BodyA == plane2 && pair.BodyB == body) || (pair.BodyA == body && pair.BodyB == plane2) { - foundPlane2 = true - } - } - if !foundPlane1 || !foundPlane2 { - t.Error("Both plane-body pairs should be found") - } -} - -// ============================================================================ -// Tests pour les cas limites -// ============================================================================ - -func TestBoundaryCases(t *testing.T) { - grid := NewSpatialGrid(1.0, 16) - - // Body exactement sur la frontière entre deux cellules - body := createTestBox(mgl64.Vec3{1.0, 1.0, 1.0}, mgl64.Vec3{0.5, 0.5, 0.5}) - - grid.Insert(0, body) - - // Vérifier que le body est dans les cellules attendues - minCell := grid.worldToCell(body.Shape.GetAABB().Min) - maxCell := grid.worldToCell(body.Shape.GetAABB().Max) - - // Devrait couvrir 2 cellules dans chaque dimension - if maxCell.X-minCell.X != 1 || maxCell.Y-minCell.Y != 1 || maxCell.Z-minCell.Z != 1 { - t.Errorf("Expected body to span 2 cells in each dimension, got %d, %d, %d", - maxCell.X-minCell.X, maxCell.Y-minCell.Y, maxCell.Z-minCell.Z) - } -} - -func TestLargeBodySpanningManyCells(t *testing.T) { - grid := NewSpatialGrid(1.0, 16) - - // Body très large couvrant beaucoup de cellules - body := createTestBox(mgl64.Vec3{0, 0, 0}, mgl64.Vec3{5.0, 5.0, 5.0}) - - grid.Insert(0, body) - - // Vérifier que le body est dans toutes les cellules attendues - minCell := grid.worldToCell(body.Shape.GetAABB().Min) - maxCell := grid.worldToCell(body.Shape.GetAABB().Max) - - expectedCells := (maxCell.X - minCell.X + 1) * (maxCell.Y - minCell.Y + 1) * (maxCell.Z - minCell.Z + 1) - actualCells := 0 - - for x := minCell.X; x <= maxCell.X; x++ { - for y := minCell.Y; y <= maxCell.Y; y++ { - for z := minCell.Z; z <= maxCell.Z; z++ { - cellKey := CellKey{x, y, z} - cellIdx := grid.hashCell(cellKey) - for _, idx := range grid.cells[cellIdx].bodyIndices { - if idx == 0 { - actualCells++ - break - } - } - } - } - } - - if actualCells != expectedCells { - t.Errorf("Expected body in %d cells, found in %d cells", expectedCells, actualCells) - } -} - -func BenchmarkFindPairsParallel(b *testing.B) { - grid := NewSpatialGrid(1.0, 1024) - bodies := make([]*actor.RigidBody, 100) - - // Créer des bodies aléatoires - for i := range bodies { - pos := mgl64.Vec3{ - float64(i%10) * 2.0, - float64((i/10)%10) * 2.0, - float64((i/100)%10) * 2.0, - } - bodies[i] = createTestBox(pos, mgl64.Vec3{0.4, 0.4, 0.4}) - } - - // Insérer les bodies - for i, body := range bodies { - grid.Insert(i, body) - } - - b.ResetTimer() - for i := 0; i < b.N; i++ { - for range grid.FindPairsParallel(bodies, 4) { - // Consume the channel - } - } -} diff --git a/tree.go b/tree.go new file mode 100644 index 0000000..5d8fd1d --- /dev/null +++ b/tree.go @@ -0,0 +1,784 @@ +package feather + +import ( + "slices" + + "github.com/akmonengine/feather/actor" + "github.com/go-gl/mathgl/mgl64" +) + +// ========== BROAD PHASE ========== +// The broad phase is a pair of dynamic AABB trees (Catto, "Dynamic Bounding Volume Hierarchies", GDC 2019; the +// b2DynamicTree of Box2D, the btDbvt of Bullet), one per kind of body as the trees of Box2D v3: one for the static +// bodies, updated when a body is added, removed or moved by the game, one for the dynamic bodies, awake or asleep. +// A dynamic body is stored with its AABB enlarged by AABBMargin: a body which moves inside its enlarged AABB doesn't +// touch the tree, a sleeping body never does. The planes and the heightfields are not in the trees: they are tested +// against every awake body. +// +// The pairs of overlapping stored AABBs are kept from a step to the next (see findPairs): only a body put back in a +// tree queries it. The pairs of the step are those whose exact AABBs overlap, sorted by the index of the first body, +// the planes of a body before its other pairs. + +const ( + // AABBMargin: the AABB of a dynamic body is enlarged by this margin in the tree (m). Larger: fewer updates of the + // tree, more candidates per query. 0.1 m as Box2D v2.4 (v3 uses 0.05 m) + AABBMargin = 0.1 + + nullNode = -1 +) + +// Pair of bodies potentially in collision, BodyA of the lower index (a plane first) +type Pair struct { + BodyA *actor.RigidBody + BodyB *actor.RigidBody + // the indices of the bodies in World.Bodies + IndexA, IndexB int32 + // the sort keys: the index of the body owning the pair, the index of the other body or of the plane + first, second int32 + plane bool + // slot of the pair in the records of the broad phase, for the contacts of the previous step + slot int32 +} + +type treeNode struct { + aabb actor.AABB + parent int32 + child1 int32 + child2 int32 + height int32 // 0 for a leaf + body int32 // index of the body (leaves) +} + +// aabbTree: a binary tree of AABBs, the bodies at its leaves, each node the union of its children. A leaf is inserted +// next to the sibling which enlarges the tree the least (the surface area heuristic: the area of a node is the chance a +// query visits it), found down the tree, then the ancestors are refitted and each of them tries the rotation which +// shrinks it the most (Catto, "Dynamic Bounding Volume Hierarchies", GDC 2019). The heights are kept for the leaves +// (0) and the tests; the balance comes from the areas, not from the heights +type aabbTree struct { + nodes []treeNode + root int32 + free []int32 // released nodes, reused first +} + +func (t *aabbTree) allocate() int32 { + if n := len(t.free); n > 0 { + index := t.free[n-1] + t.free = t.free[:n-1] + t.nodes[index] = treeNode{parent: nullNode, child1: nullNode, child2: nullNode, body: nullNode} + return index + } + t.nodes = append(t.nodes, treeNode{parent: nullNode, child1: nullNode, child2: nullNode, body: nullNode}) + return int32(len(t.nodes) - 1) +} + +func (t *aabbTree) release(n int32) { + t.free = append(t.free, n) +} + +func (t *aabbTree) clear() { + t.nodes = t.nodes[:0] + t.free = t.free[:0] + t.root = nullNode +} + +// insert a leaf for the body with the AABB, returns the node +func (t *aabbTree) insert(aabb actor.AABB, body int32) int32 { + leaf := t.allocate() + t.nodes[leaf].aabb = aabb + t.nodes[leaf].body = body + t.insertLeaf(leaf) + return leaf +} + +// remove the leaf +func (t *aabbTree) remove(leaf int32) { + t.removeLeaf(leaf) + t.release(leaf) +} + +// surfaceArea of an AABB: the cost of a node (the probability a query visits it) +func surfaceArea(a actor.AABB) float64 { + d := a.Max.Sub(a.Min) + return 2 * (d.X()*d.Y() + d.Y()*d.Z() + d.Z()*d.X()) +} + +func union(a, b actor.AABB) actor.AABB { + return actor.AABB{ + Min: mgl64.Vec3{min(a.Min.X(), b.Min.X()), min(a.Min.Y(), b.Min.Y()), min(a.Min.Z(), b.Min.Z())}, + Max: mgl64.Vec3{max(a.Max.X(), b.Max.X()), max(a.Max.Y(), b.Max.Y()), max(a.Max.Z(), b.Max.Z())}, + } +} + +func contains(outer, inner actor.AABB) bool { + return outer.Min.X() <= inner.Min.X() && outer.Min.Y() <= inner.Min.Y() && outer.Min.Z() <= inner.Min.Z() && + inner.Max.X() <= outer.Max.X() && inner.Max.Y() <= outer.Max.Y() && inner.Max.Z() <= outer.Max.Z() +} + +// insertLeaf under the best sibling, then refits and rotates the ancestors +func (t *aabbTree) insertLeaf(leaf int32) { + if t.root == nullNode { + t.root = leaf + return + } + sibling := t.bestSibling(t.nodes[leaf].aabb) + + // a new node takes the place of the sibling, with the sibling and the leaf under it + above := t.nodes[sibling].parent + pair := t.allocate() + t.nodes[pair].parent = above + t.link(pair, sibling, leaf) + if above == nullNode { + t.root = pair + } else if t.nodes[above].child1 == sibling { + t.nodes[above].child1 = pair + } else { + t.nodes[above].child2 = pair + } + // the new node is fitted by link: its ancestors grow + t.refitUp(above) +} + +// bestSibling for a leaf: the node whose pairing with the leaf costs the least, the cost being the area of their union +// plus the growth of every ancestor. The tree is descended greedily: at each node, the leaf stops there if pairing with +// the node beats what any subtree of its children can reach (under a child, the leaf pairs with the child or with a +// node below it, which costs at least the growth of the child plus the area of the leaf), else it goes under the child +// with the better reach. O(log n), the tree quality of the surface area heuristic (Catto, GDC 2019) +func (t *aabbTree) bestSibling(aabb actor.AABB) int32 { + leafArea := surfaceArea(aabb) + node, inherited := t.root, 0.0 + for { + n := &t.nodes[node] + joined := surfaceArea(union(n.aabb, aabb)) + if n.height == 0 { + return node + } + here := joined + inherited + growth := inherited + (joined - surfaceArea(n.aabb)) + reach1, reach2 := t.reach(n.child1, aabb, growth, leafArea), t.reach(n.child2, aabb, growth, leafArea) + if here <= min(reach1, reach2) { + return node + } + if reach1 < reach2 { + node = n.child1 + } else { + node = n.child2 + } + inherited = growth + } +} + +// reach: the least a pairing under the child can cost, the ancestors having grown by growth +func (t *aabbTree) reach(child int32, aabb actor.AABB, growth, leafArea float64) float64 { + c := &t.nodes[child] + joined := surfaceArea(union(c.aabb, aabb)) + pairing := joined + growth + if c.height == 0 { + return pairing + } + return min(pairing, growth+(joined-surfaceArea(c.aabb))+leafArea) +} + +// removeLeaf: its sibling takes the place of their parent, the ancestors shrink +func (t *aabbTree) removeLeaf(leaf int32) { + if leaf == t.root { + t.root = nullNode + return + } + pair := t.nodes[leaf].parent + sibling := t.nodes[pair].child1 + if sibling == leaf { + sibling = t.nodes[pair].child2 + } + above := t.nodes[pair].parent + t.nodes[sibling].parent = above + if above == nullNode { + t.root = sibling + } else { + if t.nodes[above].child1 == pair { + t.nodes[above].child1 = sibling + } else { + t.nodes[above].child2 = sibling + } + t.refitUp(above) + } + t.release(pair) +} + +// link the children to the node, and refit it +func (t *aabbTree) link(node, child1, child2 int32) { + t.nodes[node].child1, t.nodes[node].child2 = child1, child2 + t.nodes[child1].parent, t.nodes[child2].parent = node, node + t.refit(node) +} + +// refit the node on its children: its AABB and its height +func (t *aabbTree) refit(node int32) { + n := &t.nodes[node] + c1, c2 := &t.nodes[n.child1], &t.nodes[n.child2] + n.aabb = union(c1.aabb, c2.aabb) + n.height = 1 + max(c1.height, c2.height) +} + +// refitUp: the node and its ancestors are refitted, and each tries a rotation, up to the first ancestor which doesn't +// change: the ones above it don't change either, and their rotations were tried when they last changed +func (t *aabbTree) refitUp(node int32) { + for ; node != nullNode; node = t.nodes[node].parent { + before := t.nodes[node] + t.refit(node) + if n := &t.nodes[node]; n.aabb == before.aabb && n.height == before.height { + return + } + t.rotate(node) + } +} + +// rotate: among the 4 exchanges of a child of the node with a grandchild, the one which shrinks the other child the +// most (the child losing a grandchild takes the exchanged child instead). The AABB of the node itself doesn't change +func (t *aabbTree) rotate(node int32) { + n := &t.nodes[node] + if n.height < 2 { + return + } + children := [2]int32{n.child1, n.child2} + bestGain, bestChild, bestGrandchild := 0.0, int32(nullNode), int32(nullNode) + for k, child := range children { + other := &t.nodes[children[1-k]] + if other.height == 0 { + continue + } + // the child goes under the other child, in the place of one of its grandchildren + grandchildren := [2]int32{other.child1, other.child2} + for g, grandchild := range grandchildren { + kept := t.nodes[grandchildren[1-g]].aabb + shrunk := surfaceArea(union(kept, t.nodes[child].aabb)) + if gain := surfaceArea(other.aabb) - shrunk; gain > bestGain { + bestGain, bestChild, bestGrandchild = gain, child, grandchild + } + } + } + if bestChild == nullNode { + return + } + // exchange: the grandchild becomes a child of the node, the child a child of the other child + other := t.nodes[bestGrandchild].parent + if n.child1 == bestChild { + n.child1 = bestGrandchild + } else { + n.child2 = bestGrandchild + } + t.nodes[bestGrandchild].parent = node + o := &t.nodes[other] + if o.child1 == bestGrandchild { + o.child1 = bestChild + } else { + o.child2 = bestChild + } + t.nodes[bestChild].parent = other + t.refit(other) + t.refit(node) +} + +// query appends the bodies of the leaves overlapping the AABB to out, in the order of the traversal. stack is reused +func (t *aabbTree) query(aabb actor.AABB, stack []int32, out []int32) ([]int32, []int32) { + if t.root == nullNode { + return stack, out + } + stack = append(stack[:0], t.root) + for len(stack) > 0 { + n := stack[len(stack)-1] + stack = stack[:len(stack)-1] + node := &t.nodes[n] + if !node.aabb.Overlaps(aabb) { + continue + } + if node.height == 0 { + out = append(out, node.body) + } else { + stack = append(stack, node.child1, node.child2) + } + } + return stack, out +} + +// height of the tree (0 for one leaf), for the tests +func (t *aabbTree) height() int32 { + if t.root == nullNode { + return -1 + } + return t.nodes[t.root].height +} + +// ========== THE BROAD PHASE OF A WORLD ========== + +// proxyKind: where a body is +type proxyKind uint8 + +const ( + proxyDynamic proxyKind = iota + proxyStatic + proxyLarge // planes & heightfields: tested against every awake body +) + +type proxy struct { + node int32 + kind proxyKind + aabb actor.AABB // as stored in the tree (enlarged for a dynamic body) +} + +// Tree is the broad phase of a World: both AABB trees and the proxy of each body, in the order of World.Bodies +type Tree struct { + dynamics aabbTree + statics aabbTree + planes []int32 // the large bodies + proxies []proxy + bodies []*actor.RigidBody // the bodies the proxies were made for: a mismatch rebuilds everything + + // the pairs of proxies whose stored AABBs overlap, kept from a step to the next, and the proxies put in a tree + // since the last search + fat []pairRecord + fatIndex map[fatPair]struct{} + dead int // records of pairs which no longer overlap, compacted at the next search + moved []int32 + + // buffers of the pair search: a chunk of work per worker + chunks []treeChunk + queryJob func(i int) + scanJob func(i int) + bodyList []*actor.RigidBody + boxes []actor.AABB + pairs []Pair + sorted []Pair + counts []int32 +} + +// treeChunk: the buffers of a unit of work of the workers +type treeChunk struct { + stack []int32 + candidates []int32 + found []fatPair // the pairs found by the queries of the moved proxies + pairs []Pair // the pairs of the step + dead []fatPair // the pairs whose stored AABBs no longer overlap +} + +// fatPair: a pair of proxies, a < b +type fatPair struct { + a, b int32 +} + +// pairRecord: a pair kept from a step to the next, with the contacts the World computed for it during the step stamp +// (the pair cache and the warm start of the next step). A record whose pair no longer overlaps is a tombstone (a < 0) +type pairRecord struct { + key fatPair + first, count int32 + stamp uint32 +} + +func kindOf(body *actor.RigidBody) proxyKind { + if isLarge(body) { + return proxyLarge + } + if body.BodyType == actor.BodyTypeDynamic { + return proxyDynamic + } + return proxyStatic +} + +// isLarge: planes & heightfields are not in the trees +func isLarge(body *actor.RigidBody) bool { + switch body.Shape.(type) { + case *actor.Plane, *actor.Heightfield: + return true + } + return false +} + +func enlarged(aabb actor.AABB) actor.AABB { + margin := mgl64.Vec3{AABBMargin, AABBMargin, AABBMargin} + return actor.AABB{Min: aabb.Min.Sub(margin), Max: aabb.Max.Add(margin)} +} + +// sync the trees with the bodies and their AABBs of this step: a dynamic body out of its enlarged AABB is moved, a +// static body whose AABB changed too. The bodies must be the same slice as the last time, else everything is rebuilt +func (t *Tree) sync(bodies []*actor.RigidBody, boxes []actor.AABB) { + if len(t.bodies) == 0 || !t.matches(bodies) { + t.rebuild(bodies, boxes) + return + } + known := len(t.bodies) + for i := range bodies[:known] { + t.update(int32(i), bodies[i], boxes[i]) + } + // the bodies added since the last step + for i := known; i < len(bodies); i++ { + t.proxies = append(t.proxies, proxy{node: nullNode, kind: proxyLarge}) + t.bodies = append(t.bodies, bodies[i]) + t.place(int32(i), bodies[i], boxes[i]) + } +} + +// matches: the bodies known start the slice (bodies were added at its end at most) +func (t *Tree) matches(bodies []*actor.RigidBody) bool { + if len(t.bodies) > len(bodies) { + return false + } + for i, body := range t.bodies { + if bodies[i] != body { + return false + } + } + return true +} + +func (t *Tree) rebuild(bodies []*actor.RigidBody, boxes []actor.AABB) { + t.dynamics.clear() + t.statics.clear() + t.planes = t.planes[:0] + t.proxies = t.proxies[:0] + t.fat = t.fat[:0] + clear(t.fatIndex) + t.dead = 0 + t.moved = t.moved[:0] + t.bodies = append(t.bodies[:0], bodies...) + for i, body := range bodies { + t.proxies = append(t.proxies, proxy{node: nullNode, kind: proxyLarge}) + t.place(int32(i), body, boxes[i]) + } +} + +// place the body in its tree (or in the planes) +func (t *Tree) place(i int32, body *actor.RigidBody, aabb actor.AABB) { + p := &t.proxies[i] + p.kind = kindOf(body) + switch p.kind { + case proxyDynamic: + p.aabb = enlarged(aabb) + p.node = t.dynamics.insert(p.aabb, i) + t.moved = append(t.moved, i) + case proxyStatic: + p.aabb = aabb + p.node = t.statics.insert(aabb, i) + t.moved = append(t.moved, i) + default: + p.aabb = aabb + p.node = nullNode + t.planes = append(t.planes, i) + t.moved = append(t.moved, i) + } +} + +func (t *Tree) unplace(i int32) { + p := &t.proxies[i] + switch p.kind { + case proxyDynamic: + t.dynamics.remove(p.node) + case proxyStatic: + t.statics.remove(p.node) + default: + if k := slices.Index(t.planes, i); k >= 0 { + t.planes = slices.Delete(t.planes, k, k+1) + } + } + p.node = nullNode +} + +func (t *Tree) update(i int32, body *actor.RigidBody, aabb actor.AABB) { + p := &t.proxies[i] + kind := kindOf(body) + if kind != p.kind { + t.unplace(i) + t.place(i, body, aabb) + return + } + switch kind { + case proxyDynamic: + if !contains(p.aabb, aabb) { + t.dynamics.remove(p.node) + p.aabb = enlarged(aabb) + p.node = t.dynamics.insert(p.aabb, i) + t.moved = append(t.moved, i) + } + case proxyStatic: + if p.aabb != aabb { + t.statics.remove(p.node) + p.aabb = aabb + p.node = t.statics.insert(aabb, i) + t.moved = append(t.moved, i) + } + default: + if p.aabb != aabb { + p.aabb = aabb + t.moved = append(t.moved, i) + } + } +} + +// removed: the body at index k left World.Bodies (the following bodies moved up by one) +func (t *Tree) removed(k int) { + if k >= len(t.proxies) { + return + } + t.unplace(int32(k)) + t.proxies = slices.Delete(t.proxies, k, k+1) + t.bodies = slices.Delete(t.bodies, k, k+1) + t.forget(int32(k)) + for i := k; i < len(t.proxies); i++ { + p := &t.proxies[i] + if p.node != nullNode { + if p.kind == proxyDynamic { + t.dynamics.nodes[p.node].body = int32(i) + } else { + t.statics.nodes[p.node].body = int32(i) + } + } + } + for j, index := range t.planes { + if index > int32(k) { + t.planes[j] = index - 1 + } + } +} + +// forget the pairs of the proxy k, removed: the proxies after it move up by one +func (t *Tree) forget(k int32) { + kept := t.fat[:0] + for _, record := range t.fat { + pair := record.key + if pair.a < 0 || pair.a == k || pair.b == k { + continue + } + if pair.a > k { + pair.a-- + } + if pair.b > k { + pair.b-- + } + record.key = pair + kept = append(kept, record) + } + t.fat = kept + t.dead = 0 + clear(t.fatIndex) + for _, record := range t.fat { + t.fatIndex[record.key] = struct{}{} + } + for i, index := range t.moved { + if index > k { + t.moved[i] = index - 1 + } + } +} + +// shiftContacts: contacts were removed from the contacts of the step stamp: shift[i] of them before the contact i. +// The records move their contacts down +func (t *Tree) shiftContacts(shift []int32, stamp uint32) { + for i := range t.fat { + record := &t.fat[i] + if record.stamp == stamp && record.count > 0 { + record.first -= shift[record.first] + } + } +} + +// queryCandidates appends the indices of the bodies whose stored AABB overlaps the AABB, the planes first +func (t *Tree) queryCandidates(aabb actor.AABB, stack []int32, out []int32) ([]int32, []int32) { + out = append(out, t.planes...) + stack, out = t.statics.query(aabb, stack, out) + stack, out = t.dynamics.query(aabb, stack, out) + return stack, out +} + +// ========== PAIRS ========== +// The pairs of proxies whose stored AABBs overlap are kept from a step to the next, as the pairs of Box2D v3: only a +// proxy put in a tree since the last search (a dynamic body out of its enlarged AABB, a static body moved by the +// game, a body added) queries the trees, and a resting body costs nothing. The planes and the heightfields pair with +// every body which moved. The pairs are dropped when their stored AABBs no longer overlap. The pairs of the step are +// the ones whose exact AABBs overlap, with an awake dynamic body: the same pairs as a search from scratch, in the same +// order. Each pair keeps the contacts of its last step (pairRecord): the World finds them without any lookup + +// findPairs: the pairs of bodies whose AABBs overlap, with at least an awake dynamic body, sorted by the index of the +// first body (the planes of a body before its other pairs, in the order of the planes). The slice is reused +func (t *Tree) findPairs(bodies []*actor.RigidBody, boxes []actor.AABB, pool *workerPool) []Pair { + t.pairs = t.pairs[:0] + t.bodyList, t.boxes = bodies, boxes + if t.queryJob == nil { + t.queryJob, t.scanJob = t.query, t.scan + } + if t.fatIndex == nil { + t.fatIndex = map[fatPair]struct{}{} + } + if t.dead > 0 { + t.compact() + } + + // the proxies put in a tree since the last search find their pairs, by chunks; the pairs are then recorded once + t.chunks = t.chunks[:0] + queries := (len(t.moved) + movedPerChunk - 1) / movedPerChunk + t.chunks = slices.Grow(t.chunks, queries)[:queries] + pool.run(queries, 1, t.queryJob) + for c := range t.chunks { + for _, pair := range t.chunks[c].found { + if _, known := t.fatIndex[pair]; !known { + t.fatIndex[pair] = struct{}{} + t.fat = append(t.fat, pairRecord{key: pair}) + } + } + } + t.moved = t.moved[:0] + + // the pairs of the step, by chunks; the pairs which no longer overlap are then forgotten + scans := (len(t.fat) + fatPerChunk - 1) / fatPerChunk + t.chunks = slices.Grow(t.chunks[:0], scans)[:scans] + pool.run(scans, 1, t.scanJob) + for c := range t.chunks { + t.pairs = append(t.pairs, t.chunks[c].pairs...) + for _, pair := range t.chunks[c].dead { + delete(t.fatIndex, pair) + } + t.dead += len(t.chunks[c].dead) + } + t.bodyList, t.boxes = nil, nil + + t.pairs = t.sortPairs(t.pairs, len(bodies)) + return t.pairs +} + +// compact the records: the tombstones leave +func (t *Tree) compact() { + kept := t.fat[:0] + for _, record := range t.fat { + if record.key.a >= 0 { + kept = append(kept, record) + } + } + t.fat = kept + t.dead = 0 +} + +const ( + // movedPerChunk: queries of moved proxies per unit of work of the workers (a query costs ~1 µs, a unit of work + // about the same as one of fatPerChunk records) + movedPerChunk = 128 + // fatPerChunk: stored pairs per unit of work of the workers + fatPerChunk = 1024 +) + +// query: the chunk c of the moved proxies finds its pairs (a pair of static bodies never needs solving, nor a static +// body against a plane) +func (t *Tree) query(c int) { + chunk := &t.chunks[c] + chunk.found = chunk.found[:0] + start := c * movedPerChunk + for _, i := range t.moved[start:min(start+movedPerChunk, len(t.moved))] { + p := &t.proxies[i] + chunk.candidates = chunk.candidates[:0] + chunk.stack, chunk.candidates = t.dynamics.query(p.aabb, chunk.stack, chunk.candidates) + if p.kind == proxyDynamic { + chunk.stack, chunk.candidates = t.statics.query(p.aabb, chunk.stack, chunk.candidates) + chunk.candidates = append(chunk.candidates, t.planes...) + } + for _, j := range chunk.candidates { + if j == i { + continue + } + if j < i { + chunk.found = append(chunk.found, fatPair{j, i}) + } else { + chunk.found = append(chunk.found, fatPair{i, j}) + } + } + } +} + +// scan: the chunk c of the records emits the pairs of the step, and the pairs which no longer overlap become +// tombstones +func (t *Tree) scan(c int) { + chunk := &t.chunks[c] + chunk.pairs, chunk.dead = chunk.pairs[:0], chunk.dead[:0] + bodies, boxes := t.bodyList, t.boxes + start := c * fatPerChunk + for k := start; k < min(start+fatPerChunk, len(t.fat)); k++ { + record := &t.fat[k] + i, j := record.key.a, record.key.b + if !t.proxies[i].aabb.Overlaps(t.proxies[j].aabb) { + chunk.dead = append(chunk.dead, record.key) + record.key.a = -1 + continue + } + if !boxes[i].Overlaps(boxes[j]) { + continue + } + if plane := t.proxies[i].kind == proxyLarge; plane || t.proxies[j].kind == proxyLarge { + // a plane (or a heightfield) against a body: the pair of the body, the plane first + if !plane { + i, j = j, i + } + if !isAwakeDynamic(bodies[j]) { + continue + } + chunk.pairs = append(chunk.pairs, Pair{BodyA: bodies[i], BodyB: bodies[j], IndexA: i, IndexB: j, first: j, second: int32(slices.Index(t.planes, i)), plane: true, slot: int32(k)}) + continue + } + if needsSolving(bodies[i], bodies[j]) { + chunk.pairs = append(chunk.pairs, Pair{BodyA: bodies[i], BodyB: bodies[j], IndexA: i, IndexB: j, first: i, second: j, slot: int32(k)}) + } + } +} + +// sortPairs by the index of the first body: a counting sort (O(pairs + bodies), the pairs are many and the keys are +// small), then the few pairs of a body by their second key. Returns the sorted slice (the buffers are swapped) +func (t *Tree) sortPairs(pairs []Pair, bodiesCount int) []Pair { + if cap(t.counts) < bodiesCount+1 { + t.counts = make([]int32, bodiesCount+1) + } + counts := t.counts[:bodiesCount+1] + for i := range counts { + counts[i] = 0 + } + for i := range pairs { + counts[pairs[i].first+1]++ + } + for i := 1; i < len(counts); i++ { + counts[i] += counts[i-1] + } + if cap(t.sorted) < len(pairs) { + t.sorted = make([]Pair, len(pairs)) + } + sorted := t.sorted[:len(pairs)] + for i := range pairs { + k := pairs[i].first + sorted[counts[k]] = pairs[i] + counts[k]++ + } + // within a body: its planes first (in their order), then its other pairs by index: an insertion sort of a few pairs + start := 0 + for end := 1; end <= len(sorted); end++ { + if end < len(sorted) && sorted[end].first == sorted[start].first { + continue + } + for i := start + 1; i < end; i++ { + for j := i; j > start && pairBefore(sorted[j], sorted[j-1]); j-- { + sorted[j], sorted[j-1] = sorted[j-1], sorted[j] + } + } + start = end + } + t.sorted, t.pairs = pairs, sorted + return sorted +} + +// pairBefore: the order of the pairs of a body +func pairBefore(a, b Pair) bool { + if a.plane != b.plane { + return a.plane + } + return a.second < b.second +} + +// needsSolving - At least one body must be dynamic and awake +func needsSolving(a, b *actor.RigidBody) bool { + return isAwakeDynamic(a) || isAwakeDynamic(b) +} + +func isAwakeDynamic(body *actor.RigidBody) bool { + return body.BodyType == actor.BodyTypeDynamic && !body.IsSleeping +} diff --git a/tree_test.go b/tree_test.go new file mode 100644 index 0000000..5056f49 --- /dev/null +++ b/tree_test.go @@ -0,0 +1,191 @@ +package feather + +import ( + "math" + "math/rand" + "slices" + "testing" + + "github.com/akmonengine/feather/actor" + "github.com/go-gl/mathgl/mgl64" +) + +func randomAABB(r *rand.Rand, spread float64) actor.AABB { + c := mgl64.Vec3{r.Float64() * spread, r.Float64() * spread, r.Float64() * spread} + h := mgl64.Vec3{0.1 + r.Float64(), 0.1 + r.Float64(), 0.1 + r.Float64()} + return actor.AABB{Min: c.Sub(h), Max: c.Add(h)} +} + +// The tree finds exactly the leaves overlapping a query, as a brute force does, through insertions and removals +func TestTreeQueryIsExact(t *testing.T) { + r := rand.New(rand.NewSource(7)) + var tree aabbTree + tree.clear() + boxes := map[int32]actor.AABB{} + nodes := map[int32]int32{} + var stack, out []int32 + for round := 0; round < 3000; round++ { + if len(boxes) > 0 && r.Intn(3) == 0 { + // remove one + keys := make([]int32, 0, len(boxes)) + for k := range boxes { + keys = append(keys, k) + } + k := keys[r.Intn(len(keys))] + tree.remove(nodes[k]) + delete(boxes, k) + delete(nodes, k) + } else { + k := int32(round) + boxes[k] = randomAABB(r, 30) + nodes[k] = tree.insert(boxes[k], k) + } + if round%100 == 99 { + query := randomAABB(r, 30) + stack, out = tree.query(query, stack, out[:0]) + slices.Sort(out) + var want []int32 + for k, b := range boxes { + if b.Overlaps(query) { + want = append(want, k) + } + } + slices.Sort(want) + if !slices.Equal(out, want) { + t.Fatalf("round %d: query found %v, want %v", round, out, want) + } + } + } +} + +// The tree stays balanced: its height is logarithmic +func TestTreeIsBalanced(t *testing.T) { + r := rand.New(rand.NewSource(3)) + var tree aabbTree + tree.clear() + const count = 4096 + for i := 0; i < count; i++ { + tree.insert(randomAABB(r, 100), int32(i)) + } + if h := tree.height(); float64(h) > 2*math.Log2(count) { + t.Errorf("height %d for %d leaves, want at most %.0f", h, count, 2*math.Log2(count)) + } + // the invariants of every node: height, AABB containing its children + var check func(n int32) int32 + check = func(n int32) int32 { + node := tree.nodes[n] + if node.height == 0 { + return 0 + } + h1, h2 := check(node.child1), check(node.child2) + if node.height != 1+max(h1, h2) { + t.Errorf("node %d: height %d, children %d and %d", n, node.height, h1, h2) + } + if tree.nodes[node.child1].parent != n || tree.nodes[node.child2].parent != n { + t.Errorf("node %d: a child doesn't point back to it", n) + } + if !contains(node.aabb, tree.nodes[node.child1].aabb) || !contains(node.aabb, tree.nodes[node.child2].aabb) { + t.Errorf("node %d: its AABB doesn't contain its children", n) + } + return node.height + } + check(tree.root) +} + +func bruteForcePairs(bodies []*actor.RigidBody, boxes []actor.AABB) []Pair { + var pairs []Pair + for i, a := range bodies { + if isLarge(a) { + continue + } + for j, b := range bodies { + if isLarge(b) && needsSolving(a, b) && boxes[i].Overlaps(boxes[j]) { + pairs = append(pairs, Pair{BodyA: b, BodyB: a}) + } + } + for j := i + 1; j < len(bodies); j++ { + b := bodies[j] + if !isLarge(b) && needsSolving(a, b) && boxes[i].Overlaps(boxes[j]) { + pairs = append(pairs, Pair{BodyA: a, BodyB: b}) + } + } + } + return pairs +} + +func samePairs(t *testing.T, got, want []Pair, what string) { + t.Helper() + if len(got) != len(want) { + t.Fatalf("%s: %d pairs, want %d", what, len(got), len(want)) + } + for i := range got { + if got[i].BodyA != want[i].BodyA || got[i].BodyB != want[i].BodyB { + t.Fatalf("%s: pair %d differs", what, i) + } + } +} + +// The pairs of the tree are those of the brute force, in the same order (planes of a body first, then by index), with +// static and sleeping bodies, through the steps of a scene with 1 and 8 workers, and after the removal of bodies +func TestTreePairsAsBruteForce(t *testing.T) { + for _, workers := range []int{1, 8} { + w := newScene(workers) + addGround(w, 0.6) + r := rand.New(rand.NewSource(11)) + for i := 0; i < 300; i++ { + shape := actor.ShapeInterface(cube()) + if i%3 == 1 { + shape = &actor.Sphere{Radius: cubeHalf} + } + bodyType := actor.BodyTypeDynamic + if i%7 == 6 { + bodyType = actor.BodyTypeStatic + } + addBody(w, mgl64.Vec3{r.Float64()*8 - 4, 0.5 + r.Float64()*6, r.Float64()*8 - 4}, mgl64.QuatIdent(), shape, bodyType, 0.6, 0) + } + for step := 0; step < 120; step++ { + w.Step(1.0 / 60) + if step%20 == 19 { + // the pairs of the step, on the same AABBs + pool := w.workerPool() + pool.begin(workers) + got := w.tree.findPairs(w.Bodies, w.aabbs, pool) + pool.end() + samePairs(t, got, bruteForcePairs(w.Bodies, w.aabbs), "step") + } + if step == 60 { + // remove a body in the middle and one at the end + w.RemoveBody(w.Bodies[len(w.Bodies)/2]) + w.RemoveBody(w.Bodies[len(w.Bodies)-1]) + } + } + asleep := 0 + for _, b := range w.Bodies { + if b.IsSleeping { + asleep++ + } + } + if asleep == 0 { + t.Errorf("workers %d: no sleeping body after 2 s, the test doesn't cover the sleeping pairs", workers) + } + } +} + +// A body which changes of type (static to dynamic) moves between the trees +func TestTreeBodyChangesType(t *testing.T) { + w := newScene(1) + addGround(w, 0.6) + // the static cube floats at 0.5, the dynamic one lands on it, then the static one becomes dynamic: both fall + a := addBody(w, mgl64.Vec3{0, 0.5, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeStatic, 0.6, 0) + b := addBody(w, mgl64.Vec3{0, 3, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.6, 0) + simulate(w, 1, nil) + if b.Transform.Position.Y() < 0.5+2*cubeHalf-0.01 { + t.Fatalf("the cube fell through the static one: y = %.3f", b.Transform.Position.Y()) + } + a.BodyType = actor.BodyTypeDynamic + a.Material = actor.NewRigidBody(a.Transform, a.Shape, actor.BodyTypeDynamic, 1).Material + simulate(w, 1, nil) + if a.Transform.Position.Y() > cubeHalf+0.01 || b.Transform.Position.Y() > 3*cubeHalf+0.01 { + t.Errorf("after the change of type: a at y = %.3f, b at y = %.3f", a.Transform.Position.Y(), b.Transform.Position.Y()) + } +} diff --git a/world.go b/world.go index 86d08a7..f64d284 100644 --- a/world.go +++ b/world.go @@ -1,23 +1,75 @@ package feather import ( + "runtime" + "sync" + "time" + "github.com/akmonengine/feather/actor" "github.com/akmonengine/feather/constraint" "github.com/go-gl/mathgl/mgl64" ) -const DEFAULT_WORKERS = 1 +// DefaultWorkers: the collision detection and the solver run on one goroutine by default +const DefaultWorkers = 1 + +const () type World struct { // List of all rigid bodies in the world Bodies []*actor.RigidBody + // Joints between the bodies + Joints []Joint // Gravity acceleration (m/s², or N/kg) - Gravity mgl64.Vec3 - Substeps int - SpatialGrid *SpatialGrid - Workers int + Gravity mgl64.Vec3 + Substeps int + // Workers is the number of goroutines for the collision detection. + // The result is exactly the same whatever the value. + Workers int + // ContactHertz is the stiffness of the contacts (0 = DefaultContactHertz) + // Higher values = less overlap under load, lower values = softer contacts. + // It is capped to 1/8 of the sub-steps rate. + ContactHertz float64 Events Events + + solver solver + islands sleepIslands + // contacts of the previous step, to warm start the solver + contacts []constraint.Manifold + previous []constraint.Manifold + aabbs []actor.AABB + // step: the count of steps, the stamp of the contacts kept by the pairs of the broad phase + step uint32 + shift []int32 // buffer of RemoveBody + // the broad phase + tree Tree + // pairs of bodies linked by a joint that must not collide + jointPairs map[pairKey]int + solverJoints []Joint + // 2 buffers: one for the contacts of this step, one for the previous step + buffers [2][]constraint.Manifold + buffer int + // the manifolds of the pair i are at offsets[i], counts[i] of them + offsets []int + counts []int + // heightfields changed during this step: their contacts are computed again + changed []*actor.RigidBody + + // profile of the last step + profile Profile + + // parallelFrom: the step runs on several goroutines from this count of bodies (minParallelBodies if 0). The tests + // lower it, to run the parallel paths on small scenes + parallelFrom int + + // workers of the step, and the parameters of the narrow phase job + workers *workersHandle + pairs []Pair + manifolds []constraint.Manifold + dt float64 + collideJob func(i int) + aabbJob func(i int) } // AddBody adds a rigid body to the world @@ -25,6 +77,40 @@ func (w *World) AddBody(body *actor.RigidBody) { w.Bodies = append(w.Bodies, body) } +// AddJoint adds a joint between 2 bodies, and wakes them up +func (w *World) AddJoint(joint Joint) { + w.Joints = append(w.Joints, joint) + base := joint.base() + w.islands.wake(base.BodyA) + w.islands.wake(base.BodyB) + if !base.CollideConnected { + if w.jointPairs == nil { + w.jointPairs = make(map[pairKey]int) + } + w.jointPairs[makePairKey(base.BodyA, base.BodyB)]++ + } +} + +// RemoveJoint removes a joint, and wakes its bodies up +func (w *World) RemoveJoint(joint Joint) { + for i, other := range w.Joints { + if other != joint { + continue + } + w.Joints = append(w.Joints[:i], w.Joints[i+1:]...) + base := joint.base() + w.islands.wake(base.BodyA) + w.islands.wake(base.BodyB) + if !base.CollideConnected { + key := makePairKey(base.BodyA, base.BodyB) + if w.jointPairs[key]--; w.jointPairs[key] <= 0 { + delete(w.jointPairs, key) + } + } + return + } +} + // RemoveBody removes a rigid body from the world func (w *World) RemoveBody(body *actor.RigidBody) { k := -1 @@ -37,78 +123,458 @@ func (w *World) RemoveBody(body *actor.RigidBody) { if k != -1 { w.Bodies = append(w.Bodies[:k], w.Bodies[k+1:]...) + w.tree.removed(k) } - delete(w.Events.sleepStates, body) - for pair := range w.Events.previousActivePairs { - if pair.bodyA == body || pair.bodyB == body { - delete(w.Events.previousActivePairs, pair) + // the joints of the body are removed too + for i := len(w.Joints) - 1; i >= 0; i-- { + if base := w.Joints[i].base(); base.BodyA == body || base.BodyB == body { + w.RemoveJoint(w.Joints[i]) + } + } + + w.Events.forget(body) + // the bodies touching the removed body wake up (with their islands): they may have to fall. + // The sleeping bodies have no contact anymore: their AABB is used + w.islands.remove(body) + aabb := body.AABB() + margin := mgl64.Vec3{SpeculativeDistance, SpeculativeDistance, SpeculativeDistance} + aabb = actor.AABB{Min: aabb.Min.Sub(margin), Max: aabb.Max.Add(margin)} + for _, other := range w.Bodies { + if other.IsSleeping && aabb.Overlaps(other.AABB()) { + w.islands.wake(other) + } + } + // the contacts of the body leave: the contacts kept by the other pairs move down + n := 0 + for i := range w.contacts { + w.shift = append(w.shift, int32(i-n)) + contact := &w.contacts[i] + if contact.BodyA != body && contact.BodyB != body { + w.contacts[n] = *contact + n++ } } + w.contacts = w.contacts[:n] + w.tree.shiftContacts(w.shift, w.step) + w.shift = w.shift[:0] } -func (w *World) Step(dt float64) { - w.Workers = max(DEFAULT_WORKERS, w.Workers) - h := dt / float64(w.Substeps) +// workersHandle owns the workers of a World. When the World is not used anymore, the handle is collected +// and its finalizer stops the workers (they only reference the pool, not the World) +type workersHandle struct { + pool *workerPool +} - for range w.Substeps { - w.integrate(h) +func (w *World) workerPool() *workerPool { + if w.workers == nil { + w.workers = &workersHandle{pool: &workerPool{}} + runtime.SetFinalizer(w.workers, func(handle *workersHandle) { + handle.pool.close() + }) + } + return w.workers.pool +} - // Phase 2.0: Collision pair finding - Broad phase - // Phase 2.1: Collision pair finding - narrow phase - constraints := w.detectCollision() +// Close stops the workers of the world. The world can still be used, the workers are created again if needed. +func (w *World) Close() { + if w.workers != nil { + w.workers.pool.close() + } +} - constraints = w.Events.recordCollisions(constraints) +// UpdateHeightfield after a change of the heights or of the holes of the samples [minX, maxX] x [minZ, maxZ] +// of a heightfield body: the terrain is updated, the sleeping bodies above the region wake up, +// and the contacts with the terrain are computed again +func (w *World) UpdateHeightfield(body *actor.RigidBody, minX, minZ, maxX, maxZ int) { + field := body.Shape.(*actor.Heightfield) + field.Update(minX, minZ, maxX, maxZ) + body.UpdateAABB() + w.changed = append(w.changed, body) - // Phase 3: Solver, only one iteration is required thanks to substeps - w.solvePosition(h, constraints) + // the region in the local space of the terrain, around the changed samples + halfX, halfZ := float64(field.XSamples-1)/2, float64(field.ZSamples-1)/2 + regionMinX, regionMaxX := (float64(minX-1)-halfX)*field.Scale.X(), (float64(maxX+1)-halfX)*field.Scale.X() + regionMinZ, regionMaxZ := (float64(minZ-1)-halfZ)*field.Scale.Z(), (float64(maxZ+1)-halfZ)*field.Scale.Z() + for _, other := range w.Bodies { + if !other.IsSleeping { + continue + } + bounds := localBounds(body.Transform, other.AABB()) + if bounds.Max.X() >= regionMinX && bounds.Min.X() <= regionMaxX && bounds.Max.Z() >= regionMinZ && bounds.Min.Z() <= regionMaxZ { + w.islands.wake(other) + } + } +} - // Phase 4: Update Position & Velocity - // Calculate final velocities and commit positions - w.update(h) +// Contacts returns the contacts of the last step, with the impulses applied by the solver +func (w *World) Contacts() []constraint.Manifold { + return w.contacts +} - // Phase 5: Velocity - w.solveVelocity(h, constraints) +func (w *World) Step(dt float64) { + if dt <= 0 { + return + } + workers := max(DefaultWorkers, w.Workers) + substeps := max(1, w.Substeps) + contactHertz := w.ContactHertz + if contactHertz <= 0 { + contactHertz = DefaultContactHertz + } + + start := time.Now() + w.profile = Profile{} + w.wakeTouchedBodies() + pool := w.workerPool() + parallelFrom := w.parallelFrom + if parallelFrom <= 0 { + parallelFrom = minParallelBodies + } + if workers > 1 && len(w.Bodies) >= parallelFrom { + pool.begin(workers) + } + + // Phase 1: Collision detection, once per step - broad phase & narrow phase. + // The buffer of the previous step is kept for the warm start + w.previous = w.contacts + w.buffer = 1 - w.buffer + w.step++ + manifolds := w.detectCollision(dt, pool) + if w.wakeTouched(manifolds) { + // the woken bodies get their contacts in this step (as in Jolt) + manifolds = w.detectCollision(dt, pool) + } + mark := time.Now() + w.changed = w.changed[:0] + manifolds = w.Events.recordCollisions(manifolds) - w.trySleep(h) + // Phase 2: Solver, with substeps + s := &w.solver + s.joints = w.activeJoints() + s.prepare(w.Bodies, manifolds, dt, substeps, contactHertz, pool) + mark = w.lap(&w.profile.Prepare, mark) + for range substeps { + s.integrateVelocities(w.Gravity) + s.warmStart() + s.push() + s.integratePositions(dt) + s.relax() } + mark = w.lap(&w.profile.Substeps, mark) + s.restitution() + s.storeImpulses() + s.finalize() + pool.end() + mark = w.lap(&w.profile.Restitution, mark) + w.continuous(s, dt) + + w.contacts = manifolds + w.recordContacts() + mark = w.lap(&w.profile.Continuous, mark) + + // Phase 3: Sleep & events + w.islands.update(s, dt) w.Events.processSleepEvents(w.Bodies) w.Events.flush() + w.lap(&w.profile.Islands, mark) + w.profile.Step = time.Since(start) } -func (w *World) integrate(h float64) { - task(w.Workers, w.Bodies, func(body *actor.RigidBody) { - body.Integrate(h, w.Gravity) - }) +// lap adds the time since mark to the phase, and returns now +func (w *World) lap(phase *time.Duration, mark time.Time) time.Time { + now := time.Now() + *phase += now.Sub(mark) + return now } -func (w *World) detectCollision() []*constraint.ContactConstraint { - return NarrowPhase(BroadPhase(w.SpatialGrid, w.Bodies, w.Workers), w.Workers) +// detectCollision: the AABBs are enlarged by the distance the bodies can travel during the step, so that the contacts +// with the static bodies exist before the bodies touch (speculative contacts: the speculative CCD of PhysX, the +// "Continuous Speculative" mode of Unity) +func (w *World) detectCollision(dt float64, pool *workerPool) []constraint.Manifold { + if cap(w.aabbs) < len(w.Bodies) { + w.aabbs = make([]actor.AABB, len(w.Bodies)) + } + w.aabbs = w.aabbs[:len(w.Bodies)] + w.dt = dt + if w.aabbJob == nil { + w.aabbJob = w.computeAABB + } + mark := time.Now() + pool.run(len(w.Bodies), bodiesChunk, w.aabbJob) + + w.tree.sync(w.Bodies, w.aabbs) + w.pairs = w.tree.findPairs(w.Bodies, w.aabbs, pool) + mark = w.lap(&w.profile.BroadPhase, mark) + + // Narrow phase, in a buffer reused every 2 steps (the previous step is needed for the warm start). + // Each pair has its own place: 1 manifold, MaxManifoldsPerPair against a heightfield + if cap(w.offsets) < len(w.pairs)+1 { + w.offsets = make([]int, len(w.pairs)+1) + w.counts = make([]int, len(w.pairs)) + } + w.offsets, w.counts = w.offsets[:len(w.pairs)+1], w.counts[:len(w.pairs)] + for i, pair := range w.pairs { + w.offsets[i+1] = w.offsets[i] + manifoldsOf(pair) + } + total := w.offsets[len(w.pairs)] + if cap(w.buffers[w.buffer]) < total { + w.buffers[w.buffer] = make([]constraint.Manifold, total) + } + w.manifolds = w.buffers[w.buffer][:total] + if w.collideJob == nil { + w.collideJob = w.collide + } + pool.run(len(w.pairs), pairsPerChunk, w.collideJob) + + manifolds := compactManifolds(w.manifolds, w.offsets, w.counts) + w.lap(&w.profile.NarrowPhase, mark) + return manifolds } -func (w *World) solvePosition(h float64, constraints []*constraint.ContactConstraint) { - task(w.Workers, constraints, func(constraint *constraint.ContactConstraint) { - constraint.SolvePosition(h) - }) +// wakeTouched: a sleeping body touched by an awake dynamic body wakes up with its island (as in Box2D & Jolt). +// Returns true if a body woke up: its contacts must be found in this step +func (w *World) wakeTouched(manifolds []constraint.Manifold) bool { + woke := false + for i := range manifolds { + a, b := manifolds[i].BodyA, manifolds[i].BodyB + if a.IsSleeping && isAwakeDynamic(b) { + w.islands.wake(a) + woke = true + } else if b.IsSleeping && isAwakeDynamic(a) { + w.islands.wake(b) + woke = true + } + } + return woke +} + +// activeJoints: the joints with at least one awake dynamic body +func (w *World) activeJoints() []Joint { + w.solverJoints = w.solverJoints[:0] + for _, joint := range w.Joints { + base := joint.base() + if isAwakeDynamic(base.BodyA) || isAwakeDynamic(base.BodyB) { + w.solverJoints = append(w.solverJoints, joint) + } + } + return w.solverJoints +} + +// computeAABB of the body i, enlarged by the distance it can travel during the step +func (w *World) computeAABB(i int) { + body := w.Bodies[i] + aabb := body.AABB() + if _, isPlane := body.Shape.(*actor.Plane); !isPlane { + margin := reach(body, aabb, w.dt) + aabb = actor.AABB{Min: aabb.Min.Sub(mgl64.Vec3{margin, margin, margin}), Max: aabb.Max.Add(mgl64.Vec3{margin, margin, margin})} + } + w.aabbs[i] = aabb +} + +// collide the pair i. The triggers only need the real overlaps, the other pairs get speculative contacts +func (w *World) collide(i int) { + pair := w.pairs[i] + out := w.manifolds[w.offsets[i]:w.offsets[i+1]] + if len(w.jointPairs) > 0 && w.jointPairs[makePairKey(pair.BodyA, pair.BodyB)] > 0 { + w.counts[i] = 0 + return + } + margin := 0.0 + if !pair.BodyA.IsTrigger && !pair.BodyB.IsTrigger { + // against a static body, the contact exists before the body touches it, from its speed (the speculative CCD of + // PhysX): it doesn't sink into the ground. Between 2 dynamic bodies, only within SpeculativeDistance (as in + // Box2D v3): a fast impact is then absorbed by the spring of the contact over a few substeps, a rigid stop in one + // substep would throw the lighter body and turn both + margin = SpeculativeDistance + if pair.BodyA.BodyType == actor.BodyTypeStatic || pair.BodyB.BodyType == actor.BodyTypeStatic { + margin += relativeSpeed(pair.BodyA, pair.BodyB) * w.dt + } + + // pair cache: the contacts of the previous step, if the bodies barely moved relative to each other + previous := w.previousContacts(pair) + if len(previous) > 0 && !w.isChanged(pair) { + count := 0 + for k := range previous { + if count < len(out) && reuseManifold(&previous[k], margin, &out[count]) { + count++ + } + } + if count > 0 { + w.counts[i] = count + indexManifolds(out[:count], pair) + warmStartPair(out[:count], previous) + return + } + } + w.counts[i] = collidePair(pair, margin, out) + indexManifolds(out[:w.counts[i]], pair) + warmStartPair(out[:w.counts[i]], previous) + return + } + w.counts[i] = collidePair(pair, margin, out) + indexManifolds(out[:w.counts[i]], pair) +} + +// previousContacts of the pair: the manifolds it had in the previous step (none if it had none, or if the bodies +// changed) +func (w *World) previousContacts(pair Pair) []constraint.Manifold { + record := &w.tree.fat[pair.slot] + if record.stamp != w.step-1 || record.count == 0 || w.previous[record.first].BodyA != pair.BodyA { + return nil + } + return w.previous[record.first : record.first+record.count] +} + +// recordContacts: each pair keeps where its contacts of this step are, for the next step. The contacts with a trigger +// are not kept (they are not in the contacts) +func (w *World) recordContacts() { + first := int32(0) + for i := range w.pairs { + pair := &w.pairs[i] + count := int32(w.counts[i]) + if pair.BodyA.IsTrigger || pair.BodyB.IsTrigger { + count = 0 + } + record := &w.tree.fat[pair.slot] + record.first, record.count, record.stamp = first, count, w.step + first += count + } +} + +// indexManifolds: the manifolds of the pair carry the indices of its bodies, for the solver +func indexManifolds(manifolds []constraint.Manifold, pair Pair) { + for k := range manifolds { + manifolds[k].IndexA, manifolds[k].IndexB = pair.IndexA, pair.IndexB + } } -func (w *World) update(h float64) { - task(w.Workers, w.Bodies, func(body *actor.RigidBody) { - body.Update(h) - }) +// isChanged: a body of the pair is a heightfield changed during this step +func (w *World) isChanged(pair Pair) bool { + for _, body := range w.changed { + if body == pair.BodyA || body == pair.BodyB { + return true + } + } + return false } -func (w *World) solveVelocity(h float64, constraints []*constraint.ContactConstraint) { - task(w.Workers, constraints, func(constraint *constraint.ContactConstraint) { - constraint.SolveVelocity(h) - }) +// reach is the distance a body can travel during dt, plus the speculative distance +func reach(body *actor.RigidBody, aabb actor.AABB, dt float64) float64 { + if body.BodyType == actor.BodyTypeStatic || body.IsSleeping { + return SpeculativeDistance + } + radius := aabb.Max.Sub(aabb.Min).Len() / 2 + return SpeculativeDistance + (body.Velocity.Len()+body.AngularVelocity.Len()*radius)*dt +} + +// relativeSpeed is the maximum speed at which the surfaces of both bodies can get closer +func relativeSpeed(a, b *actor.RigidBody) float64 { + speed := b.Velocity.Sub(a.Velocity).Len() + for _, body := range [2]*actor.RigidBody{a, b} { + if _, isPlane := body.Shape.(*actor.Plane); isPlane { + continue + } + aabb := body.AABB() + speed += body.AngularVelocity.Len() * aabb.Max.Sub(aabb.Min).Len() / 2 + } + return speed } -// trySleep sets the body to sleep if its velocity is lower than the threshold, for a given duration -// this method is too simple to use a task, it slows down in multiple goroutines -func (w *World) trySleep(h float64) { - for _, body := range w.Bodies { - body.TrySleep(h, 0.1, 0.05) +// warmStartPair: a contact point takes the impulses of the closest point of the previous step (in the local space of +// body A, as the contact cache of Jolt matches its points; Box2D matches them by feature id), among the previous +// manifolds of the same pair. The contact takes the friction, twist & rolling impulses of the previous contact of its +// first matched point +func warmStartPair(manifolds, previous []constraint.Manifold) { + if len(previous) == 0 { + return + } + for i := range manifolds { + manifold := &manifolds[i] + used := [MaxManifoldsPerPair][constraint.MaxContactPoints]bool{} + source := -1 + for j := 0; j < manifold.Count; j++ { + local := manifold.Points[j].LocalAnchorA + closestManifold, closest, closestDistance := -1, -1, contactMatchDistance*contactMatchDistance + for k := range previous { + for o := 0; o < previous[k].Count; o++ { + if used[k][o] { + continue + } + distance := previous[k].Points[o].LocalAnchorA.Sub(local).LenSqr() + if distance <= closestDistance { + closestManifold, closest, closestDistance = k, o, distance + } + } + } + + if closest >= 0 { + used[closestManifold][closest] = true + point := &previous[closestManifold].Points[closest] + manifold.Points[j].NormalImpulse = point.NormalImpulse + if source < 0 { + source = closestManifold + } + } + } + // the impulses of the whole contact come from the previous contact of its first point + if source >= 0 { + manifold.FrictionImpulse, manifold.TwistImpulse = previous[source].FrictionImpulse, previous[source].TwistImpulse + manifold.RollingImpulse = previous[source].RollingImpulse + } + } +} + +// wakeTouchedBodies: a sleeping body touched by a moving body wakes up with its island, +// otherwise it would be pushed without moving +func (w *World) wakeTouchedBodies() { + w.islands.wakeWoken() + for _, joint := range w.Joints { + base := joint.base() + if base.BodyA.IsSleeping && isAwakeDynamic(base.BodyB) { + w.islands.wake(base.BodyA) + } else if base.BodyB.IsSleeping && isAwakeDynamic(base.BodyA) { + w.islands.wake(base.BodyB) + } + } + for i := range w.contacts { + bodyA, bodyB := w.contacts[i].BodyA, w.contacts[i].BodyB + if bodyA.IsSleeping && isMoving(bodyB) { + w.islands.wake(bodyA) + } else if bodyB.IsSleeping && isMoving(bodyA) { + w.islands.wake(bodyB) + } + } +} + +func isMoving(body *actor.RigidBody) bool { + return isAwakeDynamic(body) && + (body.Velocity.Len() >= actor.DefaultSleepSpeed || body.AngularVelocity.Len() >= actor.DefaultSleepSpeed) +} + +// parallelFor calls fn(i) for each i in [0, n), split between the workers. +// Each i writes only its own result, so the order of execution does not matter. +func parallelFor(n, workers int, fn func(i int)) { + workers = min(workers, n) + if workers <= 1 { + for i := 0; i < n; i++ { + fn(i) + } + return + } + + var wg sync.WaitGroup + chunkSize := (n + workers - 1) / workers + for start := 0; start < n; start += chunkSize { + end := min(start+chunkSize, n) + wg.Add(1) + go func() { + defer wg.Done() + for i := start; i < end; i++ { + fn(i) + } + }() } + wg.Wait() } diff --git a/world_bench_test.go b/world_bench_test.go new file mode 100644 index 0000000..504d570 --- /dev/null +++ b/world_bench_test.go @@ -0,0 +1,123 @@ +package feather + +import ( + "fmt" + "runtime" + "testing" + "time" + + "github.com/akmonengine/feather/actor" + "github.com/go-gl/mathgl/mgl64" +) + +// benchScene is a pile of boxes and spheres falling on the ground, then resting on each other +func benchScene(count, workers int) *World { + w := newScene(workers) + addGround(w, 0.6) + side := 1 + for side*side*4 < count { + side++ + } + for i := 0; i < count; i++ { + x := float64(i%side)*0.55 - float64(side)*0.275 + z := float64((i/side)%side)*0.55 - float64(side)*0.275 + y := 0.3 + float64(i/(side*side))*0.55 + var shape actor.ShapeInterface = cube() + if i%2 == 1 { + shape = &actor.Sphere{Radius: cubeHalf} + } + addBody(w, mgl64.Vec3{x, y, z}, mgl64.QuatIdent(), shape, actor.BodyTypeDynamic, 0.6, 0) + } + return w +} + +// BenchmarkWorldStep simulates the first second of the pile: the bodies fall and land on each other +func BenchmarkWorldStep(b *testing.B) { + for _, count := range []int{100, 500, 2000} { + for _, workers := range []int{1, 8} { + b.Run(fmt.Sprintf("%d_bodies_%d_workers", count, workers), func(b *testing.B) { + b.ReportAllocs() + for i := 0; i < b.N; i++ { + b.StopTimer() + w := benchScene(count, workers) + b.StartTimer() + simulate(w, 1, nil) + } + }) + } + } +} + +// BenchmarkHeightfield: the pile of BenchmarkWorldStep on a flat terrain of 512x512 samples instead of a plane +func BenchmarkHeightfield(b *testing.B) { + for _, ground := range []string{"plane", "terrain"} { + for _, workers := range []int{1, 8} { + b.Run(fmt.Sprintf("500_bodies_%s_%d_workers", ground, workers), func(b *testing.B) { + b.ReportAllocs() + for i := 0; i < b.N; i++ { + b.StopTimer() + w := benchScene(500, workers) + if ground == "terrain" { + field := actor.NewHeightfield(512, 512, make([]float32, 512*512), mgl64.Vec3{0.5, 1, 0.5}) + w.Bodies[0] = actor.NewRigidBody(actor.Transform{Rotation: mgl64.QuatIdent()}, field, actor.BodyTypeStatic, 0) + w.Bodies[0].Material = w.Bodies[1].Material + } + b.StartTimer() + simulate(w, 1, nil) + } + }) + } + } +} + +// After the first steps (buffers growing), a step doesn't allocate +func TestStepDoesNotAllocate(t *testing.T) { + if raceEnabled { + t.Skip("sync.Pool drops its items with the race detector") + } + for _, workers := range []int{1, 8} { + w := benchScene(500, workers) + // the pile has landed (its buffers have grown: the collision events, the polytopes of EPA), all its bodies + // still collide + simulate(w, 0.4, nil) + allocs := testing.AllocsPerRun(10, func() { + w.Step(sceneDt) + }) + if allocs > 0 { + t.Errorf("workers=%d: %.1f allocations per step, want 0", workers, allocs) + } + } +} + +// The workers sleep between the steps, and stop with Close, or when the World is not used anymore +func TestWorkersDoNotLeak(t *testing.T) { + waitGoroutines := func(want int) int { + n := runtime.NumGoroutine() + for i := 0; i < 200 && n > want; i++ { + runtime.GC() + time.Sleep(5 * time.Millisecond) + n = runtime.NumGoroutine() + } + return n + } + // the worlds of the previous tests are collected first + before := waitGoroutines(0) + + w := benchScene(300, 8) + simulate(w, 0.1, nil) + if n := runtime.NumGoroutine(); n != before+7 { + t.Errorf("%d goroutines during the simulation, want %d (7 workers)", n, before+7) + } + w.Close() + if n := waitGoroutines(before); n != before { + t.Errorf("%d goroutines after Close, want %d", n, before) + } + + func() { + abandoned := benchScene(300, 8) + simulate(abandoned, 0.1, nil) + }() + if n := waitGoroutines(before); n != before { + t.Errorf("%d goroutines after the World was abandoned, want %d", n, before) + } +} diff --git a/world_physics_test.go b/world_physics_test.go new file mode 100644 index 0000000..19dd473 --- /dev/null +++ b/world_physics_test.go @@ -0,0 +1,872 @@ +package feather + +import ( + "fmt" + "math" + "math/rand" + "testing" + + "github.com/akmonengine/feather/actor" + "github.com/akmonengine/feather/constraint" + "github.com/go-gl/mathgl/mgl64" +) + +// Physical scenarios with a known answer, at the rate AkmonEngine runs Feather: +// 50 Hz, 12 sub-steps. +const ( + sceneDt = 1.0 / 50 + sceneSubsteps = 12 + sceneGravity = 9.81 + cubeHalf = 0.25 +) + +func newScene(workers int) *World { + return &World{ + Gravity: mgl64.Vec3{0, -sceneGravity, 0}, + Substeps: sceneSubsteps, + Workers: workers, + Events: NewEvents(), + } +} + +func addBody(w *World, position mgl64.Vec3, rotation mgl64.Quat, shape actor.ShapeInterface, bodyType actor.BodyType, friction, restitution float64) *actor.RigidBody { + b := actor.NewRigidBody(actor.Transform{Position: position, Rotation: rotation}, shape, bodyType, 500) + b.Material.StaticFriction, b.Material.DynamicFriction, b.Material.Restitution = friction, friction, restitution + w.AddBody(b) + return b +} + +func addGround(w *World, friction float64) *actor.RigidBody { + return addBody(w, mgl64.Vec3{}, mgl64.QuatIdent(), &actor.Plane{Normal: mgl64.Vec3{0, 1, 0}}, actor.BodyTypeStatic, friction, 0) +} + +func cube() *actor.Box { return &actor.Box{HalfExtents: mgl64.Vec3{cubeHalf, cubeHalf, cubeHalf}} } + +func simulate(w *World, seconds float64, each func()) { + for i := 0; i < int(math.Round(seconds/sceneDt)); i++ { + w.Step(sceneDt) + if each != nil { + each() + } + } +} + +func finite(v mgl64.Vec3) bool { + for _, x := range v { + if math.IsNaN(x) || math.IsInf(x, 0) { + return false + } + } + return true +} + +// ========== REFERENCE ========== +// Feather must do at least as well as Box2D v3.1.1 (the release), at the same rate (50 Hz, 12 sub-steps), on the same +// scenes in 2D (extruded by 1 m in 3D). The values of Box2D below were measured on 27/09/2026. Where Feather doesn't +// reach them yet, the gap is logged, and followed by #821 + +// box2dStackSide: the sideways drift of the top box of a stack in Box2D v3.1.1, by count and gap (m) +var box2dStackSide = map[int][2]float64{3: {0.00004, 0.00001}, 5: {0.00032, 0.00169}, 10: {0.00323, 0.00818}} + +// referenceGap logs a value over the reference of Box2D (a known gap of #821) +func referenceGap(t *testing.T, what string, value, reference float64) { + t.Helper() + if value > reference { + t.Logf("known gap (#821): %s %.3f mm, Box2D v3.1 %.3f mm", what, value*1000, reference*1000) + } +} + +// contactSink: how deep a soft contact sits under a load. At rest, the spring of a contact point pushes back what the +// load adds: it sinks by (load / effective mass) / ω². For a cube, a corner has a quarter of the load and a quarter of +// the mass (its lever arm): the contact sinks by (load/mass) g / ω², twice as much between 2 cubes of the same mass +func contactSink(loadRatio, hertz float64) float64 { + omega := 2 * math.Pi * hertz + return loadRatio * sceneGravity / (omega * omega) +} + +// stackSink: how deep the top of a stack of n cubes sinks. The ground (2 × hertz) carries n cubes, the contact k from +// the top carries k cubes (twice the sink, between 2 dynamic bodies) +func stackSink(n int, hertz float64) float64 { + sink := contactSink(float64(n), 2*hertz) + for k := 1; k < n; k++ { + sink += 2 * contactSink(float64(k), hertz) + } + return sink +} + +// A stack of n boxes stands for 10 s. Its top sinks by the springs of its contacts: the ground (2 × ContactHertz) +// carries n boxes, the contact k from the top carries k boxes (twice the sink, between 2 dynamic bodies) +func TestStackStands(t *testing.T) { + for _, n := range []int{3, 5, 10} { + for g, gap := range []float64{0, 0.001} { + w := newScene(1) + addGround(w, 0.6) + var top *actor.RigidBody + for i := 0; i < n; i++ { + top = addBody(w, mgl64.Vec3{0, cubeHalf + float64(i)*(2*cubeHalf+gap), 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.6, 0) + } + simulate(w, 1, nil) + landed := top.Transform.Position + simulate(w, 9, nil) + p := top.Transform.Position + + sink := stackSink(n, DefaultContactHertz) + restingTop := cubeHalf + float64(n-1)*2*cubeHalf + if depth := restingTop - p.Y(); depth > sink+LinearSlop || depth < -LinearSlop { + t.Errorf("stack of %d, gap %g: top box %.1f mm under its resting height, %.1f mm expected", n, gap, depth*1000, sink*1000) + } + if drift := p.Sub(landed).Len(); !(drift < 0.0001) { + t.Errorf("stack of %d, gap %g: top box drifted %.3f mm after landing", n, gap, drift*1000) + } + // the stack stands: the top box stays on the box under it + side := math.Hypot(p.X(), p.Z()) + if side > cubeHalf { + t.Errorf("stack of %d, gap %g: top box moved %.1f mm sideways", n, gap, side*1000) + } + referenceGap(t, fmt.Sprintf("stack of %d, gap %g: sideways drift", n, gap), side, box2dStackSide[n][g]) + } + } +} + +// A pyramid of 55 boxes stands. v0.2.0 exploded (top box thrown 139 m up). +func TestPyramidStands(t *testing.T) { + w := newScene(1) + addGround(w, 0.6) + type placed struct { + body *actor.RigidBody + start mgl64.Vec3 + } + var boxes []placed + const base = 10 + for row := 0; row < base; row++ { + for i := 0; i < base-row; i++ { + p := mgl64.Vec3{(float64(i) - float64(base-1-row)/2) * 0.52, cubeHalf + float64(row)*0.501, 0} + boxes = append(boxes, placed{addBody(w, p, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.6, 0), p}) + } + } + simulate(w, 5, nil) + for _, b := range boxes { + p := b.body.Transform.Position + if !finite(p) || math.Abs(p.X()-b.start.X()) > 0.01 || math.Abs(p.Z()) > 0.01 || b.start.Y()-p.Y() > 0.03 { + t.Fatalf("box starting at %v moved to %v", b.start, p) + } + } +} + +// A box on a slope: Coulomb friction decides. It sticks when tan θ < µ and otherwise +// slides with a = g (sin θ - µ cos θ). v0.2.0 slid 9.9 m at 20°, µ=0.6. +func TestInclineFollowsCoulomb(t *testing.T) { + cases := []struct{ degrees, friction float64 }{{20, 0.6}, {20, 0.2}, {35, 0.3}, {10, 0.05}} + for _, c := range cases { + w := newScene(1) + theta := c.degrees * math.Pi / 180 + tilt := mgl64.QuatRotate(theta, mgl64.Vec3{0, 0, 1}) + normal := tilt.Rotate(mgl64.Vec3{0, 1, 0}) + addBody(w, mgl64.Vec3{}, mgl64.QuatIdent(), &actor.Plane{Normal: normal}, actor.BodyTypeStatic, c.friction, 0) + box := addBody(w, normal.Mul(cubeHalf), tilt, cube(), actor.BodyTypeDynamic, c.friction, 0) + + simulate(w, 0.5, nil) + start := box.Transform.Position + const duration = 2.0 + simulate(w, duration, nil) + slid := box.Transform.Position.Sub(start).Len() + + a := sceneGravity * (math.Sin(theta) - c.friction*math.Cos(theta)) + want := 0.0 + if a > 0 { + want = a*0.5*duration + 0.5*a*duration*duration + } + if math.Abs(slid-want) > 0.01+0.005*want { + t.Errorf("%.0f° µ=%.2f: slid %.4f m, want %.4f m", c.degrees, c.friction, slid, want) + } + } +} + +// A sphere dropped from 1 m bounces back to e² m (energy e² kept), and not at all when e=0. +func TestBounceRestitution(t *testing.T) { + for _, e := range []float64{0, 0.5, 0.8} { + w := newScene(1) + ground := addGround(w, 0) + ground.Material.Restitution = e + ball := addBody(w, mgl64.Vec3{0, 1 + cubeHalf, 0}, mgl64.QuatIdent(), &actor.Sphere{Radius: cubeHalf}, actor.BodyTypeDynamic, 0, e) + hit, apex := false, 0.0 + simulate(w, 2.5, func() { + h := ball.Transform.Position.Y() - cubeHalf + if h < 0.01 { + hit = true + } + if hit { + apex = math.Max(apex, h) + } + }) + // The contact takes a few sub-steps: allow 10% of the drop. + if math.Abs(apex-e*e) > 0.1 { + t.Errorf("e=%.1f: rebound %.3f m, want %.3f m", e, apex, e*e) + } + if e == 0 && apex > 0.001 { + t.Errorf("e=0: rebound %.4f m, want none", apex) + } + } +} + +// AddForce and AddTorque are in newtons and newton-metres, applied during the next step. +func TestForcesAreSI(t *testing.T) { + w := newScene(1) + w.Gravity = mgl64.Vec3{} + ball := addBody(w, mgl64.Vec3{}, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.5}, actor.BodyTypeDynamic, 0, 0) + m := ball.Material.GetMass() + inertia := ball.InertiaLocal.At(0, 0) + const force, torque = 10.0, 3.0 + for i := 0; i < 50; i++ { + ball.AddForce(mgl64.Vec3{force, 0, 0}) + ball.AddTorque(mgl64.Vec3{0, torque, 0}) + w.Step(sceneDt) + } + if want := force / m; math.Abs(ball.Velocity.X()-want) > 1e-9 { + t.Errorf("velocity after 1 s of %g N on %.2f kg = %.6f m/s, want %.6f", force, m, ball.Velocity.X(), want) + } + if want := torque / inertia; math.Abs(ball.AngularVelocity.Y()-want) > 1e-9 { + t.Errorf("angular velocity after 1 s of %g N.m = %.6f rad/s, want %.6f", torque, ball.AngularVelocity.Y(), want) + } + if ball.Force() != (mgl64.Vec3{}) || ball.Torque() != (mgl64.Vec3{}) { + t.Error("forces are not cleared after the step") + } +} + +// A free box spinning near its intermediate axis tumbles (Dzhanibekov effect) and keeps its +// angular momentum: the gyroscopic term is integrated, not dropped. +func TestTumblingKeepsAngularMomentum(t *testing.T) { + w := newScene(1) + w.Gravity = mgl64.Vec3{} + box := addBody(w, mgl64.Vec3{}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{0.1, 0.3, 0.6}}, actor.BodyTypeDynamic, 0, 0) + box.AngularVelocity = mgl64.Vec3{0.05, 4, 0.05} + momentum := func() mgl64.Vec3 { return box.GetInertiaWorld().Mul3x1(box.AngularVelocity) } + energy := func() float64 { return 0.5 * box.AngularVelocity.Dot(momentum()) } + l0, e0 := momentum(), energy() + flipped := false + simulate(w, 10, func() { + axis := box.Transform.Rotation.Rotate(mgl64.Vec3{0, 1, 0}) + if axis.Dot(l0.Normalize()) < 0 { + flipped = true + } + }) + if drift := momentum().Sub(l0).Len() / l0.Len(); drift > 0.01 { + t.Errorf("angular momentum drifted by %.2f%% over 10 s", drift*100) + } + if e := energy(); e > e0*1.001 || e < e0*0.9 { + t.Errorf("rotational energy went from %.4f to %.4f J", e0, e) + } + if !flipped { + t.Error("the box never flipped around its intermediate axis (no gyroscopic effect)") + } +} + +// Damping slows bodies down by 1/(1+h·c) per sub-step. +func TestDamping(t *testing.T) { + w := newScene(1) + w.Gravity = mgl64.Vec3{} + ball := addBody(w, mgl64.Vec3{}, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.5}, actor.BodyTypeDynamic, 0, 0) + ball.Material.LinearDamping, ball.Material.AngularDamping = 0.5, 2 + ball.Velocity, ball.AngularVelocity = mgl64.Vec3{1, 0, 0}, mgl64.Vec3{0, 0, 1} + simulate(w, 1, nil) + h := sceneDt / sceneSubsteps + steps := float64(50 * sceneSubsteps) + if want := math.Pow(1/(1+h*0.5), steps); math.Abs(ball.Velocity.X()-want) > 1e-9 { + t.Errorf("linear speed %.6f, want %.6f", ball.Velocity.X(), want) + } + if want := math.Pow(1/(1+h*2), steps); math.Abs(ball.AngularVelocity.Z()-want) > 1e-9 { + t.Errorf("angular speed %.6f, want %.6f", ball.AngularVelocity.Z(), want) + } +} + +// A rotated static box is a ramp: a ball rolls down its surface. With v0.2.0 a static body +// kept a zero inverse rotation (only integration filled it), so its collisions were wrong. +func TestRotatedStaticBoxIsARamp(t *testing.T) { + w := newScene(1) + tilt := mgl64.QuatRotate(30*math.Pi/180, mgl64.Vec3{0, 0, 1}) + addBody(w, mgl64.Vec3{}, tilt, &actor.Box{HalfExtents: mgl64.Vec3{3, 0.25, 1}}, actor.BodyTypeStatic, 0.5, 0) + ball := addBody(w, mgl64.Vec3{0, 1, 0}, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.25}, actor.BodyTypeDynamic, 0.5, 0) + normal := tilt.Rotate(mgl64.Vec3{0, 1, 0}) + + landed := false + simulate(w, 1.2, func() { + gap := ball.Transform.Position.Dot(normal) - 0.5 + if gap < 0.01 { + landed = true + } + if landed && math.Abs(ball.Transform.Position.X()) < 2.2 && (gap < -0.006 || gap > 0.005) { + t.Fatalf("ball %.4f m from the ramp surface at x=%.2f", gap, ball.Transform.Position.X()) + } + }) + if !landed || ball.Transform.Position.X() > -0.5 { + t.Errorf("ball at %v: it did not roll down the ramp", ball.Transform.Position) + } +} + +// The same scene gives the same result bit for bit, run after run, whatever the number of +// workers. v0.2.0 differed on 38 of 40 boxes between two runs. +func TestDeterminism(t *testing.T) { + run := func(workers int) []mgl64.Vec3 { + w := newScene(workers) + addGround(w, 0.6) + r := rand.New(rand.NewSource(1)) + for i := 0; i < 40; i++ { + q := mgl64.QuatRotate(r.Float64()*math.Pi, mgl64.Vec3{r.Float64(), r.Float64(), r.Float64()}.Normalize()) + var shape actor.ShapeInterface = cube() + switch i % 3 { + case 1: + shape = &actor.Sphere{Radius: cubeHalf} + case 2: + shape = &actor.Capsule{HalfHeight: 0.2, Radius: 0.15} + } + addBody(w, mgl64.Vec3{r.Float64()*3 - 1.5, 0.5 + float64(i)*0.6, r.Float64()*3 - 1.5}, q, shape, actor.BodyTypeDynamic, 0.6, 0.2) + } + simulate(w, 4, nil) + var out []mgl64.Vec3 + for _, b := range w.Bodies { + out = append(out, b.Transform.Position, b.Transform.Rotation.V) + } + return out + } + reference := run(1) + for _, workers := range []int{1, 3, 8} { + got := run(workers) + for i := range reference { + if got[i] != reference[i] { + t.Fatalf("workers=%d: value %d is %v, want %v", workers, i, got[i], reference[i]) + } + } + } +} + +// The same scene moved by 1 µm, 1 m or 100 km follows the same motion: the contacts don't depend on the rounding of the +// positions. A pyramid of cubes created overlapping by 25 % parts on 4 axes as deep, where a choice made by the rounding +// would send it on another path +func TestPlaceIndependence(t *testing.T) { + run := func(origin mgl64.Vec3) []mgl64.Vec3 { + w := newScene(1) + addBody(w, origin, mgl64.QuatIdent(), &actor.Plane{Normal: mgl64.Vec3{0, 1, 0}, Distance: -origin.Y()}, actor.BodyTypeStatic, 0.6, 0) + var cubes []*actor.RigidBody + for layer := 0; layer < 4; layer++ { + side := 4 - layer + for i := 0; i < side; i++ { + for k := 0; k < side; k++ { + offset := mgl64.Vec3{float64(i) - float64(side-1)/2, 0.5/0.75 + float64(layer), float64(k) - float64(side-1)/2}.Mul(0.75) + cubes = append(cubes, addBody(w, origin.Add(offset), mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{0.5, 0.5, 0.5}}, actor.BodyTypeDynamic, 0.6, 0)) + } + } + } + simulate(w, 3, nil) + out := make([]mgl64.Vec3, len(cubes)) + for i, c := range cubes { + out[i] = c.Transform.Position.Sub(origin) + } + return out + } + reference := run(mgl64.Vec3{}) + for _, distance := range []float64{1e-6, 1, 1e5} { + got := run(mgl64.Vec3{0.8, -0.7, 0.5}.Mul(distance)) + worst := 0.0 + for i := range reference { + worst = math.Max(worst, got[i].Sub(reference[i]).Len()) + } + // the rounding of the positions far from the origin, amplified over 3 s (~1 µm). A normal chosen by the rounding + // sent a cube 20 cm away, the points of a contact solved one after the other 0.1 mm + if worst > 1e-5 { + t.Errorf("moved by %g m: a cube is %.3g m away from its place in the scene at the origin", distance, worst) + } + } +} + +// A resting box falls asleep; a moving box that hits it wakes it up. +func TestSleepAndWake(t *testing.T) { + w := newScene(1) + addGround(w, 0.6) + sleeper := addBody(w, mgl64.Vec3{0, cubeHalf, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.6, 0) + simulate(w, 2, nil) + if !sleeper.IsSleeping { + t.Fatal("a resting box did not fall asleep within 2 s") + } + before := sleeper.Transform.Position + + striker := addBody(w, mgl64.Vec3{-1, cubeHalf, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.6, 0) + striker.Velocity = mgl64.Vec3{3, 0, 0} + woke := false + simulate(w, 1, func() { + if !sleeper.IsSleeping { + woke = true + } + }) + if !woke { + t.Fatal("the struck box never woke up") + } + if sleeper.Transform.Position.X()-before.X() < 0.05 { + t.Errorf("the struck box did not move: %v", sleeper.Transform.Position) + } + if striker.Transform.Position.X() > sleeper.Transform.Position.X()-2*cubeHalf+0.01 { + t.Errorf("the striker went through: striker x=%.3f, box x=%.3f", striker.Transform.Position.X(), sleeper.Transform.Position.X()) + } +} + +// A plane given as body B still pushes the body out of it (the old code reversed the +// normal in that case). +func TestPlaneAsBodyB(t *testing.T) { + ball := actor.NewRigidBody(actor.Transform{Position: mgl64.Vec3{0, 0.4, 0}, Rotation: mgl64.QuatIdent()}, &actor.Sphere{Radius: 0.5}, actor.BodyTypeDynamic, 1) + plane := createPlane(mgl64.Vec3{0, 1, 0}, 0) + m := NarrowPhase([]Pair{{BodyA: ball, BodyB: plane}}, 1) + if len(m) != 1 || m[0].BodyA != ball || m[0].Normal != (mgl64.Vec3{0, -1, 0}) { + t.Fatalf("manifold %+v, want the normal from the ball down to the plane", m) + } + if math.Abs(m[0].MinSeparation()+0.1) > 1e-12 { + t.Errorf("separation %.6f, want -0.1", m[0].MinSeparation()) + } +} + +// A fast ball does not tunnel through a thin static box: the speculative margin grows with +// the speed. +func TestNoTunnelling(t *testing.T) { + w := newScene(1) + w.Gravity = mgl64.Vec3{} + addBody(w, mgl64.Vec3{}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{2, 0.02, 2}}, actor.BodyTypeStatic, 0, 0) + ball := addBody(w, mgl64.Vec3{0, 3, 0}, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.1}, actor.BodyTypeDynamic, 0, 0) + ball.Velocity = mgl64.Vec3{0, -40, 0} // 80 cm per step, 40 times the wall thickness + simulate(w, 0.5, nil) + if y := ball.Transform.Position.Y(); y < 0.1 { + t.Errorf("the ball went through the wall: y=%.3f", y) + } +} + +// Collision events fire when bodies touch, not while they are only speculative contacts. +func TestCollisionEventsOnTouch(t *testing.T) { + w := newScene(1) + addGround(w, 0.6) + ball := addBody(w, mgl64.Vec3{0, 1, 0}, mgl64.QuatIdent(), &actor.Sphere{Radius: cubeHalf}, actor.BodyTypeDynamic, 0.6, 0) + enteredAt := -1.0 + elapsed := 0.0 + w.Events.Subscribe(EventCollisionEnter, func(Event) { + if enteredAt < 0 { + enteredAt = elapsed + } + }) + simulate(w, 1, func() { elapsed += sceneDt }) + // Free fall from 0.75 m: contact after sqrt(2*0.75/g) = 0.391 s. + if enteredAt < 0 || math.Abs(enteredAt-0.391) > 2*sceneDt { + t.Errorf("CollisionEnter at %.3f s, want ~0.391 s", enteredAt) + } + if ball.Transform.Position.Y() < cubeHalf-0.01 { + t.Errorf("ball sank to y=%.3f", ball.Transform.Position.Y()) + } +} + +// A heavy box (100x the mass) on a light one: the light box is not crushed through the +// ground and nothing jitters away. +func TestMassRatio(t *testing.T) { + const ratio = 100 + w := newScene(1) + addGround(w, 0.6) + light := addBody(w, mgl64.Vec3{0, cubeHalf, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.6, 0) + heavy := actor.NewRigidBody(actor.Transform{Position: mgl64.Vec3{0, 3*cubeHalf + 0.001, 0}, Rotation: mgl64.QuatIdent()}, cube(), actor.BodyTypeDynamic, ratio*500) + heavy.Material.StaticFriction, heavy.Material.DynamicFriction = 0.6, 0.6 + w.AddBody(heavy) + simulate(w, 5, nil) + // the ground carries both boxes (ratio + 1 light boxes), the light box carries the heavy one: between 2 cubes the + // effective mass of a corner is the light one's (1 + 1/ratio), the sink is (ratio + 1) g / ω² + lightSink := contactSink(ratio+1, 2*DefaultContactHertz) + heavySink := lightSink + contactSink(ratio+1, DefaultContactHertz) + if depth := cubeHalf - light.Transform.Position.Y(); depth > lightSink+LinearSlop { + t.Errorf("light box %.1f mm low, %.1f mm expected", depth*1000, lightSink*1000) + } + p := heavy.Transform.Position + if depth := 3*cubeHalf - p.Y(); depth > heavySink+LinearSlop { + t.Errorf("heavy box %.1f mm low, %.1f mm expected", depth*1000, heavySink*1000) + } + // the heavy box stays on the light one; Box2D v3.1 drifts by 4.26 mm + side := math.Hypot(p.X(), p.Z()) + if side > cubeHalf { + t.Errorf("heavy box moved %.1f mm sideways", side*1000) + } + referenceGap(t, "heavy box: sideways drift", side, 0.00426) +} + +// ContactHertz sets the stiffness: a stiffer world overlaps less under the same load. +func TestContactHertz(t *testing.T) { + const count, softHertz = 6, DefaultContactHertz / 3 + sink := func(hertz float64) float64 { + w := newScene(1) + w.ContactHertz = hertz + addGround(w, 0.6) + var top *actor.RigidBody + for i := 0; i < count; i++ { + top = addBody(w, mgl64.Vec3{0, cubeHalf + float64(i)*2*cubeHalf, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.6, 0) + } + simulate(w, 3, nil) + return cubeHalf + 2*(count-1)*cubeHalf - top.Transform.Position.Y() + } + soft, stiff := sink(softHertz), sink(0) + t.Logf("sink at %.0f Hz %.2f mm (%.2f expected), at the default %.2f mm (%.2f expected)", softHertz, soft*1000, + stackSink(count, softHertz)*1000, stiff*1000, stackSink(count, DefaultContactHertz)*1000) + // the sink goes with 1 / hertz²: 3 times softer, 9 times deeper + if soft > stackSink(count, softHertz)+LinearSlop || stiff > stackSink(count, DefaultContactHertz)+LinearSlop || !(stiff < soft/2) { + t.Errorf("sink at %.0f Hz %.2f mm, at the default %.2f mm", softHertz, soft*1000, stiff*1000) + } +} + +// A large pile uses the parallel solver (graph coloring): still the same result bit for bit for any workers +func TestDeterminismParallelSolver(t *testing.T) { + run := func(workers int) []mgl64.Vec3 { + w := benchScene(400, workers) + simulate(w, 1, nil) + var out []mgl64.Vec3 + for _, b := range w.Bodies { + out = append(out, b.Transform.Position, b.Transform.Rotation.V, b.AngularVelocity) + } + return out + } + reference := run(1) + for _, workers := range []int{2, 8, 16} { + got := run(workers) + for i := range reference { + if got[i] != reference[i] { + t.Fatalf("workers=%d: value %d is %v, want %v", workers, i, got[i], reference[i]) + } + } + } +} + +// rotationMatrix gives the same rotation as the quaternion +func TestRotationMatrix(t *testing.T) { + r := rand.New(rand.NewSource(3)) + for i := 0; i < 100; i++ { + q := mgl64.QuatRotate(r.Float64()*6, mgl64.Vec3{r.NormFloat64(), r.NormFloat64(), r.NormFloat64()}.Normalize()) + v := mgl64.Vec3{r.NormFloat64(), r.NormFloat64(), r.NormFloat64()} + if d := rotationMatrix(&q).Mul3x1(v).Sub(q.Rotate(v)).Len(); d > 1e-12 { + t.Fatalf("rotation %v of %v: %.2e from the quaternion", q, v, d) + } + } + if q := mgl64.QuatIdent(); math.Abs(rotationMatrix(&q).Det()-1) > 1e-15 { + t.Error("identity") + } +} + +// The pair cache moves the contact of the previous step with the bodies: it gives the same contact as +// the collision detection when the bodies barely moved +func TestPairCache(t *testing.T) { + ground := createBox(mgl64.Vec3{0, 0, 0}, mgl64.Vec3{2, 0.5, 2}, actor.BodyTypeStatic) + box := actor.NewRigidBody(actor.Transform{Position: mgl64.Vec3{0.3, 0.74, -0.2}, Rotation: mgl64.QuatRotate(0.3, mgl64.Vec3{0, 1, 0})}, cube(), actor.BodyTypeDynamic, 1) + var previous [1]constraint.Manifold + if collidePair(Pair{BodyA: ground, BodyB: box}, 0.02, previous[:]) == 0 { + t.Fatal("no contact") + } + + // moved by 0.5 mm and 0.5°: the contact is reused, and matches the collision detection + box.Transform.Position = box.Transform.Position.Add(mgl64.Vec3{0.0003, -0.0004, 0}) + box.Transform.Rotation = mgl64.QuatRotate(0.5*math.Pi/180, mgl64.Vec3{1, 0, 0}).Mul(box.Transform.Rotation) + var reused constraint.Manifold + var detected [1]constraint.Manifold + if !reuseManifold(&previous[0], 0.02, &reused) { + t.Fatal("the contact was not reused") + } + collidePair(Pair{BodyA: ground, BodyB: box}, 0.02, detected[:]) + fresh := detected[0] + if reused.Count != fresh.Count { + t.Fatalf("reused %d points, detection %d", reused.Count, fresh.Count) + } + for i := 0; i < reused.Count; i++ { + closest, closestIndex := math.Inf(1), 0 + for j := 0; j < fresh.Count; j++ { + if d := reused.Points[i].Position.Sub(fresh.Points[j].Position).Len(); d < closest { + closest, closestIndex = d, j + } + } + if closest > 0.005 { + t.Errorf("point %d is %.2f mm from the detected points", i, closest*1000) + } + if separation := fresh.Points[closestIndex].Separation; math.Abs(reused.Points[i].Separation-separation) > 1e-4 { + t.Errorf("point %d: separation %.6f, detection %.6f", i, reused.Points[i].Separation, separation) + } + } + + // moved by 2 mm: computed again + box.Transform.Position = box.Transform.Position.Add(mgl64.Vec3{0.002, 0, 0}) + if reuseManifold(&previous[0], 0.02, &reused) { + t.Error("the contact was reused after 2 mm") + } + // turned by 3°: computed again + box.Transform.Position = box.Transform.Position.Sub(mgl64.Vec3{0.002, 0, 0}) + box.Transform.Rotation = mgl64.QuatRotate(3*math.Pi/180, mgl64.Vec3{0, 1, 0}).Mul(box.Transform.Rotation) + if reuseManifold(&previous[0], 0.02, &reused) { + t.Error("the contact was reused after 3°") + } +} + +// A ball rolling on the ground: without rolling resistance it keeps rolling; with a rolling resistance c, +// the torque c·R·N brakes it at 5/7·c·g (solid sphere, rolling without slipping), it stops after v0²/(2·5/7·c·g) +func TestRollingResistance(t *testing.T) { + roll := func(resistance float64) (float64, bool) { + w := newScene(1) + addGround(w, 0.8).Material.RollingResistance = resistance + ball := addBody(w, mgl64.Vec3{0, cubeHalf, 0}, mgl64.QuatIdent(), &actor.Sphere{Radius: cubeHalf}, actor.BodyTypeDynamic, 0.8, 0) + ball.Velocity = mgl64.Vec3{2, 0, 0} + ball.AngularVelocity = mgl64.Vec3{0, 0, -2 / cubeHalf} + simulate(w, 6, nil) + return ball.Transform.Position.X(), ball.IsSleeping + } + + if distance, _ := roll(0); distance < 11.5 { + t.Errorf("without rolling resistance: rolled %.2f m in 6 s, want ~12 m", distance) + } + + const resistance = 0.1 + want := 2 * 2 / (2 * 5.0 / 7.0 * resistance * sceneGravity) + distance, sleeping := roll(resistance) + t.Logf("rolled %.3f m, analytic %.3f m", distance, want) + if math.Abs(distance-want) > 0.05*want { + t.Errorf("rolling resistance %.1f: rolled %.3f m, want %.3f m", resistance, distance, want) + } + if !sleeping { + t.Error("the ball did not stop") + } +} + +// Sleep islands: the bodies touching each other fall asleep together, and wake up together +func TestSleepIslands(t *testing.T) { + stack := func() (*World, *actor.RigidBody, []*actor.RigidBody) { + w := newScene(1) + support := addBody(w, mgl64.Vec3{0, -0.5, 0}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{2, 0.5, 2}}, actor.BodyTypeStatic, 0.6, 0) + var boxes []*actor.RigidBody + for i := 0; i < 3; i++ { + boxes = append(boxes, addBody(w, mgl64.Vec3{0, cubeHalf + float64(i)*2*cubeHalf, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.6, 0)) + } + return w, support, boxes + } + allSleeping := func(boxes []*actor.RigidBody) bool { + for _, b := range boxes { + if !b.IsSleeping { + return false + } + } + return true + } + + t.Run("the stack falls asleep at once", func(t *testing.T) { + w, _, boxes := stack() + for step := 0; step < 150 && !allSleeping(boxes); step++ { + w.Step(sceneDt) + sleeping := 0 + for _, b := range boxes { + if b.IsSleeping { + sleeping++ + } + } + if sleeping != 0 && sleeping != len(boxes) { + t.Fatalf("step %d: %d of %d boxes asleep, want all or none", step, sleeping, len(boxes)) + } + } + if !allSleeping(boxes) { + t.Fatal("the stack never fell asleep") + } + }) + + t.Run("a force on the top box wakes the whole stack", func(t *testing.T) { + w, _, boxes := stack() + simulate(w, 3, nil) + boxes[2].AddForce(mgl64.Vec3{1, 0, 0}) + w.Step(sceneDt) + for i, b := range boxes { + if b.IsSleeping { + t.Errorf("box %d still asleep", i) + } + } + }) + + t.Run("a ball hitting the bottom box wakes the whole stack", func(t *testing.T) { + w, _, boxes := stack() + simulate(w, 3, nil) + ball := addBody(w, mgl64.Vec3{-1, cubeHalf, 0}, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.2}, actor.BodyTypeDynamic, 0.6, 0) + ball.Velocity = mgl64.Vec3{4, 0, 0} + woken := false + simulate(w, 0.5, func() { + if !boxes[0].IsSleeping { + woken = true + for i, b := range boxes { + if b.IsSleeping { + t.Fatalf("box 0 woke up, box %d still asleep", i) + } + } + } + }) + if !woken { + t.Error("the stack never woke up") + } + }) + + t.Run("removing the support wakes the stack, it falls", func(t *testing.T) { + w, support, boxes := stack() + simulate(w, 3, nil) + if !allSleeping(boxes) { + t.Fatal("the stack is not asleep") + } + w.RemoveBody(support) + simulate(w, 0.5, nil) + if y := boxes[0].Transform.Position.Y(); y > cubeHalf-0.5 { + t.Errorf("the bottom box is still at y=%.3f: it did not fall", y) + } + }) +} + +// An impulse changes the velocity immediately: Δv = J / m, and Δω = I⁻¹ (r × J) at a point +func TestImpulses(t *testing.T) { + w := newScene(1) + w.Gravity = mgl64.Vec3{} + box := addBody(w, mgl64.Vec3{}, mgl64.QuatIdent(), &actor.Box{HalfExtents: mgl64.Vec3{0.5, 0.25, 0.25}}, actor.BodyTypeDynamic, 0, 0) + m := box.Material.GetMass() + + box.AddImpulse(mgl64.Vec3{0, 0, 10}) + if want := 10 / m; math.Abs(box.Velocity.Z()-want) > 1e-12 { + t.Errorf("velocity %.6f, want %.6f", box.Velocity.Z(), want) + } + + // hit at the end of the box, sideways: it moves and spins around Y + box.Velocity = mgl64.Vec3{} + box.AddImpulseAtPoint(mgl64.Vec3{0, 0, 10}, mgl64.Vec3{0.5, 0, 0}) + wantSpin := box.GetInverseInertiaWorld().Mul3x1(mgl64.Vec3{0.5, 0, 0}.Cross(mgl64.Vec3{0, 0, 10})) + if box.AngularVelocity.Sub(wantSpin).Len() > 1e-12 || math.Abs(box.Velocity.Z()-10/m) > 1e-12 { + t.Errorf("velocity %v spin %v, want %v and %v", box.Velocity, box.AngularVelocity, 10/m, wantSpin) + } + + // the simulation keeps it: linear and angular momentum are conserved in free flight + simulate(w, 1, nil) + if math.Abs(box.Velocity.Z()-10/m) > 1e-9 || math.Abs(box.GetInertiaWorld().Mul3x1(box.AngularVelocity).Y()-(-5)) > 1e-6 { + t.Errorf("after 1 s: velocity %v, angular momentum %v", box.Velocity, box.GetInertiaWorld().Mul3x1(box.AngularVelocity)) + } + + // a force at a point is a force plus a torque + point := box.Transform.Position.Add(mgl64.Vec3{0, 0, 2}) + box.AddForceAtPoint(mgl64.Vec3{0, 3, 0}, point) + if box.Force() != (mgl64.Vec3{0, 3, 0}) || box.Torque().Sub(mgl64.Vec3{0, 0, 2}.Cross(mgl64.Vec3{0, 3, 0})).Len() > 1e-12 { + t.Errorf("force %v torque %v", box.Force(), box.Torque()) + } + + // a sleeping body wakes up with its island + ground := newScene(1) + addGround(ground, 0.6) + resting := addBody(ground, mgl64.Vec3{0, cubeHalf, 0}, mgl64.QuatIdent(), cube(), actor.BodyTypeDynamic, 0.6, 0) + simulate(ground, 2, nil) + if !resting.IsSleeping { + t.Fatal("not asleep") + } + resting.AddImpulse(mgl64.Vec3{0, 200, 0}) + simulate(ground, 0.2, nil) + if resting.Transform.Position.Y() < cubeHalf+0.1 { + t.Errorf("the impulse did not throw the box up: y=%.3f", resting.Transform.Position.Y()) + } +} + +// The rotation is limited per substep (as in Box2D v3), not per step: a ball rolls as fast as the slope allows +func TestFastRollingIsNotCapped(t *testing.T) { + angle := 30 * math.Pi / 180 + normal := mgl64.Vec3{-math.Sin(angle), math.Cos(angle), 0} + w := newScene(1) + addBody(w, mgl64.Vec3{}, mgl64.QuatIdent(), &actor.Plane{Normal: normal}, actor.BodyTypeStatic, 0.6, 0) + ball := addBody(w, normal.Mul(0.12), mgl64.QuatIdent(), &actor.Sphere{Radius: 0.12}, actor.BodyTypeDynamic, 0.6, 0) + simulate(w, 2, nil) + // rolling without sliding: a = 5/7 g sin(angle) + want := 5.0 / 7 * sceneGravity * math.Sin(angle) * 2 + t.Logf("speed %.3f m/s (want %.3f), spin %.1f rad/s", ball.Velocity.Len(), want, ball.AngularVelocity.Len()) + if math.Abs(ball.Velocity.Len()-want) > 0.05 { + t.Errorf("speed %.3f m/s, want %.3f", ball.Velocity.Len(), want) + } +} + +// A body falling very fast on a terrain stops on it (continuous collision against the triangles) +func TestFastBodyOnTerrain(t *testing.T) { + w := newScene(1) + terrain := slopeTerrain(w, 0.2, 0.5) + field := terrain.Shape.(*actor.Heightfield) + box := addBody(w, mgl64.Vec3{0.3, 3, 0.4}, mgl64.QuatRotate(0.7, mgl64.Vec3{1, 1, 0}.Normalize()), cube(), actor.BodyTypeDynamic, 0.5, 0) + box.Velocity = mgl64.Vec3{0, -80, 0} + worst := 0.0 + simulate(w, 1, func() { + for c := 0; c < 8; c++ { + corner := mgl64.Vec3{cubeHalf, cubeHalf, cubeHalf} + for k := 0; k < 3; k++ { + if c&(1< 0.02 { + t.Errorf("the box went %.1f mm under the terrain", worst*1000) + } +} + +// The restitution never adds energy: a pile of bouncing balls, and a box landing on a corner, never get more energy +// than they had (kinetic + potential). With e = 1, Newton alone gains 411 J, Poisson alone 3523 J +func TestRestitutionNeverAddsEnergy(t *testing.T) { + energy := func(bodies []*actor.RigidBody) float64 { + total := 0.0 + for _, b := range bodies { + mass := b.Material.GetMass() + total += 0.5*mass*b.Velocity.LenSqr() + mass*sceneGravity*b.Transform.Position.Y() + total += 0.5 * b.AngularVelocity.Dot(b.GetInertiaWorld().Mul3x1(b.AngularVelocity)) + } + return total + } + for _, e := range []float64{0.5, 1} { + w := newScene(1) + ground := addGround(w, 0) + ground.Material.Restitution = e + var bodies []*actor.RigidBody + for i := 0; i < 3; i++ { + bodies = append(bodies, addBody(w, mgl64.Vec3{0, 1 + float64(i)*0.6, 0}, mgl64.QuatIdent(), &actor.Sphere{Radius: 0.25}, actor.BodyTypeDynamic, 0, e)) + } + bodies = append(bodies, addBody(w, mgl64.Vec3{2, 1.5, 0}, mgl64.QuatRotate(0.6, mgl64.Vec3{1, 0, 1}.Normalize()), cube(), actor.BodyTypeDynamic, 0, e)) + start := energy(bodies) + worst := 0.0 + simulate(w, 4, func() { worst = math.Max(worst, energy(bodies)-start) }) + t.Logf("e=%.1f: start %.2f J, worst gain %.4f J", e, start, worst) + // a sub-step of free fall integrated explicitly: under 0.1 % of the energy + if worst > 1e-3*start { + t.Errorf("e=%.1f: the energy grew by %.4f J", e, worst) + } + } +} + +// A World without settings works: default spatial grid, 1 substep, 1 worker +func TestZeroWorld(t *testing.T) { + var w World + w.Gravity = mgl64.Vec3{0, -sceneGravity, 0} + w.AddBody(actor.NewRigidBody(actor.Transform{Rotation: mgl64.QuatIdent()}, &actor.Plane{Normal: mgl64.Vec3{0, 1, 0}}, actor.BodyTypeStatic, 0)) + ball := actor.NewRigidBody(actor.Transform{Position: mgl64.Vec3{0, 1, 0}, Rotation: mgl64.QuatIdent()}, &actor.Sphere{Radius: 0.25}, actor.BodyTypeDynamic, 1) + w.AddBody(ball) + for i := 0; i < 120; i++ { + w.Step(sceneDt) + } + if math.Abs(ball.Transform.Position.Y()-0.25) > 0.005 { + t.Errorf("the ball rests at %.4f m, want 0.25", ball.Transform.Position.Y()) + } +} + +// A shape has no state: 2 bodies share the same box, each keeps its AABB and lands on the ground +func TestSharedShape(t *testing.T) { + w := newScene(1) + addGround(w, 0.6) + shape := cube() + left := addBody(w, mgl64.Vec3{-2, 1, 0}, mgl64.QuatIdent(), shape, actor.BodyTypeDynamic, 0.6, 0) + right := addBody(w, mgl64.Vec3{2, 3, 0}, mgl64.QuatIdent(), shape, actor.BodyTypeDynamic, 0.6, 0) + if left.AABB() == right.AABB() { + t.Fatal("both bodies have the same AABB") + } + simulate(w, 2, nil) + for _, body := range []*actor.RigidBody{left, right} { + if math.Abs(body.Transform.Position.Y()-cubeHalf) > 0.002 || body.AABB().Min.Y() > 0.001 { + t.Errorf("body at %v, AABB %v: not resting on the ground", body.Transform.Position, body.AABB()) + } + } +}