Util::Vector SocialForcesAgent::calcProximityForce(float dt)
{
// Find all neighbors.
std::set<SteerLib::SpatialDatabaseItemPtr> _neighbors;
gSpatialDatabase->getItemsInRange(_neighbors,
_position.x - this->_radius - _SocialForcesParams.sf_query_radius,
_position.x + this->_radius + _SocialForcesParams.sf_query_radius,
_position.z - this->_radius - _SocialForcesParams.sf_query_radius,
_position.z + this->_radius + _SocialForcesParams.sf_query_radius,
dynamic_cast<SteerLib::SpatialDatabaseItemPtr>(this));
// Iterate through neighbors.
std::set<SteerLib::SpatialDatabaseItemPtr>::iterator neighbor = _neighbors.begin();
Util::Vector agentForce = Util::Vector(0,0,0);
Util::Vector obstacleForce = Util::Vector(0,0,0);
SteerLib::AgentInterface *neighborInterface;
SteerLib::ObstacleInterface *objectInterface;
Util::Vector differenceNormal;
float exponentialFactor;
while (neighbor != _neighbors.end()){
if ((*neighbor)->isAgent()){
neighborInterface = dynamic_cast<SteerLib::AgentInterface *>(*neighbor);
differenceNormal = normalize(position() - neighborInterface->position());
exponentialFactor = exp((radius() + neighborInterface->radius() - (position() - neighborInterface->position()).length())
/ _SocialForcesParams.sf_agent_b);
agentForce += differenceNormal * _SocialForcesParams.sf_agent_a * exponentialFactor * dt;
}
else{
objectInterface = dynamic_cast<SteerLib::ObstacleInterface*>(*neighbor);
Util::Vector obstacleNormal = calcWallNormal(objectInterface);
std::pair<Util::Point, Util::Point> wallPoints = calcWallPointsFromNormal(objectInterface, obstacleNormal);
std::pair<float, Util::Point> minDistToPoint = minimum_distance(wallPoints.first, wallPoints.second, position());
differenceNormal = normalize(position() - minDistToPoint.second);
exponentialFactor = exp((radius() - (position() - minDistToPoint.second).length()) / _SocialForcesParams.sf_wall_b);
obstacleForce += differenceNormal * _SocialForcesParams.sf_wall_a * exponentialFactor * dt;
}
++neighbor;
}
return agentForce + obstacleForce;
}
Util::Vector SocialForcesAgent::calcProximityForce(float dt)
{
Util::Vector proximity_force = Util::Vector(0, 0, 0);
std::set<SteerLib::SpatialDatabaseItemPtr> _neighbors;
gSpatialDatabase->getItemsInRange(_neighbors,
_position.x - (this->_radius + _SocialForcesParams.sf_query_radius),
_position.x + (this->_radius + _SocialForcesParams.sf_query_radius),
_position.z - (this->_radius + _SocialForcesParams.sf_query_radius),
_position.z + (this->_radius + _SocialForcesParams.sf_query_radius),
dynamic_cast<SteerLib::SpatialDatabaseItemPtr>(this));
SteerLib::AgentInterface * tempAgent;
SteerLib::ObstacleInterface * tempObstacle;
Util::Vector dist = Util::Vector(0, 0, 0);
for (std::set<SteerLib::SpatialDatabaseItemPtr>::iterator neighbour = _neighbors.begin(); neighbour != _neighbors.end(); neighbour++)
{
if ((*neighbour)->isAgent())
{
tempAgent = dynamic_cast<SteerLib::AgentInterface *>(*neighbour);
Util::Vector distTemp = normalize(_position - tempAgent->position());
dist = dist +
(distTemp * _SocialForcesParams.sf_agent_a *
exp(((this->radius() + tempAgent->radius()) - (this->position() - tempAgent->position()).length() ) / _SocialForcesParams.sf_agent_b)
);
}
else
{
tempObstacle = dynamic_cast<SteerLib::ObstacleInterface*>(*neighbour);
Util::Vector wall_normal = calcWallNormal(tempObstacle);
std::pair<Util::Point, Util::Point> line = calcWallPointsFromNormal(tempObstacle, wall_normal);
std::pair<float, Util::Point> min_stuff = minimum_distance(line.first, line.second, position());
Util::Vector distObstacle = normalize(_position - min_stuff.second);
dist = dist +
(distObstacle * _SocialForcesParams.sf_wall_a *
exp(((this->radius()) - (this->position() - min_stuff.second).length() ) / _SocialForcesParams.sf_wall_b)
);
}
}
proximity_force = dist;
return proximity_force;
}
Util::Vector SocialForcesAgent::calcProximityForce(float dt)
{
Util::Vector proximityForce = Util::Vector(0,0,0);
std::set<SteerLib::SpatialDatabaseItemPtr> _neighbors;
gSpatialDatabase->getItemsInRange(_neighbors,
position().x - (radius() + _SocialForcesParams.sf_query_radius),
position().x + (radius() + _SocialForcesParams.sf_query_radius),
position().z - (radius() + _SocialForcesParams.sf_query_radius),
position().z + (radius() + _SocialForcesParams.sf_query_radius),
this);
for (std::set<SteerLib::SpatialDatabaseItemPtr>::iterator neighbor = _neighbors.begin();
neighbor != _neighbors.end(); neighbor++) {
if ((*neighbor)->isAgent()) {
SteerLib::AgentInterface *other = dynamic_cast<SteerLib::AgentInterface *>(*neighbor);
if (id() != other->id()) {
proximityForce += _SocialForcesParams.sf_agent_a
* exp(((radius()+other->radius())-(position()-other->position()).length())/_SocialForcesParams.sf_agent_b)
* normalize(position() - other->position());
}
}
else {
SteerLib::ObstacleInterface *wall = dynamic_cast<SteerLib::ObstacleInterface *>(*neighbor);
if (wall->computePenetration(position(), radius()) > 0.000001) {
Util::Vector wallNormal = calcWallNormal(wall);
std::pair<Util::Point, Util::Point> edge = calcWallPointsFromNormal(wall, wallNormal);
std::pair<float, Util::Point> distance = minimum_distance(edge.first, edge.second, position());
proximityForce += _SocialForcesParams.sf_wall_a
* exp( (radius()-distance.first) / _SocialForcesParams.sf_wall_b )
* normalize(position() - distance.second);
}
}
}
return proximityForce;
}
Util::Vector SocialForcesAgent::calcAgentRepulsionForce(float dt)
{
Util::Vector agentRepulsionForce = Util::Vector(0,0,0);
std::set<SteerLib::SpatialDatabaseItemPtr> _neighbors;
gSpatialDatabase->getItemsInRange(_neighbors,
position().x - (radius() + _SocialForcesParams.sf_query_radius),
position().x + (radius() + _SocialForcesParams.sf_query_radius),
position().z - (radius() + _SocialForcesParams.sf_query_radius),
position().z + (radius() + _SocialForcesParams.sf_query_radius),
this);
for (std::set<SteerLib::SpatialDatabaseItemPtr>::iterator neighbor = _neighbors.begin();
neighbor != _neighbors.end(); neighbor++) {
if ((*neighbor)->isAgent()) {
SteerLib::AgentInterface *other = dynamic_cast<SteerLib::AgentInterface *>(*neighbor);
if (id() != other->id() && other->computePenetration(position(), radius()) > 0.000001) {
Util::Vector agentNorm = normalize(position() - other->position());
// Repulsion
agentRepulsionForce += _SocialForcesParams.sf_agent_body_force
* other->computePenetration(position(), radius())
* agentNorm;
// Friction
Util::Vector agentTangent = Util::Vector(-agentNorm.z, 0, agentNorm.x);
agentRepulsionForce -= _SocialForcesParams.sf_sliding_friction_force // k
* other->computePenetration(position(), radius()) // g
* ((velocity() - other->velocity()) * agentTangent) // tangential velocity difference
* agentTangent; // tangential direction
}
}
}
return agentRepulsionForce;
}
Util::Vector SocialForcesAgent::calcAgentRepulsionForce(float dt)
{
Util::Vector agentRepulsion = Util::Vector(0, 0, 0);
std::set<SteerLib::SpatialDatabaseItemPtr> _neighbors;
gSpatialDatabase->getItemsInRange(_neighbors,
_position.x - (this->_radius + _SocialForcesParams.sf_query_radius),
_position.x + (this->_radius + _SocialForcesParams.sf_query_radius),
_position.z - (this->_radius + _SocialForcesParams.sf_query_radius),
_position.z + (this->_radius + _SocialForcesParams.sf_query_radius),
dynamic_cast<SteerLib::SpatialDatabaseItemPtr>(this));
SteerLib::AgentInterface * tempAgent;
for (std::set<SteerLib::SpatialDatabaseItemPtr>::iterator neighbour = _neighbors.begin(); neighbour != _neighbors.end(); neighbour++)
{
if ((*neighbour)->isAgent())
{
tempAgent = dynamic_cast<SteerLib::AgentInterface *>(*neighbour);
}
else
{
continue;
}
if ((id() != tempAgent->id()) && (tempAgent->computePenetration(this->position(), this->radius()) > 0.000001))
{
agentRepulsion = agentRepulsion + (tempAgent->computePenetration(this->position(), this->radius()) * _SocialForcesParams.sf_agent_body_force * normalize(position() - tempAgent->position()));
}
}
return agentRepulsion;
}
Util::Vector SocialForcesAgent::calcAgentRepulsionForce(float dt)
{
Util::Vector agentRepulsionForce = Util::Vector(0, 0, 0);
std::set<SteerLib::SpatialDatabaseItemPtr> _neighbors;
gSpatialDatabase->getItemsInRange(_neighbors,
_position.x - this->_radius - _SocialForcesParams.sf_query_radius,
_position.x + this->_radius + _SocialForcesParams.sf_query_radius,
_position.z - this->_radius - _SocialForcesParams.sf_query_radius,
_position.z + this->_radius + _SocialForcesParams.sf_query_radius,
dynamic_cast<SteerLib::SpatialDatabaseItemPtr>(this));
SteerLib::AgentInterface *neighborInterface;
std::set<SteerLib::SpatialDatabaseItemPtr>::iterator neighbor = _neighbors.begin();
while (neighbor != _neighbors.end())
{
if ((*neighbor)->isAgent())
{
neighborInterface = dynamic_cast<SteerLib::AgentInterface *>(*neighbor);
float penetration = neighborInterface->computePenetration(this->position(), this->radius());
if (id() != neighborInterface->id())
agentRepulsionForce += penetration * _SocialForcesParams.sf_agent_body_force * dt * normalize(position() - neighborInterface->position());
}
neighbor++;
}
return agentRepulsionForce;
}