Ped::Tvector ShoppingPlanner::createRandomOffset() const
{
const double radiusStd = 4;
std::normal_distribution<double> radiusDistribution ( 0, radiusStd );
double radius = radiusDistribution ( RNG() );
std::discrete_distribution<int> angleDistribution {0,45,90,135,180,225,270,315,360};
double angle = angleDistribution ( RNG() );
Ped::Tvector randomOffset = Ped::Tvector::fromPolar ( Ped::Tangle::fromDegree ( angle ), radius );
// only update for significant shopping idea change
if (randomOffset.lengthSquared() < 2.0)
return Ped::Tvector(0, 0, 0);
return randomOffset;
}
/// Calculates the force between this agent and the nearest obstacle in this scene.
/// Iterates over all obstacles == O(N).
/// \date 2012-01-17
/// \return Tvector: the calculated force
Ped::Tvector Ped::Tagent::obstacleForce() {
// obstacle which is closest only
Ped::Tvector minDiff;
double minDistanceSquared = INFINITY;
for (const Tobstacle* obstacle : scene->obstacles) {
Ped::Tvector closestPoint = obstacle->closestPoint(p);
Ped::Tvector diff = p - closestPoint;
double distanceSquared = diff.lengthSquared(); // use squared distance to avoid computing square root
if (distanceSquared < minDistanceSquared) {
minDistanceSquared = distanceSquared;
minDiff = diff;
}
}
double distance = sqrt(minDistanceSquared) - agentRadius;
double forceAmount = exp(-distance/obstacleForceSigma);
return forceAmount * minDiff.normalized();
}
void QueueingWaypointPlanner::onFollowedAgentPositionChanged ( double xIn, double yIn )
{
// sanity checks
if ( currentWaypoint == nullptr )
{
ROS_DEBUG ( "Queued agent cannot update queueing position, because there's no waypoint set!" );
return;
}
Ped::Tvector followedPosition ( xIn, yIn );
addPrivateSpace ( followedPosition );
//HACK: don't update minor changes (prevent over-correcting)
//TODO: integrate update importance to waypoint (force?)
const double minUpdateDistance = 0.7;
Ped::Tvector diff = followedPosition - currentWaypoint->getPosition();
if ( diff.length() < minUpdateDistance )
return;
currentWaypoint->setPosition ( followedPosition );
}