Пример #1
0
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;
}
Пример #2
0
/// 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();
}
Пример #3
0
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 );
}