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::calcWallRepulsionForce(float dt) { //std::cerr<<"<<<calcWallRepulsionForce>>> Please Implement my body\n"; Util::Vector wall_repulsion_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::ObstacleInterface * tempObstacle; for (std::set<SteerLib::SpatialDatabaseItemPtr>::iterator neighbour = _neighbors.begin(); neighbour != _neighbors.end(); neighbour++) { if (!(*neighbour)->isAgent()) { tempObstacle = dynamic_cast<SteerLib::ObstacleInterface *>(*neighbour); } else { continue; } if (tempObstacle->computePenetration(this->position(), this->radius()) > 0.000001) { 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()); wall_repulsion_force = wall_normal * (min_stuff.first + radius()) * _SocialForcesParams.sf_body_force; } } return wall_repulsion_force; }
Util::Vector SocialForcesAgent::calcWallRepulsionForce(float dt) { Util::Vector wallRepulsionForce = 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::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()); // Repulsion wallRepulsionForce += wallNormal * (radius() - distance.first) * _SocialForcesParams.sf_body_force; // Friction Util::Vector wallTangent = normalize(Util::Vector(velocity().x * abs(wallNormal.z), 0, velocity().z * abs(wallNormal.x))); wallRepulsionForce -= _SocialForcesParams.sf_sliding_friction_force // k * (radius() - distance.first) // g * (velocity() * wallTangent) * wallTangent; } } } return wallRepulsionForce; }
Util::Vector SocialForcesAgent::calcWallRepulsionForce(float dt) { Util::Vector wallRepulsionForce = Util::Vector(0, 0, 0); SteerLib::ObstacleInterface *objectInterface; 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)); std::set<SteerLib::SpatialDatabaseItemPtr>::iterator neighbor = _neighbors.begin(); while (neighbor != _neighbors.end()){ if (!(*neighbor)->isAgent()){ objectInterface = dynamic_cast<SteerLib::ObstacleInterface *>(*neighbor); float penetration = objectInterface->computePenetration(this->position(), this->radius()); if (penetration){ Util::Vector objectNormal = calcWallNormal(objectInterface); // std::pair<Util::Point, Util::Point> wallPoints = calcWallPointsFromNormal(objectInterface, objectNormal); // std::pair<float, Util::Point> minDistToPoint = minimum_distance(wallPoints.first, wallPoints.second, position()); //Does this have units of force? wallRepulsionForce += objectNormal * penetration * _SocialForcesParams.sf_body_force * dt; } } neighbor++; } return wallRepulsionForce; }