fix for collision detection, if the polygons do not collide at T=0, but shortly afterwards

We had some trouble with the collision detection: under some particular edge cases,

• CollisionDetector::CalculateCrash() would be called, because a collision had been detected between the bounding boxes of two cars
• however, subsequently CollisionDetectionPostCrash::GetFirstContact() could not determine the first contact between the two polygons
• subsequently, CollisionDetectionPostCrash::CreatePostCrashDynamics() could not fill in the correct values (speed, etc.) for the collision.

This is probably related to #247 (closed) - at least that is where we encountered the bug.

The reason for this bug appears to be: sometimes CalculateCrash() is called for a time T that is a few milliseconds before the actual collision of the polygons.

This fix extends the logic in GetFirstContact() The old logic was:

• starting with T1=0, search backwards in 100ms steps, until a time T2 where both shapes do not collide
• then search for the exact time of first contact between T1 and T2

The new behaviour adds another step before that:

• if the polygons do not collide at T=0, search forwards and backwards to find a time T1 where both shapes do collide
• then, as before, starting with T1, search backwards in 100ms steps, until a time T2 where both shapes do not collide
• then, as before search for the exact time of first contact between T1 and T2

This fix also includes some log warnings and errors when the collision detection fails, and some debug log messages in the normal case. To allow logging, FrameworkModuleContainer now passes the CallbackInterface to the CollisionDetector.

Benjamin Hohl, on behalf of Mercedes-Benz Tech Innovation GmbH, Provider Information

sim/src/core/opSimulation/framework/srcCollisionPostCrash/collisionDetection_Impact_implementation.cpp

@@ -436,7 +436,11 @@ bool CollisionDetectionPostCrash::CalculatePostCrashDynamic(Common::Vector2d<uni
   //for debug purpose
   // LogPostCrashDynamic(postCrashDynamic1, agent1->GetId());
   // LogPostCrashDynamic(postCrashDynamic2, agent2->GetId());
-
+  {
+    std::stringstream msg;
+    msg << "PostCrashDynamic calculated between agent " << agent1->GetId() << " and agent " << agent2->GetId(); 
+    LOGDEBUG(msg.str());
+  }
   return true;  // Calculation successful
 }
 
