예제 #1
0
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;
}
예제 #3
0
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;
}
예제 #4
0
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;
}