Line data    Source code

       1             : #include "CollisionHandler.hpp"
       2             : #include "Disc.hpp"
       3             : #include "MathUtils.hpp"
       4             : 
       5             : #include <ranges>
       6             : 
       7             : namespace cell
       8             : {
       9             : 
      10          11 : CollisionHandler::CollisionHandler(const DiscTypeRegistry& discTypeRegistry,
      11          11 :                                    const MembraneTypeRegistry& membraneTypeRegistry)
      12          11 :     : discTypeRegistry_(discTypeRegistry)
      13          11 :     , membraneTypeRegistry_(membraneTypeRegistry)
      14             : {
      15          11 : }
      16             : 
      17          26 : void CollisionHandler::resolveCollisions(const std::vector<CollisionDetector::Collision>& collisions) const
      18             : {
      19          26 :     if (collisions.empty())
      20          16 :         return;
      21             : 
      22             :     // Sweep-and-prune always returns collisions in left-to-right order
      23             :     // Handling collisions always in the same order will give the discs a drift to the left
      24             :     // We randomly change the order to avoid that
      25             : 
      26          10 :     if (mathutils::getRandomInt() % 2 == 0)
      27             :     {
      28          11 :         for (const auto& collision : collisions)
      29           6 :             handleCollision(collision);
      30             :     }
      31             :     else
      32             :     {
      33          14 :         for (const auto& collision : std::ranges::reverse_view(collisions))
      34           9 :             handleCollision(collision);
      35             :     }
      36             : }
      37             : 
      38             : CollisionHandler::CollisionContext
      39          15 : CollisionHandler::calculateCollisionContext(const CollisionDetector::Collision& collision) const
      40             : {
      41          15 :     CollisionContext context;
      42             :     using CollisionType = CollisionDetector::CollisionType;
      43             : 
      44          15 :     const bool isMembraneCollision =
      45          15 :         collision.type == CollisionType::DiscContainingMembrane || collision.type == CollisionType::DiscChildMembrane;
      46             : 
      47          15 :     if (collision.invalidatedByDestroyedDiscs() || collision.allowedToPass)
      48             :     {
      49           5 :         context.skipCollision = true;
      50           5 :         return context;
      51             :     }
      52             : 
      53          10 :     context.disc = collision.disc;
      54          10 :     context.invMass1 = 1.0 / discTypeRegistry_.getByID(context.disc->getTypeID()).getMass();
      55          10 :     const double R1 = discTypeRegistry_.getByID(context.disc->getTypeID()).getRadius();
      56             : 
      57          10 :     double R2 = NAN;
      58          10 :     if (isMembraneCollision)
      59             :     {
      60           3 :         context.obj2 = collision.membrane;
      61           3 :         R2 = membraneTypeRegistry_.getByID(collision.membrane->getTypeID()).getRadius();
      62           3 :         context.invMass2 = 0;
      63             :     }
      64             :     else
      65             :     {
      66           7 :         context.obj2 = collision.otherDisc;
      67           7 :         context.invMass2 = 1.0 / discTypeRegistry_.getByID(collision.otherDisc->getTypeID()).getMass();
      68           7 :         R2 = discTypeRegistry_.getByID(collision.otherDisc->getTypeID()).getRadius();
      69             :     }
      70             : 
      71          10 :     context.effMass = 1.0 / (context.invMass1 + context.invMass2);
      72             : 
      73          10 :     const Vector2d diff = context.obj2->getPosition() - context.disc->getPosition();
      74          10 :     const double distance = mathutils::abs(diff);
      75             : 
      76          10 :     if (collision.type == CollisionType::DiscContainingMembrane)
      77           0 :         context.penetration = std::abs(R2 - R1 - distance);
      78             :     else
      79          10 :         context.penetration = std::abs(R1 + R2 - distance);
      80             : 
      81             :     // The normal points in the direction of the impulse change
      82          10 :     if (std::abs(distance) < 1e-3)
      83           1 :         context.normal = Vector2d{1, 0};
      84           9 :     else if (collision.type == CollisionType::DiscChildMembrane)
      85           3 :         context.normal = -diff / distance;
      86             :     else
      87           6 :         context.normal = diff / distance;
      88             : 
      89          10 :     double relativeNormalSpeed = NAN;
      90          10 :     const double e = 1.0;
      91             : 
      92          10 :     if (isMembraneCollision)
      93           3 :         relativeNormalSpeed = context.normal * collision.disc->getVelocity();
      94             :     else
      95           7 :         relativeNormalSpeed = context.normal * (context.obj2->getVelocity() - collision.disc->getVelocity());
      96             : 
      97          10 :     context.impulseChange = -context.effMass * (1 + e) * relativeNormalSpeed;
      98             : 
      99          10 :     return context;
     100             : }
     101             : 
     102          15 : void CollisionHandler::handleCollision(const CollisionDetector::Collision& collision) const
     103             : {
     104             :     using CollisionType = CollisionDetector::CollisionType;
     105             : 
     106          15 :     const auto context = calculateCollisionContext(collision);
     107             : 
     108          15 :     if (context.skipCollision)
     109           5 :         return;
     110             : 
     111          10 :     if (context.impulseChange <= 0)
     112             :     {
     113             :         // Separation will always happen within 2 time steps
     114           2 :         double beta = context.penetration / 2;
     115             : 
     116           2 :         switch (collision.type)
     117             :         {
     118           1 :         case CollisionType::DiscContainingMembrane:
     119           1 :         case CollisionType::DiscChildMembrane: context.disc->move(beta * context.normal); break;
     120           1 :         default:
     121           1 :             context.disc->move(-beta * context.invMass1 * context.effMass * context.normal);
     122           1 :             context.obj2->move(beta * context.invMass2 * context.effMass * context.normal);
     123             :         }
     124             :     }
     125             :     else
     126             :     {
     127           8 :         switch (collision.type)
     128             :         {
     129           2 :         case CollisionType::DiscContainingMembrane:
     130             :         case CollisionType::DiscChildMembrane:
     131           2 :             context.disc->accelerate(context.impulseChange * context.normal * context.invMass1);
     132           2 :             break;
     133           6 :         default:
     134           6 :             context.disc->accelerate(-context.impulseChange * context.normal * context.invMass1);
     135           6 :             context.obj2->accelerate(context.impulseChange * context.normal * context.invMass2);
     136             :         }
     137             :     }
     138             : }
     139             : 
     140             : } // namespace cell