@@ -471,62 +475,111 @@ bool CollisionDetectionPostCrash::GetFirstContact(const AgentInterface *agent1,
 
   Polygon agent2Polygon(agent2Corners);
 
+  {
+    std::stringstream msg;
+    msg << "Determining first contact between agent " << agent1->GetId() << " and agent " << agent2->GetId() << ". polygon1[";
+    for (auto vertex : agent1Polygon.GetVertices())
+    {
+      msg << vertex.x << "," << vertex.y << "; ";
+    }
+    msg << "] polygon2[";
+    for (auto vertex : agent2Polygon.GetVertices())
+    {
+      msg << vertex.x << "," << vertex.y << "; ";
+    }
+    msg << "]";
+    LOGDEBUG(msg.str());
+  }
   Common::Vector2d<units::velocity::meters_per_second_t> agent1VelocityVector = GetAgentVelocityVector(agent1);
   Common::Vector2d<units::velocity::meters_per_second_t> agent2VelocityVector = GetAgentVelocityVector(agent2);
 
-  int cycleTime = 100;  // assumption
 
-  timeFirstContact = 0;       //start of range
-  int lastTimeNoContact = 0;  //end of range
+  int cycleTime = 100;  // assumption
 
   // Check if velocities are nearly same. If true, time of first contact will be very high
   // (infinity if velocities are exactly the same)
   if ((agent1VelocityVector - agent2VelocityVector).Length() < units::velocity::meters_per_second_t(1E-5))
   {
+    std::stringstream msg;
+    msg << "Cannot determine first contact between agent " << agent1->GetId() << " and agent " << agent2->GetId()
+        << ": velocities are nearly same: " << agent1VelocityVector << " and " << agent2VelocityVector << " !"; 
+    LOGWARN(msg.str());
+    timeFirstContact = 0;
     return false;
   }
 
-  //get last time with no contact --> calculate end of range
-  bool intersected = true;
-  while (intersected)
-  {
-    timeFirstContact = lastTimeNoContact;
-    lastTimeNoContact -= cycleTime;  // one cycleTime to the past --> negative
+  int timeDuringCollision = 0;
+  bool collisionFound = ShiftPolygonsAndCheckIntersection(agent1Polygon, agent2Polygon, agent1VelocityVector, agent2VelocityVector, timeDuringCollision);
 
-    const auto time = units::make_unit<units::time::millisecond_t>(lastTimeNoContact);
-    Common::Vector2d<units::length::meter_t> shiftVector1{agent1VelocityVector.x * time, agent1VelocityVector.y * time};
-    Common::Vector2d<units::length::meter_t> shiftVector2{agent2VelocityVector.x * time, agent2VelocityVector.y * time};
+  if (!collisionFound) {
+    // there are valid reasons why the shapes may not collide at t=0, even thought the CollisionDetection was triggered at that time:
+    // e.g. the trigger is based on the bounding boxes, but the actual shapes may not be rectangles.
+    // however, currently this happens occasionally with rectangular axis-aligned shapes, which may indicate a bug.
 
-    intersected = ShiftPolygonsAndCheckIntersection(agent1Polygon, agent2Polygon, shiftVector1, shiftVector2);
+    const int searchWindow = cycleTime*100;
+    const int stepSize = std::max(1, cycleTime/10);
+    for (int offsetFromZero = 1; offsetFromZero <= searchWindow && !collisionFound; offsetFromZero++)
+    {
+      for(int sign = -1; sign <= 1 && !collisionFound; sign += 2)
+      {
+        const int time = sign*offsetFromZero*stepSize;
+        if (ShiftPolygonsAndCheckIntersection(agent1Polygon, agent2Polygon, agent1VelocityVector, agent2VelocityVector, time)) {
+          timeDuringCollision = time;
+          collisionFound = true;
+        }
+      }
+    }
   }
 
-  bool everIntersected = false;
-  //calculate first time of contact with range getting smaller
-  while (std::abs(timeFirstContact - lastTimeNoContact) > 1)
-  {
-    int nextTime = lastTimeNoContact - (lastTimeNoContact - timeFirstContact) / 2;
-    const auto time = units::make_unit<units::time::millisecond_t>(nextTime);
-
-    Common::Vector2d<units::length::meter_t> shiftVector1{agent1VelocityVector.x * time, agent1VelocityVector.y * time};
-    Common::Vector2d<units::length::meter_t> shiftVector2{agent2VelocityVector.x * time, agent2VelocityVector.y * time};
+  if (!collisionFound) {
+    std::stringstream msg;
+    msg << "Cannot determine first contact between agent " << agent1->GetId() << " and agent " << agent2->GetId()
+        << ": their polygons never overlap! polygon1[";
+    for (auto vertex : agent1Polygon.GetVertices())
+    {
+      msg << vertex.x << "," << vertex.y << "; ";
+    }
+    msg << "] polygon2[";
+    for (auto vertex : agent2Polygon.GetVertices())
+    {
+      msg << vertex.x << "," << vertex.y << "; ";
+    } 
+    msg << "]";
+    LOGWARN(msg.str());
+    timeFirstContact = 0;
+    return false;
+  }
 
-    intersected = ShiftPolygonsAndCheckIntersection(agent1Polygon, agent2Polygon, shiftVector1, shiftVector2);
+  int timeBeforeCollision = timeDuringCollision-cycleTime;
+  bool startOfCollisionFound = !ShiftPolygonsAndCheckIntersection(agent1Polygon, agent2Polygon, agent1VelocityVector, agent2VelocityVector, timeBeforeCollision);;
+  
+  while (!startOfCollisionFound)
+  {
+    timeDuringCollision -= cycleTime;
+    timeBeforeCollision -= cycleTime;
+    startOfCollisionFound = !ShiftPolygonsAndCheckIntersection(agent1Polygon, agent2Polygon, agent1VelocityVector, agent2VelocityVector, timeBeforeCollision);
+  }
 
-    if (intersected)
+  while (std::abs(timeBeforeCollision - timeDuringCollision) > 1)
+  {
+    int middle = timeBeforeCollision + (timeDuringCollision - timeBeforeCollision) / 2;
+    if (ShiftPolygonsAndCheckIntersection(agent1Polygon, agent2Polygon, agent1VelocityVector, agent2VelocityVector, middle))
     {
-      timeFirstContact = nextTime;
-      everIntersected = true;
+      timeDuringCollision = middle;
     }
     else
     {
-      lastTimeNoContact = nextTime;
+      timeBeforeCollision = middle;
     }
   }
 
-  if (!everIntersected)
   {
-    return false;
+    std::stringstream msg;	
+    msg << "First contact between agent " << agent1->GetId() << " and agent " << agent2->GetId() << " at time T" << timeDuringCollision;
+    LOGDEBUG(msg.str());
   }
+
+  timeFirstContact = timeDuringCollision;
   return true;
 }
 
@@ -589,6 +642,22 @@ Common::Vector2d<units::velocity::meters_per_second_t> CollisionDetectionPostCra
   return agent->GetVelocity();
 }
 
+bool CollisionDetectionPostCrash::ShiftPolygonsAndCheckIntersection(
+    Polygon polygon1,
+    Polygon polygon2,
+    Common::Vector2d<units::velocity::meters_per_second_t> velocity1,
+    Common::Vector2d<units::velocity::meters_per_second_t> velocity2,
+    int timeShiftMs) {
+
+  const auto time = units::make_unit<units::time::millisecond_t>(timeShiftMs);
+  Common::Vector2d<units::length::meter_t> shiftVector1{velocity1.x * time, velocity1.y * time};
+  Common::Vector2d<units::length::meter_t> shiftVector2{velocity2.x * time, velocity2.y * time};
+
+
+  return ShiftPolygonsAndCheckIntersection(polygon1, polygon2, shiftVector1, shiftVector2);
+}
+
+
 bool CollisionDetectionPostCrash::ShiftPolygonsAndCheckIntersection(
     Polygon polygon1,
     Polygon polygon2,
@@ -709,6 +778,9 @@ bool CollisionDetectionPostCrash::CreatePostCrashDynamics(const AgentInterface *
   timeOfFirstContact = 0;
   if (!GetFirstContact(agent1, agent2, timeOfFirstContact))
   {
+    std::stringstream msg;
+    msg << "Cannot calculate PostCrashDynamics: No collision detected between agent " << agent1->GetId() << " and agent "<< agent2->GetId();
+    LOGERROR(msg.str());
     return false;
   }
   //