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