CostFunction: Improved correction for the agent radius in TTCA/DCA calculation.

src/Engine/core/costFunction.cpp

@@ -324,16 +324,57 @@ float CostFunction::ComputeTimeToCollision_LineSegmentInterior(const Vector2D& p
 	return std::max(0.0f, timeToCollision - timeToSubtract);
 }
 
+/*std::pair<float, float> CostFunction::ComputeTimeAndDistanceToClosestApproach(
+	const Vector2D& position1, const Vector2D& velocity1, const float radius1,
+	const Vector2D& position2, const Vector2D& velocity2, const float radius2) const
+{
+	// dp = difference in position
+	Vector2D dp(position2 - position1);
+
+	// dv = different in velocity
+	Vector2D dv(velocity2 - velocity1);
+
+	// ttca = time to closest approach. Source: Dutra et al, "Gradient-based steering for vision-based crowd simulation algorithms", 2016.
+	float ttca = (dv.sqrMagnitude() == 0 ? 0 : -dp.dot(dv) / dv.sqrMagnitude());
+
+	// The "classical" definition of ttca does not take agent radii into account,
+	// and in case of a collision, it computes the time to the biggest overlap.
+	// This affects cost functions incorrectly. We use the following variant:
+	// if there is a future collision (i.e. ttc is defined), then ttca = tca. Otherwise, ttca = the classical version.
+
+	float ttc = ComputeTimeToCollision(position1, velocity1, radius1, position2, velocity2, radius2);
+	ttca = std::min(ttca, ttc);
+
+	float dca = (dp + dv * ttca).magnitude() - radius1 - radius2;
+	if (dca < 0) dca = 0;
+
+	return { ttca, dca };
+}*/
+
 std::pair<float, float> CostFunction::ComputeTimeAndDistanceToClosestApproach(
 	const Vector2D& position1, const Vector2D& velocity1, const float radius1,
 	const Vector2D& position2, const Vector2D& velocity2, const float radius2) const
 {
-	const Vector2D dp(position2 - position1);
-	const Vector2D dv(velocity2 - velocity1);
+	const Vector2D dp_center(position2 - position1);
+	const Vector2D dv_center(velocity2 - velocity1);
+	
+	// The standard TTCA definition works with these dp and dv immediately.
+	// We make some corrections to account for the radii of agents:
+	float dpMag = dp_center.magnitude();
+	const Vector2D& dp = dp_center / dpMag * (dpMag - radius1 - radius2);
+	Vector2D dpNormal(-dp_center.y, dp_center.x);
+	const Vector2D dv = dv_center + dpNormal * dpNormal.dot(dv_center);
 
 	// Source: Dutra et al, "Gradient-based steering for vision-based crowd simulation algorithms", 2016.
 	float ttca = (dv.sqrMagnitude() == 0 ? 0 : -dp.dot(dv) / dv.sqrMagnitude());
-	float dca = (dp + dv * ttca).magnitude() - radius1 - radius2;
+	// Prevent negative ttca?
+	// This means that if the agents are moving away from each other, it does not matter how strongly they do this
+	if (ttca < 0) ttca = 0;
+
+	float dca = (dp + dv * ttca).magnitude(); // subtracting the radii is not needed anymore, dp and dv already incorporate this
+	// Prevent negative dca? 
+	// Combined with a non-negative ttca, this can only happen if agents are already colliding now.
+	if (dca < 0) dca = 0;
 
 	return { ttca, dca };
 }
\ No newline at end of file