diff --git a/collision.go b/collision.go index 735f761..4368074 100644 --- a/collision.go +++ b/collision.go @@ -32,22 +32,22 @@ type CollisionPair struct { // 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) <-chan Pair { +func BroadPhase(spatialGrid *SpatialGrid, bodies []*actor.RigidBody, workersCount int) <-chan Pair { spatialGrid.Clear() for i, body := range bodies { spatialGrid.Insert(i, body) } spatialGrid.SortCells() - checkingPairs := spatialGrid.FindPairsParallel(bodies, WORKERS) + checkingPairs := spatialGrid.FindPairsParallel(bodies, workersCount) return checkingPairs } -func NarrowPhase(pairs <-chan Pair) []*constraint.ContactConstraint { +func NarrowPhase(pairs <-chan Pair, workersCount int) []*constraint.ContactConstraint { // Dispatcher: separate pairs with planes, and normal convex objects - planePairs := make(chan Pair, WORKERS) - gjkPairs := make(chan Pair, WORKERS) + planePairs := make(chan Pair, workersCount) + gjkPairs := make(chan Pair, workersCount) go func() { defer close(planePairs) @@ -66,14 +66,14 @@ func NarrowPhase(pairs <-chan Pair) []*constraint.ContactConstraint { }() // Canal pour collecter tous les contacts - allContacts := make(chan *constraint.ContactConstraint, WORKERS*2) + 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) - contactsChan := EPA(collisionPairs) + collisionPairs := GJK(gjkPairs, workersCount) + contactsChan := EPA(collisionPairs, workersCount) for contact := range contactsChan { allContacts <- contact } @@ -83,7 +83,7 @@ func NarrowPhase(pairs <-chan Pair) []*constraint.ContactConstraint { wg.Add(1) go func() { defer wg.Done() - contactsChan := collidePlane(planePairs) + contactsChan := collidePlane(planePairs, workersCount) for contact := range contactsChan { allContacts <- contact } @@ -104,14 +104,14 @@ func NarrowPhase(pairs <-chan Pair) []*constraint.ContactConstraint { return contacts } -func GJK(pairChan <-chan Pair) <-chan CollisionPair { - collisionChan := make(chan CollisionPair, WORKERS) +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 WORKERS { + for range workersCount { wg.Add(1) go func() { defer wg.Done() @@ -139,14 +139,14 @@ func GJK(pairChan <-chan Pair) <-chan CollisionPair { return collisionChan } -func EPA(p <-chan CollisionPair) <-chan *constraint.ContactConstraint { - ch := make(chan *constraint.ContactConstraint, WORKERS) +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 WORKERS { + for range workersCount { wg.Add(1) go func() { defer wg.Done() @@ -167,14 +167,14 @@ func EPA(p <-chan CollisionPair) <-chan *constraint.ContactConstraint { return ch } -func collidePlane(pairs <-chan Pair) <-chan *constraint.ContactConstraint { - ch := make(chan *constraint.ContactConstraint, WORKERS) +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 WORKERS { + for range workersCount { wg.Add(1) go func() { defer wg.Done() diff --git a/collision_test.go b/collision_test.go index 3b3e6b8..dc5e851 100644 --- a/collision_test.go +++ b/collision_test.go @@ -44,8 +44,9 @@ func TestBroadPhaseNoBodies(t *testing.T) { world := World{ Bodies: []*actor.RigidBody{}, SpatialGrid: NewSpatialGrid(1.0, 1024), + Workers: 8, } - pairs := BroadPhase(world.SpatialGrid, world.Bodies) + pairs := BroadPhase(world.SpatialGrid, world.Bodies, world.Workers) if len(pairs) != 0 { t.Errorf("BroadPhase with no bodies returned %d pairs, want 0", len(pairs)) @@ -56,9 +57,10 @@ func TestBroadPhaseSingleBody(t *testing.T) { world := World{ Bodies: []*actor.RigidBody{}, SpatialGrid: NewSpatialGrid(1.0, 1024), + Workers: 8, } world.AddBody(createBox(mgl64.Vec3{0, 0, 0}, mgl64.Vec3{1, 1, 1}, actor.BodyTypeDynamic)) - pairs := BroadPhase(world.SpatialGrid, world.Bodies) + pairs := BroadPhase(world.SpatialGrid, world.Bodies, world.Workers) if len(pairs) != 0 { t.Errorf("BroadPhase with single body returned %d pairs, want 0", len(pairs)) @@ -69,10 +71,11 @@ func TestBroadPhaseTwoBodiesOverlapping(t *testing.T) { world := World{ Bodies: []*actor.RigidBody{}, SpatialGrid: NewSpatialGrid(1.0, 1024), + Workers: 8, } world.AddBody(createBox(mgl64.Vec3{0, 0, 0}, mgl64.Vec3{1, 1, 1}, actor.BodyTypeDynamic)) world.AddBody(createBox(mgl64.Vec3{1.5, 0, 0}, mgl64.Vec3{1, 1, 1}, actor.BodyTypeDynamic)) - pairs := BroadPhase(world.SpatialGrid, world.Bodies) + pairs := BroadPhase(world.SpatialGrid, world.Bodies, world.Workers) var contactPairs []Pair for p := range pairs { @@ -91,10 +94,11 @@ func TestBroadPhaseTwoBodiesNotOverlapping(t *testing.T) { world := World{ Bodies: []*actor.RigidBody{}, SpatialGrid: NewSpatialGrid(1.0, 1024), + Workers: 8, } world.AddBody(createBox(mgl64.Vec3{0, 0, 0}, mgl64.Vec3{1, 1, 1}, actor.BodyTypeDynamic)) world.AddBody(createBox(mgl64.Vec3{10.0, 0, 0}, mgl64.Vec3{1, 1, 1}, actor.BodyTypeDynamic)) - pairs := BroadPhase(world.SpatialGrid, world.Bodies) + pairs := BroadPhase(world.SpatialGrid, world.Bodies, world.Workers) var contactPairs []Pair for p := range pairs { @@ -110,10 +114,11 @@ func TestBroadPhaseTwoStaticBodies(t *testing.T) { world := World{ Bodies: []*actor.RigidBody{}, SpatialGrid: NewSpatialGrid(1.0, 1024), + Workers: 8, } world.AddBody(createBox(mgl64.Vec3{0, 0, 0}, mgl64.Vec3{1, 1, 1}, actor.BodyTypeStatic)) world.AddBody(createBox(mgl64.Vec3{1.5, 0, 0}, mgl64.Vec3{1, 1, 1}, actor.BodyTypeStatic)) - pairs := BroadPhase(world.SpatialGrid, world.Bodies) + pairs := BroadPhase(world.SpatialGrid, world.Bodies, world.Workers) var contactPairs []Pair for p := range pairs { @@ -130,10 +135,11 @@ func TestBroadPhaseStaticDynamicOverlapping(t *testing.T) { world := World{ Bodies: []*actor.RigidBody{}, SpatialGrid: NewSpatialGrid(1.0, 1024), + Workers: 8, } world.AddBody(createBox(mgl64.Vec3{0, 0, 0}, mgl64.Vec3{1, 1, 1}, actor.BodyTypeStatic)) world.AddBody(createBox(mgl64.Vec3{1.5, 0, 0}, mgl64.Vec3{1, 1, 1}, actor.BodyTypeDynamic)) - pairs := BroadPhase(world.SpatialGrid, world.Bodies) + pairs := BroadPhase(world.SpatialGrid, world.Bodies, world.Workers) var contactPairs []Pair for p := range pairs { @@ -148,6 +154,7 @@ func TestBroadPhaseMultipleBodies(t *testing.T) { world := World{ Bodies: []*actor.RigidBody{}, SpatialGrid: NewSpatialGrid(1.0, 1024), + Workers: 8, } // Create bodies @@ -161,7 +168,7 @@ func TestBroadPhaseMultipleBodies(t *testing.T) { world.AddBody(body2) world.AddBody(body3) - pairs := BroadPhase(world.SpatialGrid, world.Bodies) + pairs := BroadPhase(world.SpatialGrid, world.Bodies, world.Workers) // Expected pairs: (0,1), (1,2) expectedPairs := 2 @@ -208,12 +215,13 @@ func TestBroadPhaseSpheresOverlapping(t *testing.T) { world := World{ Bodies: []*actor.RigidBody{}, SpatialGrid: NewSpatialGrid(1.0, 1024), + Workers: 8, } world.AddBody(createSphere(mgl64.Vec3{0, 0, 0}, 1.0, actor.BodyTypeDynamic)) world.AddBody(createSphere(mgl64.Vec3{1.5, 0, 0}, 1.0, actor.BodyTypeDynamic)) - pairs := BroadPhase(world.SpatialGrid, world.Bodies) + pairs := BroadPhase(world.SpatialGrid, world.Bodies, world.Workers) var contactPairs []Pair for p := range pairs { @@ -232,12 +240,13 @@ func TestBroadPhaseSpheresNotOverlapping(t *testing.T) { world := World{ Bodies: []*actor.RigidBody{}, SpatialGrid: NewSpatialGrid(1.0, 1024), + Workers: 8, } world.AddBody(createSphere(mgl64.Vec3{0, 0, 0}, 1.0, actor.BodyTypeDynamic)) world.AddBody(createSphere(mgl64.Vec3{3, 0, 0}, 1.0, actor.BodyTypeDynamic)) - pairs := BroadPhase(world.SpatialGrid, world.Bodies) + pairs := BroadPhase(world.SpatialGrid, world.Bodies, world.Workers) var contactPairs []Pair for p := range pairs { @@ -256,12 +265,13 @@ func TestBroadPhaseMixedShapes(t *testing.T) { world := World{ Bodies: []*actor.RigidBody{}, SpatialGrid: NewSpatialGrid(1.0, 1024), + Workers: 8, } world.AddBody(createBox(mgl64.Vec3{0, 0, 0}, mgl64.Vec3{1, 1, 1}, actor.BodyTypeDynamic)) world.AddBody(createSphere(mgl64.Vec3{1.5, 0, 0}, 1.0, actor.BodyTypeDynamic)) - pairs := BroadPhase(world.SpatialGrid, world.Bodies) + pairs := BroadPhase(world.SpatialGrid, world.Bodies, world.Workers) var contactPairs []Pair for p := range pairs { @@ -280,12 +290,13 @@ func TestBroadPhaseWithPlane(t *testing.T) { world := World{ Bodies: []*actor.RigidBody{}, SpatialGrid: NewSpatialGrid(1.0, 1024), + Workers: 8, } world.AddBody(createPlane(mgl64.Vec3{0, 1, 0}, 0)) // Ground plane at y=0 world.AddBody(createBox(mgl64.Vec3{0, 0.5, 0}, mgl64.Vec3{1, 1, 1}, actor.BodyTypeDynamic)) - pairs := BroadPhase(world.SpatialGrid, world.Bodies) + pairs := BroadPhase(world.SpatialGrid, world.Bodies, world.Workers) var contactPairs []Pair for p := range pairs { @@ -303,7 +314,7 @@ func TestNarrowPhaseNoPairs(t *testing.T) { pairs := make(chan Pair) close(pairs) // Close immediately to signal no more pairs - contacts := NarrowPhase(pairs) + contacts := NarrowPhase(pairs, 8) if len(contacts) != 0 { t.Errorf("NarrowPhase with no pairs returned %d contacts, want 0", len(contacts)) @@ -320,7 +331,7 @@ func TestNarrowPhaseOverlappingBoxes(t *testing.T) { pairs <- Pair{BodyA: bodyA, BodyB: bodyB} close(pairs) - contacts := NarrowPhase(pairs) + contacts := NarrowPhase(pairs, 8) // Should detect collision if len(contacts) == 0 { @@ -338,7 +349,7 @@ func TestNarrowPhaseNonOverlappingBoxes(t *testing.T) { pairs <- Pair{BodyA: bodyA, BodyB: bodyB} close(pairs) - contacts := NarrowPhase(pairs) + contacts := NarrowPhase(pairs, 8) // Should not detect collision if len(contacts) != 0 { @@ -356,7 +367,7 @@ func TestNarrowPhaseOverlappingSpheres(t *testing.T) { pairs <- Pair{BodyA: bodyA, BodyB: bodyB} close(pairs) - contacts := NarrowPhase(pairs) + contacts := NarrowPhase(pairs, 8) // Should detect collision if len(contacts) == 0 { @@ -374,7 +385,7 @@ func TestNarrowPhaseNonOverlappingSpheres(t *testing.T) { pairs <- Pair{BodyA: bodyA, BodyB: bodyB} close(pairs) - contacts := NarrowPhase(pairs) + contacts := NarrowPhase(pairs, 8) // Should not detect collision if len(contacts) != 0 { @@ -392,7 +403,7 @@ func TestNarrowPhaseBoxSphere(t *testing.T) { pairs <- Pair{BodyA: bodyA, BodyB: bodyB} close(pairs) - contacts := NarrowPhase(pairs) + contacts := NarrowPhase(pairs, 8) // Should detect collision if len(contacts) == 0 { @@ -410,7 +421,7 @@ func TestNarrowPhaseSphereOnPlane(t *testing.T) { pairs <- Pair{BodyA: bodyA, BodyB: bodyB} close(pairs) - contacts := NarrowPhase(pairs) + contacts := NarrowPhase(pairs, 8) // Should detect collision (sphere penetrating plane) if len(contacts) == 0 { @@ -428,7 +439,7 @@ func TestNarrowPhaseBoxOnPlane(t *testing.T) { pairs <- Pair{BodyA: bodyA, BodyB: bodyB} close(pairs) - contacts := NarrowPhase(pairs) + contacts := NarrowPhase(pairs, 8) // Should detect collision (box penetrating plane) if len(contacts) == 0 { @@ -449,7 +460,7 @@ func TestNarrowPhaseMultiplePairs(t *testing.T) { pairs <- Pair{BodyA: bodyC, BodyB: bodyD} // Should collide close(pairs) - contacts := NarrowPhase(pairs) + contacts := NarrowPhase(pairs, 8) // Should detect both collisions if len(contacts) < 2 { @@ -484,6 +495,7 @@ func TestIntegrationBroadAndNarrowPhase(t *testing.T) { world := World{ Bodies: []*actor.RigidBody{}, SpatialGrid: NewSpatialGrid(1.0, 1024), + Workers: 8, } body0 := createBox(mgl64.Vec3{0, 0, 0}, mgl64.Vec3{1, 1, 1}, actor.BodyTypeDynamic) @@ -495,7 +507,7 @@ func TestIntegrationBroadAndNarrowPhase(t *testing.T) { world.AddBody(body2) // Broad phase - pairs := BroadPhase(world.SpatialGrid, world.Bodies) + pairs := BroadPhase(world.SpatialGrid, world.Bodies, world.Workers) var contactPairs []Pair for p := range pairs { @@ -513,7 +525,7 @@ func TestIntegrationBroadAndNarrowPhase(t *testing.T) { } close(pairChan) - contacts := NarrowPhase(pairChan) + contacts := NarrowPhase(pairChan, 8) if len(contacts) == 0 { t.Error("NarrowPhase returned no contacts, expected at least 1") @@ -554,7 +566,7 @@ func BenchmarkLargeBroadPhase2(b *testing.B) { b.ReportAllocs() b.ResetTimer() for i := 0; i < b.N; i++ { - pair := BroadPhase(world.SpatialGrid, world.Bodies) + pair := BroadPhase(world.SpatialGrid, world.Bodies, world.Workers) for p := range pair { p.BodyA.IsSleeping = true @@ -598,10 +610,10 @@ func BenchmarkLargeGJK2(b *testing.B) { b.ResetTimer() for i := 0; i < b.N; i++ { b.StopTimer() - pair := BroadPhase(world.SpatialGrid, world.Bodies) + pair := BroadPhase(world.SpatialGrid, world.Bodies, world.Workers) b.StartTimer() - collisionPair := GJK(pair) + collisionPair := GJK(pair, 8) cp := <-collisionPair cp.BodyA.IsSleeping = false } @@ -626,6 +638,7 @@ func BenchmarkLargeEPA2(b *testing.B) { world := World{ SpatialGrid: NewSpatialGrid(6.0, 4096), + Workers: 8, } for i := 0; i < cubesCount; i++ { row := i / rowSize @@ -653,11 +666,11 @@ func BenchmarkLargeEPA2(b *testing.B) { b.ResetTimer() for i := 0; i < b.N; i++ { b.StopTimer() - pair := BroadPhase(world.SpatialGrid, world.Bodies) - collisionPair := GJK(pair) + pair := BroadPhase(world.SpatialGrid, world.Bodies, world.Workers) + collisionPair := GJK(pair, world.Workers) b.StartTimer() - c := EPA(collisionPair) + c := EPA(collisionPair, world.Workers) for cp := range c { cp.Normal.Add(mgl64.Vec3{1, 1, 1}) @@ -738,6 +751,7 @@ func BenchmarkLargeWorldStep(b *testing.B) { Gravity: mgl64.Vec3{}, Substeps: 20, SpatialGrid: NewSpatialGrid(6.0, 4096), + Workers: 8, } bodies := make([]*actor.RigidBody, cubesCount) diff --git a/world.go b/world.go index d9fceca..0113cd8 100644 --- a/world.go +++ b/world.go @@ -6,7 +6,7 @@ import ( "github.com/go-gl/mathgl/mgl64" ) -const WORKERS = 8 +const DEFAULT_WORKERS = 1 type World struct { // List of all rigid bodies in the world @@ -15,6 +15,7 @@ type World struct { Gravity mgl64.Vec3 Substeps int SpatialGrid *SpatialGrid + Workers int } // AddBody adds a rigid body to the world @@ -38,6 +39,7 @@ func (w *World) RemoveBody(body *actor.RigidBody) { } func (w *World) Step(dt float64) { + w.Workers = max(DEFAULT_WORKERS, w.Workers) h := dt / float64(w.Substeps) for range w.Substeps { @@ -45,7 +47,7 @@ func (w *World) Step(dt float64) { // Phase 2.0: Collision pair finding - Broad phase // Phase 2.1: Collision pair finding - narrow phase - constraints := NarrowPhase(BroadPhase(w.SpatialGrid, w.Bodies)) + constraints := w.detectCollision() // Phase 3: Solver, only one iteration is required thanks to substeps w.solvePosition(h, constraints) @@ -62,25 +64,29 @@ func (w *World) Step(dt float64) { } func (w *World) integrate(h float64) { - task(WORKERS, w.Bodies, func(body *actor.RigidBody) { + task(w.Workers, w.Bodies, func(body *actor.RigidBody) { body.Integrate(h, w.Gravity) }) } +func (w *World) detectCollision() []*constraint.ContactConstraint { + return NarrowPhase(BroadPhase(w.SpatialGrid, w.Bodies, w.Workers), w.Workers) +} + func (w *World) solvePosition(h float64, constraints []*constraint.ContactConstraint) { - task(WORKERS, constraints, func(constraint *constraint.ContactConstraint) { + task(w.Workers, constraints, func(constraint *constraint.ContactConstraint) { constraint.SolvePosition(h) }) } func (w *World) update(h float64) { - task(WORKERS, w.Bodies, func(body *actor.RigidBody) { + task(w.Workers, w.Bodies, func(body *actor.RigidBody) { body.Update(h) }) } func (w *World) solveVelocity(h float64, constraints []*constraint.ContactConstraint) { - task(WORKERS, constraints, func(constraint *constraint.ContactConstraint) { + task(w.Workers, constraints, func(constraint *constraint.ContactConstraint) { constraint.SolveVelocity(h) }) }