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;
}
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;
}