Commit 58f57b28 authored by VAN TOLL Wouter's avatar VAN TOLL Wouter

Fixed bugs in TtcaDca::GetGradient():

- Removed an incorrect minus sign.
- Now correcting dv and dp by the agent radii, as in GetCost.
- Now using dv instead of the "regular" velocity when necessary.

TtcaDca is now using the closed-form gradient again, instead of the approximation with sampling.
parent d30a8c6d
......@@ -53,7 +53,7 @@ std::pair<float, float> TtcaDca::getMovementCostGradient(const Vector2D& velocit
float ds = (velocity.magnitude() - agent->getPreferredSpeed());
// Gradients of the movement cost C_m (Eq. 14 and 15 in Dutra et al.)
double DC_angle = -alpha / (2*sigAngle_goal_*sigAngle_goal_) * exp(-0.5 * pow(alpha / sigAngle_goal_, 2.0));
double DC_angle = alpha / (2*sigAngle_goal_*sigAngle_goal_) * exp(-0.5 * pow(alpha / sigAngle_goal_, 2.0));
double DC_speed = ds / (2*sigSpeed_goal_*sigSpeed_goal_) * exp(-0.5 * pow(ds / sigSpeed_goal_, 2.0));
return { (float)DC_angle, (float)DC_speed };
......@@ -84,8 +84,8 @@ float TtcaDca::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase *
continue;
// Compute relative velocity and relative position
const Vector2D& relPos = neighborPos - Position;
const Vector2D& relVelocity = neighbor.GetVelocity() - velocity;
//const Vector2D& relPos = neighborPos - Position;
//const Vector2D& relVelocity = neighbor.GetVelocity() - velocity;
// Only see agents in front of the direction of motion
// --> disabled because it makes the cost function non-smooth
......@@ -107,8 +107,11 @@ float TtcaDca::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase *
//if (ttca_dca.first < 0)
// continue;
// computing the cost for this neighbor
float scale = 1 / relPos.sqrMagnitude(); //simulate num of pixels of an agent in screen
// The original method does this per pixel; we do it per obstacle.
// To simulate the "number of pixels" for this obstacle, scale by the distance
float distance = (neighborPos - Position).magnitude() - Radius - neighbor.realAgent->getRadius();
float scale = 1 / (distance*distance); //simulate num of pixels of an agent in screen
ObstacleCostScale += scale;
float cost = costForTtcaDca(ttca_dca.first, ttca_dca.second) * scale;
......@@ -128,101 +131,112 @@ float TtcaDca::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase *
Vector2D TtcaDca::GetGradient(const Vector2D& velocity, Agent* agent, const WorldBase * world) const
{
// For now, use the approximated gradient
return CostFunction::GetGradient(velocity, agent, world);
// Below is the code for the analytical gradient. It seems incorrect, although it literally follows the paper by Dutra et al.
const float Radius = agent->getRadius();
const Vector2D& Position = agent->getPosition();
const float VelMagnitude = velocity.magnitude();
const Vector2D& VelNorm = velocity / VelMagnitude;
const Vector2D VelRot(velocity.y, -velocity.x);
const float rangeSquared = range_ * range_;
// --- collision avoidance
float GradTh = 0;
float GradS = 0;
float totalScale = 0;
const auto& neighbors = agent->getNeighbors();
const auto& neighbors = agent->getNeighbors();
// for each agent of the neighbourhood
for (const auto& neighbor : neighbors.first)
{
{
if (neighbor.GetDistanceSquared() > rangeSquared)
continue;
// Compute relative velocity and relative position
const Vector2D& relPos = neighbor.GetPosition() - Position;
const Vector2D& relVelocity = neighbor.GetVelocity() - velocity;
// Only see agents in front of the direction of motion
if (relPos.dot(velocity) < 0)
continue;
// ------------------
// --- The following code is very similar to CostFunction::ComputeTimeAndDistanceToClosestApproach,
// but we need certain components of it later in the calculation.
// there is adaptation only if relative velocity is not zero
float relVelocitySqrMagnitude = relVelocity.sqrMagnitude();
if (relVelocitySqrMagnitude <= eps)
continue;
const Vector2D dp_center(neighbor.GetPosition() - Position);
const Vector2D dv_center(neighbor.GetVelocity() - velocity);
/*// Option 1: TTCA and DCA according to our reusable functions
const auto& ttca_dca = ComputeTimeAndDistanceToClosestApproach(
Position, velocity, Radius,
neighbor.position, neighbor.velocity, neighbor.realAgent->getRadius());
float ttca = ttca_dca.first;
float dca = ttca_dca.second;
const Vector2D& vdca = (relPos + ttca * relVelocity);*/
// Option 2: TTCA and DCA according to Dutra et al.
// (the only difference is that it does not include agents' radii for DCA)
float ttca = -relPos.dot(relVelocity) / relVelocitySqrMagnitude;
const Vector2D& vdca = (relPos + ttca * relVelocity);
float dca = vdca.magnitude();
if (ttca <= eps)
continue;
// 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 - Radius - neighbor.realAgent->getRadius());
Vector2D dpNormal(-dp_center.y, dp_center.x);
const Vector2D dv = dv_center + dpNormal * dpNormal.dot(dv_center);
float dvSqrMagnitude = dv.sqrMagnitude();
// Source: Dutra et al, "Gradient-based steering for vision-based crowd simulation algorithms", 2016.
float ttca = (dvSqrMagnitude == 0 ? 0 : -dp.dot(dv) / dvSqrMagnitude);
// 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;
const Vector2D vdca(dp + dv * ttca);
float dca = vdca.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;
// --- End copy from CostFunction::ComputeTimeAndDistanceToClosestApproach
// ------------------
// Only see agents in front of the direction of motion
// --> disabled because it makes the cost function non-smooth
//if (dp.dot(velocity) < 0)
// continue;
// skip extreme TTCAs
if (ttca > 10)
// If the relative velocity is almost zero, then the gradient is very unreliable; ignore it.
// This is dangerous in GetCost(), but acceptable for GetGradient().
if (dvSqrMagnitude <= eps)
continue;
// Partial derivatives of TTCA (Eq. 29 and 30 of Dutra et al.)
const Vector2D& rel = (relPos + 2 * ttca*relVelocity);
float gradTtcaAngle = -rel.dot(VelRot) / relVelocitySqrMagnitude;
float gradTtcaSpeed = rel.dot(VelNorm) / relVelocitySqrMagnitude;
const Vector2D VelRot(dv.y, -dv.x);
const float VelMagnitude = sqrt(dvSqrMagnitude);
const Vector2D& VelNorm = dv / VelMagnitude;
// Partial derivatives of TTCA (Eq. 29 and 30 of Dutra et al.)
float gradTtcaAngle, gradTtcaSpeed;
if (abs(ttca) > eps)
{
const Vector2D& rel = (dp + 2 * ttca*dv);
gradTtcaAngle = -rel.dot(VelRot) / dvSqrMagnitude;
gradTtcaSpeed = rel.dot(VelNorm) / dvSqrMagnitude;
}
else
{
// TTCA is too small, so gradient cannot be properly determined
gradTtcaAngle = 0;
gradTtcaSpeed = 0;
}
// Partial derivatives of DCA (Eq. 36 and 37 of Dutra et al.)
float gradDcaAngle, gradDcaSpeed;
float gradDcaAngle, gradDcaSpeed;
if (abs(dca) > eps)
{
gradDcaAngle = vdca.dot(gradTtcaAngle*relVelocity + ttca * VelRot) / dca;
gradDcaSpeed = vdca.dot(gradTtcaSpeed*relVelocity - ttca * VelNorm) / dca;
}
else
{
// DCA is too small, so gradient cannot be properly determined
gradDcaAngle = 0;
gradDcaSpeed = 0;
}
gradDcaAngle = vdca.dot(gradTtcaAngle*dv + ttca * VelRot) / dca;
gradDcaSpeed = vdca.dot(gradTtcaSpeed*dv - ttca * VelNorm) / dca;
}
else
{
// DCA is too small, so gradient cannot be properly determined
gradDcaAngle = 0;
gradDcaSpeed = 0;
}
// Overall gradients of the obstacle cost function (Eq. 16 and 17 in Dutra et al.)
float cost = costForTtcaDca(ttca, dca);
float cost = costForTtcaDca(ttca, dca);
float ttcaFrac = ttca / (sigTtca_*sigTtca_);
float dcaFrac = dca / (sigDca_*sigDca_);
float gradCSpeed = -cost * (gradTtcaSpeed * ttcaFrac + gradDcaSpeed * dcaFrac);
float gradCAngle = -cost * (gradTtcaAngle * ttcaFrac + gradDcaAngle * dcaFrac);
float gradCSpeed = -cost * (gradTtcaSpeed * ttcaFrac + gradDcaSpeed * dcaFrac);
float gradCAngle = -cost * (gradTtcaAngle * ttcaFrac + gradDcaAngle * dcaFrac);
// The original method does this per pixel; we do it per obstacle.
// To simulate the "number of pixels" for this obstacle, scale by the distance
float scale = 1 / relPos.sqrMagnitude();
float scale = 1 / dp.sqrMagnitude();
totalScale += scale;
GradTh += gradCAngle * scale;
GradS += gradCSpeed * scale;
}
}
// TODO: check neighboring obstacles
// ...
......@@ -240,21 +254,20 @@ Vector2D TtcaDca::GetGradient(const Vector2D& velocity, Agent* agent, const Worl
const auto& GradThS_movement = getMovementCostGradient(velocity, agent);
GradTh += GradThS_movement.first;
GradS += GradThS_movement.second;
// clamp the change in angle
//GradTh = clampVector(GradTh, -1.f, 1.f);
// --- convert to a gradient in Euclidean velocity space
Vector2D Gradient;
if (VelMagnitude < 0.1)
if (velocity.sqrMagnitude() < 0.01)
{
// If the velocity is zero, then the transformation will return (0,0) and the agent will never start moving.
// If the velocity is (almost) zero, then the transformation will return (nearly) (0,0) and the agent will never start moving.
Gradient = RotateGradientToEuclideanCoordinates(GradTh, GradS,
agent->getPreferredVelocity().getnormalized(), agent->getPreferredVelocity().magnitude());
}
else
{
Gradient = RotateGradientToEuclideanCoordinates(GradTh, GradS, VelNorm, VelMagnitude);
Gradient = RotateGradientToEuclideanCoordinates(GradTh, GradS, velocity.getnormalized(), velocity.magnitude());
}
return Gradient;
......
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