Find islands, integrate and solve constraints, solve position constraints
(step B2TimeStep)
| 430 | |
| 431 | // Find islands, integrate and solve constraints, solve position constraints |
| 432 | func (world *B2World) Solve(step B2TimeStep) { |
| 433 | world.M_profile.SolveInit = 0.0 |
| 434 | world.M_profile.SolveVelocity = 0.0 |
| 435 | world.M_profile.SolvePosition = 0.0 |
| 436 | |
| 437 | // Size the island for the worst case. |
| 438 | island := MakeB2Island( |
| 439 | world.M_bodyCount, |
| 440 | world.M_contactManager.M_contactCount, |
| 441 | world.M_jointCount, |
| 442 | world.M_contactManager.M_contactListener, |
| 443 | ) |
| 444 | |
| 445 | // Clear all the island flags. |
| 446 | for b := world.M_bodyList; b != nil; b = b.M_next { |
| 447 | b.M_flags &= ^B2Body_Flags.E_islandFlag |
| 448 | } |
| 449 | for c := world.M_contactManager.M_contactList; c != nil; c = c.GetNext() { |
| 450 | c.SetFlags(c.GetFlags() & ^B2Body_Flags.E_islandFlag) |
| 451 | } |
| 452 | |
| 453 | for j := world.M_jointList; j != nil; j = j.GetNext() { |
| 454 | j.SetIslandFlag(false) |
| 455 | } |
| 456 | |
| 457 | // Build and simulate all awake islands. |
| 458 | stackSize := world.M_bodyCount |
| 459 | stack := make([]*B2Body, stackSize) |
| 460 | |
| 461 | for seed := world.M_bodyList; seed != nil; seed = seed.M_next { |
| 462 | if (seed.M_flags & B2Body_Flags.E_islandFlag) != 0x0000 { |
| 463 | continue |
| 464 | } |
| 465 | |
| 466 | if seed.IsAwake() == false || seed.IsActive() == false { |
| 467 | continue |
| 468 | } |
| 469 | |
| 470 | // The seed can be dynamic or kinematic. |
| 471 | if seed.GetType() == B2BodyType.B2_staticBody { |
| 472 | continue |
| 473 | } |
| 474 | |
| 475 | // Reset island and stack. |
| 476 | island.Clear() |
| 477 | stackCount := 0 |
| 478 | stack[stackCount] = seed |
| 479 | stackCount++ |
| 480 | seed.M_flags |= B2Body_Flags.E_islandFlag |
| 481 | |
| 482 | // Perform a depth first search (DFS) on the constraint graph. |
| 483 | for stackCount > 0 { |
| 484 | // Grab the next body off the stack and add it to the island. |
| 485 | stackCount-- |
| 486 | b := stack[stackCount] |
| 487 | B2Assert(b.IsActive() == true) |
| 488 | island.AddBody(b) |
| 489 |
no test coverage detected