Commit ba6cbc5e authored by VAN TOLL Wouter's avatar VAN TOLL Wouter

TtcaDca now ignores neighbors behind the agent (assuming that the velocity ==...

TtcaDca now ignores neighbors behind the agent (assuming that the velocity == the viewing direction). This is important for this particular navigation algorithm.

TODO: Consider adding a "viewing angle" option to all cost functions. However, it is debatable whether or not this is part of an algorithm.
parent 58f57b28
......@@ -84,13 +84,12 @@ 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
//if (relPos.dot(velocity) < 0)
// continue;
// ignore neighbors that are behind the agent; the original Dutra method uses rendering, and agents always face forward
if (angle(relVelocity, agent->getVelocity()) > viewingAngleHalf_)
continue;
// there is adaptation only if relative velocity is not zero
// --> disabled because it makes the cost function non-smooth
......@@ -110,7 +109,7 @@ float TtcaDca::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase *
// 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 distance = relPos.magnitude() - Radius - neighbor.realAgent->getRadius();
float scale = 1 / (distance*distance); //simulate num of pixels of an agent in screen
ObstacleCostScale += scale;
......@@ -156,6 +155,10 @@ Vector2D TtcaDca::GetGradient(const Vector2D& velocity, Agent* agent, const Worl
const Vector2D dp_center(neighbor.GetPosition() - Position);
const Vector2D dv_center(neighbor.GetVelocity() - velocity);
// ignore neighbors that are behind the agent; the original Dutra method uses rendering, and agents always face forward
if (angle(dp_center, agent->getVelocity()) > viewingAngleHalf_)
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();
......@@ -179,11 +182,6 @@ Vector2D TtcaDca::GetGradient(const Vector2D& velocity, Agent* agent, const Worl
// --- 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;
// 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)
......@@ -194,33 +192,23 @@ Vector2D TtcaDca::GetGradient(const Vector2D& velocity, Agent* agent, const Worl
const Vector2D& VelNorm = dv / VelMagnitude;
// Partial derivatives of TTCA (Eq. 29 and 30 of Dutra et al.)
float gradTtcaAngle, gradTtcaSpeed;
float gradTtcaAngle = 0, gradTtcaSpeed = 0;
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;
}
// otherwise TTCA is too small, so gradient cannot be properly determined
// Partial derivatives of DCA (Eq. 36 and 37 of Dutra et al.)
float gradDcaAngle, gradDcaSpeed;
float gradDcaAngle = 0, gradDcaSpeed = 0;
if (abs(dca) > eps)
{
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;
}
// otherwise DCA is too small, so gradient cannot be properly determined
// Overall gradients of the obstacle cost function (Eq. 16 and 17 in Dutra et al.)
float cost = costForTtcaDca(ttca, dca);
......
......@@ -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;
......
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