Commit 82b560d5 authored by VAN TOLL Wouter's avatar VAN TOLL Wouter

Merge branch 'Dutra-improvements' into 'master'

Dutra improvements

See merge request !105
parents 26f071b2 5eda2fb0
This diff is collapsed.
......@@ -64,6 +64,8 @@ private:
float sigTtca_ = 1;
float sigDca_ = 0.3f;
const float viewingAngleHalf_ = (float)(PI / 2.0);
float costForTtcaDca(float ttca, float dca) const;
float getMovementCost(const Vector2D& velocity, const Agent* agent) const;
......
......@@ -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
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment