<commit_before>/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.
    * Neither the name of the Universite de Sherbrooke nor the
      names of its contributors may be used to endorse or promote products
      derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include <ros/ros.h>
#include "rtabmap_ros/MapData.h"
#include "rtabmap_ros/MsgConversion.h"
#include "rtabmap_ros/MapsManager.h"
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_filtering.h>
#include <rtabmap/core/util3d_mapping.h>
#include <rtabmap/core/Compression.h>
#include <rtabmap/core/Graph.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UFile.h>
#include <pcl_ros/transforms.h>
#include <pcl_conversions/pcl_conversions.h>
#include <nav_msgs/OccupancyGrid.h>
#include <std_srvs/Empty.h>

using namespace rtabmap;

class MapAssembler
{

public:
	MapAssembler(int & argc, char** argv)
	{
		ros::NodeHandle pnh("~");
		ros::NodeHandle nh;

		std::string configPath;
		pnh.param("config_path", configPath, configPath);

		//parameters
		rtabmap::ParametersMap parameters;
		uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid"));
		uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM"));
		if(!configPath.empty())
		{
			if(UFile::exists(configPath.c_str()))
			{
				ROS_INFO( "%s: Loading parameters from %s", ros::this_node::getName().c_str(), configPath.c_str());
				rtabmap::ParametersMap allParameters;
				Parameters::readINI(configPath.c_str(), allParameters);
				// only update odometry parameters
				for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
				{
					ParametersMap::iterator jter = allParameters.find(iter->first);
					if(jter!=allParameters.end())
					{
						iter->second = jter->second;
					}
				}
			}
			else
			{
				ROS_ERROR( "Config file \"%s\" not found!", configPath.c_str());
			}
		}
		for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
		{
			std::string vStr;
			bool vBool;
			int vInt;
			double vDouble;
			if(pnh.getParam(iter->first, vStr))
			{
				ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), vStr.c_str());
				iter->second = vStr;
			}
			else if(pnh.getParam(iter->first, vBool))
			{
				ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uBool2Str(vBool).c_str());
				iter->second = uBool2Str(vBool);
			}
			else if(pnh.getParam(iter->first, vDouble))
			{
				ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uNumber2Str(vDouble).c_str());
				iter->second = uNumber2Str(vDouble);
			}
			else if(pnh.getParam(iter->first, vInt))
			{
				ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uNumber2Str(vInt).c_str());
				iter->second = uNumber2Str(vInt);
			}

			if(iter->first.compare(Parameters::kVisMinInliers()) == 0 && atoi(iter->second.c_str()) < 8)
			{
				ROS_WARN( "Parameter min_inliers must be >= 8, setting to 8...");
				iter->second = uNumber2Str(8);
			}
		}

		rtabmap::ParametersMap argParameters = rtabmap::Parameters::parseArguments(argc, argv);
		for(rtabmap::ParametersMap::iterator iter=argParameters.begin(); iter!=argParameters.end(); ++iter)
		{
			rtabmap::ParametersMap::iterator jter = parameters.find(iter->first);
			if(jter!=parameters.end())
			{
				ROS_INFO( "Update %s parameter \"%s\"=\"%s\" from arguments", ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.c_str());
				jter->second = iter->second;
			}
		}

		// Backward compatibility
		for(std::map<std::string, std::pair<bool, std::string> >::const_iterator iter=Parameters::getRemovedParameters().begin();
			iter!=Parameters::getRemovedParameters().end();
			++iter)
		{
			std::string vStr;
			if(pnh.getParam(iter->first, vStr))
			{
				if(iter->second.first && parameters.find(iter->second.second) != parameters.end())
				{
					// can be migrated
					parameters.at(iter->second.second)= vStr;
					ROS_WARN( "%s: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.",
							ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.second.c_str(), vStr.c_str());
				}
				else
				{
					if(iter->second.second.empty())
					{
						ROS_ERROR( "%s: Parameter \"%s\" doesn't exist anymore!",
								ros::this_node::getName().c_str(), iter->first.c_str());
					}
					else
					{
						ROS_ERROR( "%s: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"",
								ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.second.c_str());
					}
				}
			}
		}

		mapsManager_.init(nh, pnh, ros::this_node::getName(), false);
		mapsManager_.setParameters(parameters);

		mapDataTopic_ = nh.subscribe("mapData", 1, &MapAssembler::mapDataReceivedCallback, this);

		// private service
		resetService_ = pnh.advertiseService("reset", &MapAssembler::reset, this);
	}

	~MapAssembler()
	{
	}

	void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr & msg)
	{
		UTimer timer;

		std::map<int, Transform> poses;
		std::multimap<int, Link> constraints;
		Transform mapOdom;
		rtabmap_ros::mapGraphFromROS(msg->graph, poses, constraints, mapOdom);
		for(unsigned int i=0; i<msg->nodes.size(); ++i)
		{
			if(msg->nodes[i].image.size() ||
			   msg->nodes[i].depth.size() ||
			   msg->nodes[i].laserScan.size())
			{
				uInsert(nodes_, std::make_pair(msg->nodes[i].id, rtabmap_ros::nodeDataFromROS(msg->nodes[i])));
			}
		}

		// create a tmp signature with latest sensory data
		if(poses.size() && nodes_.find(poses.rbegin()->first) != nodes_.end())
		{
			Signature tmpS = nodes_.at(poses.rbegin()->first);
			SensorData tmpData = tmpS.sensorData();
			tmpData.setId(-1);
			uInsert(nodes_, std::make_pair(-1, Signature(-1, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), Transform(), tmpData)));
			poses.insert(std::make_pair(-1, poses.rbegin()->second));
		}

		// Update maps
		poses = mapsManager_.updateMapCaches(
				poses,
				0,
				false,
				false,
				nodes_);

		mapsManager_.publishMaps(poses, msg->header.stamp, msg->header.frame_id);

		ROS_INFO("map_assembler: Publishing data = %fs", timer.ticks());
	}

	bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
	{
		ROS_INFO("map_assembler: reset!");
		mapsManager_.clear();
		return true;
	}

private:
	MapsManager mapsManager_;
	std::map<int, Signature> nodes_;

	ros::Subscriber mapDataTopic_;

	ros::ServiceServer resetService_;
};

int main(int argc, char** argv)
{
	ros::init(argc, argv, "map_assembler");

	// process "--params" argument
	for(int i=1;i<argc;++i)
	{
		if(strcmp(argv[i], "--params") == 0)
		{
			rtabmap::ParametersMap parameters;
			uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid"));
			uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM"));
			for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
			{
				std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
				std::cout <<
						str <<
						std::setw(60 - str.size()) <<
						" [" <<
						rtabmap::Parameters::getDescription(iter->first).c_str() <<
						"]" <<
						std::endl;
			}
			ROS_WARN("Node will now exit after showing default parameters because "
					 "argument \"--params\" is detected!");
			exit(0);
		}
		else if(strcmp(argv[i], "--udebug") == 0)
		{
			ULogger::setLevel(ULogger::kDebug);
		}
		else if(strcmp(argv[i], "--uinfo") == 0)
		{
			ULogger::setLevel(ULogger::kInfo);
		}
	}

	MapAssembler assembler(argc, argv);
	ros::spin();
	return 0;
}
<commit_msg>map_assembler: added backward compatibility parameters update from MapsManager<commit_after>/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.
    * Neither the name of the Universite de Sherbrooke nor the
      names of its contributors may be used to endorse or promote products
      derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include <ros/ros.h>
#include "rtabmap_ros/MapData.h"
#include "rtabmap_ros/MsgConversion.h"
#include "rtabmap_ros/MapsManager.h"
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_filtering.h>
#include <rtabmap/core/util3d_mapping.h>
#include <rtabmap/core/Compression.h>
#include <rtabmap/core/Graph.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UFile.h>
#include <pcl_ros/transforms.h>
#include <pcl_conversions/pcl_conversions.h>
#include <nav_msgs/OccupancyGrid.h>
#include <std_srvs/Empty.h>

using namespace rtabmap;

class MapAssembler
{

public:
	MapAssembler(int & argc, char** argv)
	{
		ros::NodeHandle pnh("~");
		ros::NodeHandle nh;

		std::string configPath;
		pnh.param("config_path", configPath, configPath);

		//parameters
		rtabmap::ParametersMap parameters;
		uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid"));
		uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM"));
		if(!configPath.empty())
		{
			if(UFile::exists(configPath.c_str()))
			{
				ROS_INFO( "%s: Loading parameters from %s", ros::this_node::getName().c_str(), configPath.c_str());
				rtabmap::ParametersMap allParameters;
				Parameters::readINI(configPath.c_str(), allParameters);
				// only update odometry parameters
				for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
				{
					ParametersMap::iterator jter = allParameters.find(iter->first);
					if(jter!=allParameters.end())
					{
						iter->second = jter->second;
					}
				}
			}
			else
			{
				ROS_ERROR( "Config file \"%s\" not found!", configPath.c_str());
			}
		}
		for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
		{
			std::string vStr;
			bool vBool;
			int vInt;
			double vDouble;
			if(pnh.getParam(iter->first, vStr))
			{
				ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), vStr.c_str());
				iter->second = vStr;
			}
			else if(pnh.getParam(iter->first, vBool))
			{
				ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uBool2Str(vBool).c_str());
				iter->second = uBool2Str(vBool);
			}
			else if(pnh.getParam(iter->first, vDouble))
			{
				ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uNumber2Str(vDouble).c_str());
				iter->second = uNumber2Str(vDouble);
			}
			else if(pnh.getParam(iter->first, vInt))
			{
				ROS_INFO( "Setting %s parameter \"%s\"=\"%s\"", ros::this_node::getName().c_str(), iter->first.c_str(), uNumber2Str(vInt).c_str());
				iter->second = uNumber2Str(vInt);
			}

			if(iter->first.compare(Parameters::kVisMinInliers()) == 0 && atoi(iter->second.c_str()) < 8)
			{
				ROS_WARN( "Parameter min_inliers must be >= 8, setting to 8...");
				iter->second = uNumber2Str(8);
			}
		}

		rtabmap::ParametersMap argParameters = rtabmap::Parameters::parseArguments(argc, argv);
		for(rtabmap::ParametersMap::iterator iter=argParameters.begin(); iter!=argParameters.end(); ++iter)
		{
			rtabmap::ParametersMap::iterator jter = parameters.find(iter->first);
			if(jter!=parameters.end())
			{
				ROS_INFO( "Update %s parameter \"%s\"=\"%s\" from arguments", ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.c_str());
				jter->second = iter->second;
			}
		}

		// Backward compatibility
		for(std::map<std::string, std::pair<bool, std::string> >::const_iterator iter=Parameters::getRemovedParameters().begin();
			iter!=Parameters::getRemovedParameters().end();
			++iter)
		{
			std::string vStr;
			if(pnh.getParam(iter->first, vStr))
			{
				if(iter->second.first && parameters.find(iter->second.second) != parameters.end())
				{
					// can be migrated
					parameters.at(iter->second.second)= vStr;
					ROS_WARN( "%s: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.",
							ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.second.c_str(), vStr.c_str());
				}
				else
				{
					if(iter->second.second.empty())
					{
						ROS_ERROR( "%s: Parameter \"%s\" doesn't exist anymore!",
								ros::this_node::getName().c_str(), iter->first.c_str());
					}
					else
					{
						ROS_ERROR( "%s: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"",
								ros::this_node::getName().c_str(), iter->first.c_str(), iter->second.second.c_str());
					}
				}
			}
		}

		mapsManager_.init(nh, pnh, ros::this_node::getName(), false);
		mapsManager_.backwardCompatibilityParameters(pnh, parameters);
		mapsManager_.setParameters(parameters);

		mapDataTopic_ = nh.subscribe("mapData", 1, &MapAssembler::mapDataReceivedCallback, this);

		// private service
		resetService_ = pnh.advertiseService("reset", &MapAssembler::reset, this);
	}

	~MapAssembler()
	{
	}

	void mapDataReceivedCallback(const rtabmap_ros::MapDataConstPtr & msg)
	{
		UTimer timer;

		std::map<int, Transform> poses;
		std::multimap<int, Link> constraints;
		Transform mapOdom;
		rtabmap_ros::mapGraphFromROS(msg->graph, poses, constraints, mapOdom);
		for(unsigned int i=0; i<msg->nodes.size(); ++i)
		{
			if(msg->nodes[i].image.size() ||
			   msg->nodes[i].depth.size() ||
			   msg->nodes[i].laserScan.size())
			{
				uInsert(nodes_, std::make_pair(msg->nodes[i].id, rtabmap_ros::nodeDataFromROS(msg->nodes[i])));
			}
		}

		// create a tmp signature with latest sensory data
		if(poses.size() && nodes_.find(poses.rbegin()->first) != nodes_.end())
		{
			Signature tmpS = nodes_.at(poses.rbegin()->first);
			SensorData tmpData = tmpS.sensorData();
			tmpData.setId(-1);
			uInsert(nodes_, std::make_pair(-1, Signature(-1, -1, 0, tmpS.getStamp(), "", tmpS.getPose(), Transform(), tmpData)));
			poses.insert(std::make_pair(-1, poses.rbegin()->second));
		}

		// Update maps
		poses = mapsManager_.updateMapCaches(
				poses,
				0,
				false,
				false,
				nodes_);

		mapsManager_.publishMaps(poses, msg->header.stamp, msg->header.frame_id);

		ROS_INFO("map_assembler: Publishing data = %fs", timer.ticks());
	}

	bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
	{
		ROS_INFO("map_assembler: reset!");
		mapsManager_.clear();
		return true;
	}

private:
	MapsManager mapsManager_;
	std::map<int, Signature> nodes_;

	ros::Subscriber mapDataTopic_;

	ros::ServiceServer resetService_;
};

int main(int argc, char** argv)
{
	ros::init(argc, argv, "map_assembler");

	// process "--params" argument
	for(int i=1;i<argc;++i)
	{
		if(strcmp(argv[i], "--params") == 0)
		{
			rtabmap::ParametersMap parameters;
			uInsert(parameters, rtabmap::Parameters::getDefaultParameters("Grid"));
			uInsert(parameters, rtabmap::Parameters::getDefaultParameters("StereoBM"));
			for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
			{
				std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
				std::cout <<
						str <<
						std::setw(60 - str.size()) <<
						" [" <<
						rtabmap::Parameters::getDescription(iter->first).c_str() <<
						"]" <<
						std::endl;
			}
			ROS_WARN("Node will now exit after showing default parameters because "
					 "argument \"--params\" is detected!");
			exit(0);
		}
		else if(strcmp(argv[i], "--udebug") == 0)
		{
			ULogger::setLevel(ULogger::kDebug);
		}
		else if(strcmp(argv[i], "--uinfo") == 0)
		{
			ULogger::setLevel(ULogger::kInfo);
		}
	}

	MapAssembler assembler(argc, argv);
	ros::spin();
	return 0;
}
<|endoftext|>


<commit_before>#include "Enemy.h"
#include "SDL.h"
#include "InputHandler.h"

Enemy::Enemy(const LoaderParams* pParams) : SDLGameObject(pParams) { }

void Enemy::draw()
{
	SDLGameObject::draw(); //now we use SDLGameObject
}

//Update function for Enemy object
void Enemy::update()
{
	m_velocity.setX(0);
	m_velocity.setY(0);

	handleInput();	//handles the input from joystick [2nd controller]

	m_currentFrame = int(((SDL_GetTicks() / 100) % 6));

	SDLGameObject::update();
}

void Enemy::clean()
{
	//blank clean() function
}

//Enemy class uses the Second Joystick
void Enemy::handleInput()
{
	//check if a second controller is available and use it for enemy object
	if ((TheInputHandler::Instance()->joysticksInitialised()) && (SDL_NumJoysticks() > 1))
	{
		//for analog sticks of the second joystick
		if ((TheInputHandler::Instance()->xvalue(1, 1) > 0) || (TheInputHandler::Instance()->xvalue(1, 1) < 0))
		{
			//if we are moving the player with analog stick while pressing Y
			//it will get an acceleration of 2 in that direction
			m_velocity.setX((float)1 * TheInputHandler::Instance()->xvalue(1, 1));
			if (TheInputHandler::Instance()->getButtonState(0, 3))	//Button 3(Yellow)
			{
				//give the player some acceleration
				m_acceleration.setX((float)2 * TheInputHandler::Instance()->xvalue(1, 1));
			}
		}

		if ((TheInputHandler::Instance()->yvalue(1, 1) > 0) || (TheInputHandler::Instance()->yvalue(1, 1) < 0))
		{
			m_velocity.setY((float)1 * TheInputHandler::Instance()->yvalue(1, 1));
			if (TheInputHandler::Instance()->getButtonState(1, 3))
			{
				m_acceleration.setY((float)2 * TheInputHandler::Instance()->yvalue(1, 1));
			}
		}

		if ((TheInputHandler::Instance()->xvalue(1, 2) > 0) || (TheInputHandler::Instance()->xvalue(1, 2) < 0))
		{
			m_velocity.setX((float)1 * TheInputHandler::Instance()->xvalue(1, 2));
			if (TheInputHandler::Instance()->getButtonState(1, 3))
			{
				m_acceleration.setX((float)2 * TheInputHandler::Instance()->xvalue(1, 1));
			}
		}

		if ((TheInputHandler::Instance()->yvalue(1, 2) > 0) || (TheInputHandler::Instance()->yvalue(1, 2) < 0))
		{
			m_velocity.setY((float)1 * TheInputHandler::Instance()->yvalue(1, 2));
			if (TheInputHandler::Instance()->getButtonState(1, 3))
			{
				m_acceleration.setY((float)2 * TheInputHandler::Instance()->yvalue(1, 1));
			}
		}
	}

	//no mouse control for enemy

	//keyboard A W S D for movement, Right Shift for acceleration
	if (TheInputHandler::Instance()->isKeyDown(SDL_SCANCODE_D))
	{
		m_velocity.setX(2);
		//check if Right Ctrl is pressed, provide acc. if true
		if (TheInputHandler::Instance()->isKeyDown(SDL_SCANCODE_LSHIFT))
		{
			m_acceleration.setX(2.5);
		}
	}
	if (TheInputHandler::Instance()->isKeyDown(SDL_SCANCODE_A))
	{
		m_velocity.setX(-2);
		//check if Right Ctrl is pressed, provide acc. if true
		if (TheInputHandler::Instance()->isKeyDown(SDL_SCANCODE_LSHIFT))
		{
			m_acceleration.setX(-2.5);
		}
	}
	//remember the cartesian plane is inverted along Y Axis
	if (TheInputHandler::Instance()->isKeyDown(SDL_SCANCODE_W))
	{
		m_velocity.setY(-2);
		//check if Right Ctrl is pressed, provide acc. if true
		if (TheInputHandler::Instance()->isKeyDown(SDL_SCANCODE_LSHIFT))
		{
			m_acceleration.setY(-2.5);
		}
	}
	if (TheInputHandler::Instance()->isKeyDown(SDL_SCANCODE_S))
	{
		m_velocity.setY(2);
		//check if Right Ctrl is pressed, provide acc. if true
		if (TheInputHandler::Instance()->isKeyDown(SDL_SCANCODE_LSHIFT))
		{
			m_acceleration.setY(2.5);
		}
	}
}<commit_msg>(Enemy) Acceleration becomes 0 when L Shift if released<commit_after>#include "Enemy.h"
#include "SDL.h"
#include "InputHandler.h"

Enemy::Enemy(const LoaderParams* pParams) : SDLGameObject(pParams) { }

void Enemy::draw()
{
	SDLGameObject::draw(); //now we use SDLGameObject
}

//Update function for Enemy object
void Enemy::update()
{
	m_velocity.setX(0);
	m_velocity.setY(0);

	m_acceleration.setX(0);
	m_acceleration.setY(0);

	handleInput();	//handles the input from joystick [2nd controller]

	m_currentFrame = int(((SDL_GetTicks() / 100) % 6));

	SDLGameObject::update();
}

void Enemy::clean()
{
	//blank clean() function
}

//Enemy class uses the Second Joystick
void Enemy::handleInput()
{
	//check if a second controller is available and use it for enemy object
	if ((TheInputHandler::Instance()->joysticksInitialised()) && (SDL_NumJoysticks() > 1))
	{
		//for analog sticks of the second joystick
		if ((TheInputHandler::Instance()->xvalue(1, 1) > 0) || (TheInputHandler::Instance()->xvalue(1, 1) < 0))
		{
			//if we are moving the player with analog stick while pressing Y
			//it will get an acceleration of 2 in that direction
			m_velocity.setX((float)1 * TheInputHandler::Instance()->xvalue(1, 1));
			if (TheInputHandler::Instance()->getButtonState(0, 3))	//Button 3(Yellow)
			{
				//give the player some acceleration
				m_acceleration.setX((float)2 * TheInputHandler::Instance()->xvalue(1, 1));
			}
		}

		if ((TheInputHandler::Instance()->yvalue(1, 1) > 0) || (TheInputHandler::Instance()->yvalue(1, 1) < 0))
		{
			m_velocity.setY((float)1 * TheInputHandler::Instance()->yvalue(1, 1));
			if (TheInputHandler::Instance()->getButtonState(1, 3))
			{
				m_acceleration.setY((float)2 * TheInputHandler::Instance()->yvalue(1, 1));
			}
		}

		if ((TheInputHandler::Instance()->xvalue(1, 2) > 0) || (TheInputHandler::Instance()->xvalue(1, 2) < 0))
		{
			m_velocity.setX((float)1 * TheInputHandler::Instance()->xvalue(1, 2));
			if (TheInputHandler::Instance()->getButtonState(1, 3))
			{
				m_acceleration.setX((float)2 * TheInputHandler::Instance()->xvalue(1, 1));
			}
		}

		if ((TheInputHandler::Instance()->yvalue(1, 2) > 0) || (TheInputHandler::Instance()->yvalue(1, 2) < 0))
		{
			m_velocity.setY((float)1 * TheInputHandler::Instance()->yvalue(1, 2));
			if (TheInputHandler::Instance()->getButtonState(1, 3))
			{
				m_acceleration.setY((float)2 * TheInputHandler::Instance()->yvalue(1, 1));
			}
		}
	}

	//no mouse control for enemy

	//keyboard A W S D for movement, Right Shift for acceleration
	if (TheInputHandler::Instance()->isKeyDown(SDL_SCANCODE_D))
	{
		m_velocity.setX(2);
		//check if Right Ctrl is pressed, provide acc. if true
		if (TheInputHandler::Instance()->isKeyDown(SDL_SCANCODE_LSHIFT))
		{
			m_acceleration.setX(2.5);
		}
	}
	if (TheInputHandler::Instance()->isKeyDown(SDL_SCANCODE_A))
	{
		m_velocity.setX(-2);
		//check if Right Ctrl is pressed, provide acc. if true
		if (TheInputHandler::Instance()->isKeyDown(SDL_SCANCODE_LSHIFT))
		{
			m_acceleration.setX(-2.5);
		}
	}
	//remember the cartesian plane is inverted along Y Axis
	if (TheInputHandler::Instance()->isKeyDown(SDL_SCANCODE_W))
	{
		m_velocity.setY(-2);
		//check if Right Ctrl is pressed, provide acc. if true
		if (TheInputHandler::Instance()->isKeyDown(SDL_SCANCODE_LSHIFT))
		{
			m_acceleration.setY(-2.5);
		}
	}
	if (TheInputHandler::Instance()->isKeyDown(SDL_SCANCODE_S))
	{
		m_velocity.setY(2);
		//check if Right Ctrl is pressed, provide acc. if true
		if (TheInputHandler::Instance()->isKeyDown(SDL_SCANCODE_LSHIFT))
		{
			m_acceleration.setY(2.5);
		}
	}
}<|endoftext|>


<commit_before>//Copyright (c) 2018 Ultimaker B.V.
//CuraEngine is released under the terms of the AGPLv3 or higher.

#include "utils/linearAlg2D.h" //Distance from point to line.
#include "MergeInfillLines.h"

namespace cura
{

    MergeInfillLines::MergeInfillLines(ExtruderPlan& plan) : extruder_plan(plan)
    {
        //Just copy the parameters to their fields.
    }

    coord_t MergeInfillLines::calcPathLength(const Point path_start, GCodePath& path) const
    {
        Point previous_point = path_start;
        coord_t result = 0;
        for (const Point point : path.points)
        {
            result += vSize(point - previous_point);
            previous_point = point;
        }
        return result;
    }

    /*
     * first_is_already_merged == false
     *
     *       o     o
     *      /     /
     *     /     /
     *    /  +  /      --->   -(t)-o-----o
     *   /     /
     *  /     /
     * o     o
     *
     * travel (t) to first location is done through first_path_start_changed and new_first_path_start.
     * this gets rid of the tiny "blips". Depending on the merged line distance a small gap may appear, but this is
     * accounted for in the volume.
     *
     * first_is_already_merged == true
     *
     *                   o
     *                  /
     *                 /
     *    o-----o  +  /     --->    o-----------o   or with slight   o-----o-----o
     *   /           /             /                   bend         /
     *  /           /             /                                /
     * o           o             o                                o
     *
     */
    bool MergeInfillLines::mergeLinesSideBySide(const bool first_is_already_merged, GCodePath& first_path, const Point first_path_start, GCodePath& second_path, const Point second_path_start, Point& new_first_path_start) const
    {
        Point average_first_path;

        coord_t first_path_length = calcPathLength(first_path_start, first_path);
        coord_t first_path_length_flow = first_path_length * first_path.flow; //To get the volume we don't need to include the line width since it's the same for both lines.

        if (first_is_already_merged)
        {
            // take second point of path, first_path.points[0]
            average_first_path = first_path.points[0];
        }
        else
        {
            average_first_path += first_path_start;
            for (const Point point : first_path.points)
            {
                average_first_path += point;
            }
            average_first_path /= (first_path.points.size() + 1);
        }

        coord_t second_path_length = calcPathLength(second_path_start, second_path);
        Point average_second_path = second_path_start;
        for (const Point point : second_path.points)
        {
            average_second_path += point;
        }
        coord_t second_path_length_flow = second_path_length *= second_path.flow;
        average_second_path /= (second_path.points.size() + 1);

        // predict new length and flow, returning false doesn't quite work as expected
        // this is to pretend huge blobs, but they don't occur anymore after tweaking

//        coord_t new_length = first_path_length;
//        if (first_is_already_merged)
//        {
//            // check if the new point is a good extension of last part of existing polyline
//            // because of potential accumulation of errors introduced each time a line is merged, we do not allow any error.
//            if (first_path.points.size() > 1 && LinearAlg2D::getDist2FromLine(average_second_path, first_path.points[first_path.points.size() - 2], first_path.points[first_path.points.size() - 1]) == 0)
//            {
//                new_length -= vSize(first_path.points[first_path.points.size() - 2] - first_path.points[first_path.points.size() - 1]);
//                first_path.points[first_path.points.size() - 1] = average_second_path;
//            } else {
//                first_path.points.push_back(average_second_path);
//            }
//            new_length += vSize(first_path.points[first_path.points.size() - 2] - first_path.points[first_path.points.size() - 1]);
//        }
//        else
//        {
//            new_length -= vSize(first_path.points.back() - first_path_start);
//            first_path.points.clear();
//            new_first_path_start = average_first_path;
//            first_path.points.push_back(average_second_path);
//            new_length += vSize(first_path.points.back() - new_first_path_start);
//        }
//        double new_flow = ((first_path_length_flow + second_path_length_flow) / static_cast<double>(new_length));
//        if (new_flow > 3.5)
//        {
//            std::cout << "new flow:" << new_flow << "\n";
//            std::cout << "new length: " << new_length << "\n";
//            std::cout << "length factor: " << (first_path_length + second_path_length) / static_cast<double>(new_length) << "\n";
//            //return false;
//        }

        if (first_is_already_merged)
        {
            // check if the new point is a good extension of last part of existing polyline
            // because of potential accumulation of errors introduced each time a line is merged, we do not allow any error.
            if (first_path.points.size() > 1 && LinearAlg2D::getDist2FromLine(average_second_path, first_path.points[first_path.points.size() - 2], first_path.points[first_path.points.size() - 1]) == 0)
            {
                first_path.points[first_path.points.size() - 1] = average_second_path;
            } else {
                first_path.points.push_back(average_second_path);
            }
        }
        else
        {
            first_path.points.clear();
            new_first_path_start = average_first_path;
            first_path.points.push_back(average_second_path);
        }

        coord_t new_path_length = calcPathLength(first_path_start, first_path);
        first_path.flow = static_cast<double>(first_path_length_flow + second_path_length_flow) / new_path_length;
        std::cout << "new flow:" << first_path.flow << "\n";

        return true;
    }

    bool MergeInfillLines::tryMerge(const bool first_is_already_merged, GCodePath& first_path, const Point first_path_start, GCodePath& second_path, const Point second_path_start, Point& new_first_path_start) const
    {
        const Point first_path_end = first_path.points.back();
        const Point second_path_end = second_path.points.back();
        const coord_t line_width = first_path.config->getLineWidth();

        //Lines may be adjacent side-by-side then.
        Point first_path_leave_point;
        coord_t merged_size2;
        if (first_is_already_merged)
        {
            first_path_leave_point = first_path.points.back();  // this is the point that's going to merge
        } else {
            first_path_leave_point = (first_path_start + first_path_end) / 2;
        }
        const Point second_path_destination_point = (second_path_start + second_path_end) / 2;
        const Point merged_direction = second_path_destination_point - first_path_leave_point;
        if (first_is_already_merged)
        {
            merged_size2 = vSize2(second_path_destination_point - first_path.points.back());  // check distance with last point in merged line that is to be replaced
        }
        else
        {
            merged_size2 = vSize2(merged_direction);
        }
        if (merged_size2 > 25 * line_width * line_width)
        {
            return false; //Lines are too far away from each other.
        }
        if (merged_direction.X == 0 && merged_direction.Y == 0)
        {
            return true;  // we can just disregard the second point as it's exactly at the leave point of the first path.
        }

        // Max 1 line width to the side of the merged_direction
        if (LinearAlg2D::getDist2FromLine(first_path_end, second_path_destination_point, second_path_destination_point + merged_direction) > 4 * line_width * line_width
            || LinearAlg2D::getDist2FromLine(second_path_start, first_path_leave_point, first_path_leave_point + merged_direction) > 4 * line_width * line_width
            || LinearAlg2D::getDist2FromLine(second_path_end,   first_path_leave_point, first_path_leave_point + merged_direction) > 4 * line_width * line_width
            //|| abs(dot(normal(merged_direction, 1000), normal(second_path_end - second_path_start, 1000))) > 866000    // 866000 angle of old second_path with new merged direction should not be too small (30 degrees), as it will introduce holes
            )
        {
            return false; //One of the lines is too far from the merged line. Lines would be too wide or too far off.
        }
        if (first_is_already_merged && first_path.points.size() > 1 && first_path.points[first_path.points.size() - 2] == second_path_destination_point)  // yes this can actually happen
        {
            return false;
        }

        return mergeLinesSideBySide(first_is_already_merged, first_path, first_path_start, second_path, second_path_start, new_first_path_start);
    }

    bool MergeInfillLines::mergeInfillLines(std::vector<GCodePath>& paths, const Point& starting_position) const
    {
        /* Algorithm overview:
            1. Loop over all lines to see if they can be merged.
            1a. Check if two adjacent lines can be merged (skipping travel moves in
                between).
            1b. If they can, merge both lines into the first line.
            1c. If they are merged, check next that the first line can be merged
                with the line after the second line.
            2. Do a second iteration over all paths to remove the tombstones. */
        std::vector<size_t> remove_path_indices;
        std::set<size_t> is_merged;
        std::set<size_t> removed;  // keep track of what we already removed, so don't remove it again

        //For each two adjacent lines, see if they can be merged.
        size_t first_path_index = 0;
        Point first_path_start = Point(starting_position.X, starting_position.Y);  // this one is not going to be overwritten
        size_t second_path_index = 1;

        for (; second_path_index < paths.size(); second_path_index++)
        {
            GCodePath& first_path = paths[first_path_index];
            GCodePath& second_path = paths[second_path_index];
            Point second_path_start = paths[second_path_index - 1].points.back();

            if (second_path.config->isTravelPath())
            {
                continue; //Skip travel paths, we're looking for the first non-travel path.
            }

            bool allow_try_merge = true;
            // see if we meet criteria to merge. should be: travel - path1 not travel - (...) - travel - path2 not travel - travel
            // we're checking the travels here
            if (first_path_index <= 1 || !paths[first_path_index - 1].isTravelPath())  // "<= 1" because we don't want the first travel being changed. That may introduce a hole somewhere
            {
                allow_try_merge = false;
            }
            if (second_path_index + 1 >= paths.size() || !paths[second_path_index + 1].isTravelPath())
            {
                allow_try_merge = false;
            }
            if (first_path.config->isTravelPath()) //Don't merge travel moves.
            {
                allow_try_merge = false;
            }
            if (first_path.config != second_path.config) //Only merge lines that have the same type.
            {
                allow_try_merge = false;
            }
            if (first_path.config->type != PrintFeatureType::Infill && first_path.config->type != PrintFeatureType::Skin) //Only merge skin and infill lines.
            {
                allow_try_merge = false;
            }
            const bool first_is_already_merged = is_merged.find(first_path_index) != is_merged.end();
            if ((!first_is_already_merged && first_path.points.size() > 1) || second_path.points.size() > 1)
            {
                // For now we only merge simple lines, not polylines, to keep it simple.
                // If the first line is already a merged line, then allow it.
                allow_try_merge = false;
            }

            Point new_first_path_start;
            if (allow_try_merge && tryMerge(first_is_already_merged, first_path, first_path_start, second_path, second_path_start, new_first_path_start))
            {
                if (!first_is_already_merged)
                {
                    paths[first_path_index - 1].points.back().X = new_first_path_start.X;
                    paths[first_path_index - 1].points.back().Y = new_first_path_start.Y;
                }
                /* If we combine two lines, the next path may also be merged into the fist line, so we do NOT update
                first_path_index. */
                for (size_t to_delete_index = first_path_index + 1; to_delete_index <= second_path_index; to_delete_index++)
                {
                    if (removed.find(to_delete_index) == removed.end())  // if there are line(s) between first and second, then those lines are already marked as to be deleted, only add the new line(s)
                    {
                        remove_path_indices.push_back(to_delete_index);
                        removed.insert(to_delete_index);
                    }
                }
                is_merged.insert(first_path_index);
            }
            else
            {
                /* If we do not combine, the next iteration we must simply merge the
                second path with the line after it. */
                first_path_index = second_path_index;
                first_path_start = second_path_start;
            }
        }

        //Delete all removed lines in one pass so that we need to move lines less often.
        if (!remove_path_indices.empty())
        {
            size_t path_index = remove_path_indices[0];
            for (size_t removed_position = 1; removed_position < remove_path_indices.size(); removed_position++)
            {
                for (; path_index < remove_path_indices[removed_position] - removed_position; path_index++)
                {
                    paths[path_index] = paths[path_index + removed_position]; //Shift all paths.
                }
            }
            for (; path_index < paths.size() - remove_path_indices.size(); path_index++) //Remaining shifts at the end.
            {
                paths[path_index] = paths[path_index + remove_path_indices.size()];
            }
            paths.erase(paths.begin() + path_index, paths.end());
            return true;
        }
        else
        {
            return false;
        }
    }
}//namespace cura
<commit_msg>Remove debug print. CURA-5535<commit_after>//Copyright (c) 2018 Ultimaker B.V.
//CuraEngine is released under the terms of the AGPLv3 or higher.

#include "utils/linearAlg2D.h" //Distance from point to line.
#include "MergeInfillLines.h"

namespace cura
{

    MergeInfillLines::MergeInfillLines(ExtruderPlan& plan) : extruder_plan(plan)
    {
        //Just copy the parameters to their fields.
    }

    coord_t MergeInfillLines::calcPathLength(const Point path_start, GCodePath& path) const
    {
        Point previous_point = path_start;
        coord_t result = 0;
        for (const Point point : path.points)
        {
            result += vSize(point - previous_point);
            previous_point = point;
        }
        return result;
    }

    /*
     * first_is_already_merged == false
     *
     *       o     o
     *      /     /
     *     /     /
     *    /  +  /      --->   -(t)-o-----o
     *   /     /
     *  /     /
     * o     o
     *
     * travel (t) to first location is done through first_path_start_changed and new_first_path_start.
     * this gets rid of the tiny "blips". Depending on the merged line distance a small gap may appear, but this is
     * accounted for in the volume.
     *
     * first_is_already_merged == true
     *
     *                   o
     *                  /
     *                 /
     *    o-----o  +  /     --->    o-----------o   or with slight   o-----o-----o
     *   /           /             /                   bend         /
     *  /           /             /                                /
     * o           o             o                                o
     *
     */
    bool MergeInfillLines::mergeLinesSideBySide(const bool first_is_already_merged, GCodePath& first_path, const Point first_path_start, GCodePath& second_path, const Point second_path_start, Point& new_first_path_start) const
    {
        Point average_first_path;

        coord_t first_path_length = calcPathLength(first_path_start, first_path);
        coord_t first_path_length_flow = first_path_length * first_path.flow; //To get the volume we don't need to include the line width since it's the same for both lines.

        if (first_is_already_merged)
        {
            // take second point of path, first_path.points[0]
            average_first_path = first_path.points[0];
        }
        else
        {
            average_first_path += first_path_start;
            for (const Point point : first_path.points)
            {
                average_first_path += point;
            }
            average_first_path /= (first_path.points.size() + 1);
        }

        coord_t second_path_length = calcPathLength(second_path_start, second_path);
        Point average_second_path = second_path_start;
        for (const Point point : second_path.points)
        {
            average_second_path += point;
        }
        coord_t second_path_length_flow = second_path_length *= second_path.flow;
        average_second_path /= (second_path.points.size() + 1);

        // predict new length and flow, returning false doesn't quite work as expected
        // this is to pretend huge blobs, but they don't occur anymore after tweaking

//        coord_t new_length = first_path_length;
//        if (first_is_already_merged)
//        {
//            // check if the new point is a good extension of last part of existing polyline
//            // because of potential accumulation of errors introduced each time a line is merged, we do not allow any error.
//            if (first_path.points.size() > 1 && LinearAlg2D::getDist2FromLine(average_second_path, first_path.points[first_path.points.size() - 2], first_path.points[first_path.points.size() - 1]) == 0)
//            {
//                new_length -= vSize(first_path.points[first_path.points.size() - 2] - first_path.points[first_path.points.size() - 1]);
//                first_path.points[first_path.points.size() - 1] = average_second_path;
//            } else {
//                first_path.points.push_back(average_second_path);
//            }
//            new_length += vSize(first_path.points[first_path.points.size() - 2] - first_path.points[first_path.points.size() - 1]);
//        }
//        else
//        {
//            new_length -= vSize(first_path.points.back() - first_path_start);
//            first_path.points.clear();
//            new_first_path_start = average_first_path;
//            first_path.points.push_back(average_second_path);
//            new_length += vSize(first_path.points.back() - new_first_path_start);
//        }
//        double new_flow = ((first_path_length_flow + second_path_length_flow) / static_cast<double>(new_length));
//        if (new_flow > 3.5)
//        {
//            std::cout << "new flow:" << new_flow << "\n";
//            std::cout << "new length: " << new_length << "\n";
//            std::cout << "length factor: " << (first_path_length + second_path_length) / static_cast<double>(new_length) << "\n";
//            //return false;
//        }

        if (first_is_already_merged)
        {
            // check if the new point is a good extension of last part of existing polyline
            // because of potential accumulation of errors introduced each time a line is merged, we do not allow any error.
            if (first_path.points.size() > 1 && LinearAlg2D::getDist2FromLine(average_second_path, first_path.points[first_path.points.size() - 2], first_path.points[first_path.points.size() - 1]) == 0)
            {
                first_path.points[first_path.points.size() - 1] = average_second_path;
            } else {
                first_path.points.push_back(average_second_path);
            }
        }
        else
        {
            first_path.points.clear();
            new_first_path_start = average_first_path;
            first_path.points.push_back(average_second_path);
        }

        coord_t new_path_length = calcPathLength(first_path_start, first_path);
        first_path.flow = static_cast<double>(first_path_length_flow + second_path_length_flow) / new_path_length;

        return true;
    }

    bool MergeInfillLines::tryMerge(const bool first_is_already_merged, GCodePath& first_path, const Point first_path_start, GCodePath& second_path, const Point second_path_start, Point& new_first_path_start) const
    {
        const Point first_path_end = first_path.points.back();
        const Point second_path_end = second_path.points.back();
        const coord_t line_width = first_path.config->getLineWidth();

        //Lines may be adjacent side-by-side then.
        Point first_path_leave_point;
        coord_t merged_size2;
        if (first_is_already_merged)
        {
            first_path_leave_point = first_path.points.back();  // this is the point that's going to merge
        } else {
            first_path_leave_point = (first_path_start + first_path_end) / 2;
        }
        const Point second_path_destination_point = (second_path_start + second_path_end) / 2;
        const Point merged_direction = second_path_destination_point - first_path_leave_point;
        if (first_is_already_merged)
        {
            merged_size2 = vSize2(second_path_destination_point - first_path.points.back());  // check distance with last point in merged line that is to be replaced
        }
        else
        {
            merged_size2 = vSize2(merged_direction);
        }
        if (merged_size2 > 25 * line_width * line_width)
        {
            return false; //Lines are too far away from each other.
        }
        if (merged_direction.X == 0 && merged_direction.Y == 0)
        {
            return true;  // we can just disregard the second point as it's exactly at the leave point of the first path.
        }

        // Max 1 line width to the side of the merged_direction
        if (LinearAlg2D::getDist2FromLine(first_path_end, second_path_destination_point, second_path_destination_point + merged_direction) > 4 * line_width * line_width
            || LinearAlg2D::getDist2FromLine(second_path_start, first_path_leave_point, first_path_leave_point + merged_direction) > 4 * line_width * line_width
            || LinearAlg2D::getDist2FromLine(second_path_end,   first_path_leave_point, first_path_leave_point + merged_direction) > 4 * line_width * line_width
            //|| abs(dot(normal(merged_direction, 1000), normal(second_path_end - second_path_start, 1000))) > 866000    // 866000 angle of old second_path with new merged direction should not be too small (30 degrees), as it will introduce holes
            )
        {
            return false; //One of the lines is too far from the merged line. Lines would be too wide or too far off.
        }
        if (first_is_already_merged && first_path.points.size() > 1 && first_path.points[first_path.points.size() - 2] == second_path_destination_point)  // yes this can actually happen
        {
            return false;
        }

        return mergeLinesSideBySide(first_is_already_merged, first_path, first_path_start, second_path, second_path_start, new_first_path_start);
    }

    bool MergeInfillLines::mergeInfillLines(std::vector<GCodePath>& paths, const Point& starting_position) const
    {
        /* Algorithm overview:
            1. Loop over all lines to see if they can be merged.
            1a. Check if two adjacent lines can be merged (skipping travel moves in
                between).
            1b. If they can, merge both lines into the first line.
            1c. If they are merged, check next that the first line can be merged
                with the line after the second line.
            2. Do a second iteration over all paths to remove the tombstones. */
        std::vector<size_t> remove_path_indices;
        std::set<size_t> is_merged;
        std::set<size_t> removed;  // keep track of what we already removed, so don't remove it again

        //For each two adjacent lines, see if they can be merged.
        size_t first_path_index = 0;
        Point first_path_start = Point(starting_position.X, starting_position.Y);  // this one is not going to be overwritten
        size_t second_path_index = 1;

        for (; second_path_index < paths.size(); second_path_index++)
        {
            GCodePath& first_path = paths[first_path_index];
            GCodePath& second_path = paths[second_path_index];
            Point second_path_start = paths[second_path_index - 1].points.back();

            if (second_path.config->isTravelPath())
            {
                continue; //Skip travel paths, we're looking for the first non-travel path.
            }

            bool allow_try_merge = true;
            // see if we meet criteria to merge. should be: travel - path1 not travel - (...) - travel - path2 not travel - travel
            // we're checking the travels here
            if (first_path_index <= 1 || !paths[first_path_index - 1].isTravelPath())  // "<= 1" because we don't want the first travel being changed. That may introduce a hole somewhere
            {
                allow_try_merge = false;
            }
            if (second_path_index + 1 >= paths.size() || !paths[second_path_index + 1].isTravelPath())
            {
                allow_try_merge = false;
            }
            if (first_path.config->isTravelPath()) //Don't merge travel moves.
            {
                allow_try_merge = false;
            }
            if (first_path.config != second_path.config) //Only merge lines that have the same type.
            {
                allow_try_merge = false;
            }
            if (first_path.config->type != PrintFeatureType::Infill && first_path.config->type != PrintFeatureType::Skin) //Only merge skin and infill lines.
            {
                allow_try_merge = false;
            }
            const bool first_is_already_merged = is_merged.find(first_path_index) != is_merged.end();
            if ((!first_is_already_merged && first_path.points.size() > 1) || second_path.points.size() > 1)
            {
                // For now we only merge simple lines, not polylines, to keep it simple.
                // If the first line is already a merged line, then allow it.
                allow_try_merge = false;
            }

            Point new_first_path_start;
            if (allow_try_merge && tryMerge(first_is_already_merged, first_path, first_path_start, second_path, second_path_start, new_first_path_start))
            {
                if (!first_is_already_merged)
                {
                    paths[first_path_index - 1].points.back().X = new_first_path_start.X;
                    paths[first_path_index - 1].points.back().Y = new_first_path_start.Y;
                }
                /* If we combine two lines, the next path may also be merged into the fist line, so we do NOT update
                first_path_index. */
                for (size_t to_delete_index = first_path_index + 1; to_delete_index <= second_path_index; to_delete_index++)
                {
                    if (removed.find(to_delete_index) == removed.end())  // if there are line(s) between first and second, then those lines are already marked as to be deleted, only add the new line(s)
                    {
                        remove_path_indices.push_back(to_delete_index);
                        removed.insert(to_delete_index);
                    }
                }
                is_merged.insert(first_path_index);
            }
            else
            {
                /* If we do not combine, the next iteration we must simply merge the
                second path with the line after it. */
                first_path_index = second_path_index;
                first_path_start = second_path_start;
            }
        }

        //Delete all removed lines in one pass so that we need to move lines less often.
        if (!remove_path_indices.empty())
        {
            size_t path_index = remove_path_indices[0];
            for (size_t removed_position = 1; removed_position < remove_path_indices.size(); removed_position++)
            {
                for (; path_index < remove_path_indices[removed_position] - removed_position; path_index++)
                {
                    paths[path_index] = paths[path_index + removed_position]; //Shift all paths.
                }
            }
            for (; path_index < paths.size() - remove_path_indices.size(); path_index++) //Remaining shifts at the end.
            {
                paths[path_index] = paths[path_index + remove_path_indices.size()];
            }
            paths.erase(paths.begin() + path_index, paths.end());
            return true;
        }
        else
        {
            return false;
        }
    }
}//namespace cura
<|endoftext|>


<commit_before>#include "../ht.h"
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
//#include <memory.h>
#include <string.h>
#include <time.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <dirent.h>

namespace wi {

class FileStream : public Stream
{
public:
	FileStream();
	~FileStream();
	bool Init(char *pszMode, char *pszFile, char *pszDelete, char *pszFinal);

	virtual void Close();
	virtual dword Read(void *pv, dword cb);
	virtual dword Write(void *pv, dword cb);
	virtual bool IsSuccess();

private:
	bool m_fSuccess;
	FILE *m_pf;
	char m_szFile[PATH_MAX];
	char m_szDelete[PATH_MAX];
	char m_szFinal[PATH_MAX];
};

FileStream::FileStream()
{
	m_fSuccess = true;
	m_pf = NULL;
	m_szFile[0] = 0;
	m_szDelete[0] = 0;
	m_szFinal[0] = 0;
}

FileStream::~FileStream()
{
	Assert(m_pf == NULL);
}

bool FileStream::Init(char *pszMode, char *pszFile, char *pszDelete, char *pszFinal)
{
	m_pf = fopen(pszFile, pszMode);
	if (m_pf == NULL)
		return false;
	strcpy(m_szFile, pszFile);
	if (pszDelete != NULL)
		strcpy(m_szDelete, pszDelete);
	if (pszFinal != NULL)
		strcpy(m_szFinal, pszFinal);
	return true;
}

void FileStream::Close()
{
	fclose(m_pf);
	m_pf = NULL;

	// Delete and rename file if no error

	if (m_fSuccess) {
		if (m_szDelete[0] != 0) {
            unlink(m_szDelete);
		}
		if (m_szFinal[0] != 0) {
            rename(m_szFile, m_szFinal);
		}
	}
}

dword FileStream::Read(void *pv, dword cb)
{
	size_t cbT = fread(pv, 1, cb, m_pf);
	if (cb != cbT)
		m_fSuccess = false;
	return cbT;
}

dword FileStream::Write(void *pv, dword cb)
{
	size_t cbT = fwrite(pv, 1, cb, m_pf);
	if (cb != cbT)
		m_fSuccess = false;
	return cbT;
}

bool FileStream::IsSuccess()
{
	return m_fSuccess;
}

void PrependSavesDirectory(char *pszIn, char *pszOut)
{
	strcpy(pszOut, IPhone::GetSaveGamesDir());
	strcat(pszOut, "/");
	strcat(pszOut, pszIn);
}

bool FindSaveGame(int nGame, char *psz, int cb, int *pc = NULL)
{
	if (pc != NULL)
		*pc = 0;

    // This is the prefix of the file being looked for

	char szCompare[PATH_MAX];
	sprintf(szCompare, "htsave%d_", nGame);
	int cchCompare = strlen(szCompare);

    // This is the special save game that is only used
    // when the game exits and reloads right away

	char szReinitializeSave[20];
	sprintf(szReinitializeSave, "htsave%d_", knGameReinitializeSave);
	int cchReinitializeSave = strlen(szReinitializeSave);

    // Enum files in this directory

	char szFileSpec[PATH_MAX];
	PrependSavesDirectory("", szFileSpec);
    DIR *pdir = opendir(szFileSpec);
    if (pdir == NULL) {
        return false;
    }

    int c = 0;
    dirent *pdent;
    while ((pdent = readdir(pdir)) != NULL) {
        // Only consider save games, because if the desired save game is
        // not found, we need to count "slots".

        if (strncmp("htsave", pdent->d_name, 6) != 0) {
            continue;
        }

        // Save game found?

        if (strncmp(szCompare, pdent->d_name, cchCompare) == 0) {
			if (psz != NULL)
				strncpyz(psz, pdent->d_name, cb);
			closedir(pdir);
			return true;
		}

        // Count save games but don't count the temporary "Reinitialize"
        // saved game

		if (strncmp(szReinitializeSave, pdent->d_name,
                cchReinitializeSave) == 0) {
			continue;
        }

        // Count this save game as a slot

		c++;
    }
    closedir(pdir);

    // Didn't find the saved game, but did count the number of occupied slots

	if (pc != NULL)
		*pc = c;
	return false;
}

int HostGetSaveGameCount()
{
	int c;
	FindSaveGame(-1, NULL, 0, &c);
	return c;
}

bool HostGetSaveGameName(int nGame, char *psz, int cb, Date *pdate, int *pnHours24, int *pnMinutes, int *pnSeconds)
{
	// Find the game

	char szT[PATH_MAX];
	if (!FindSaveGame(nGame, szT, sizeof(szT))) {
		strncpyz(psz, "-- Empty --", cb);
		return false;
	}

	char szPath[PATH_MAX];
	PrependSavesDirectory(szT, szPath);

	// Get the creation time in hours24, minutes

    struct stat st;
    if (stat(szPath, &st) > 0) {
        return false;
    }
    time_t tim = 0;
    struct tm *ptm = localtime(&tim);
	*pnHours24 = ptm->tm_hour;
	*pnMinutes = ptm->tm_min;
	*pnSeconds = ptm->tm_sec;
	pdate->nDay = ptm->tm_mday;
	pdate->nMonth = ptm->tm_mon;
	pdate->nYear = ptm->tm_year + 1900;

	// Copy over filename, lose prefix

	char *pszName = strchr(szT, '_') + 1;
	int cbName = strlen(pszName) - 4 + 1;
	strncpyz(psz, pszName, _min(cb, cbName));

	// restore '#' to ':'

	char *pchInvalid = psz;
	do {
		pchInvalid = strchr(pchInvalid, '#');
		if (pchInvalid != 0)
			*pchInvalid = ':';
	} while (pchInvalid != 0);

	return true;
}

Stream *HostNewSaveGameStream(int nGame, char *pszName)
{
	// Get the old file name - we'll delete this if successful

	char szOld[PATH_MAX];
	char szOldFull[PATH_MAX];
	if (!FindSaveGame(nGame, szOld, sizeof(szOld))) {
		szOldFull[0] = 0;
	} else {
		PrependSavesDirectory(szOld, szOldFull);
	}

	// New file name

	char szNew[PATH_MAX];
	sprintf(szNew, "htsave%d_%s.bin", nGame, pszName);

	// windows disallows ':' in a filename, so sub those out

	char *pchInvalid = szNew;
	do {
		pchInvalid = strchr(pchInvalid, ':');
		if (pchInvalid != 0)
			*pchInvalid = '#';
	} while (pchInvalid != 0);

	char szNewFull[PATH_MAX];
	PrependSavesDirectory(szNew, szNewFull);

	// Get stream over temp file

	FileStream *pstm = new FileStream();
	if (pstm == NULL)
		return NULL;

	// If save is successful, szOld will be deleted and httempsave.bin
	// will be renamed to szNew

	char szTempSaveFull[PATH_MAX];
	PrependSavesDirectory("httempsave.bin", szTempSaveFull);

	if (!pstm->Init("wb", szTempSaveFull, szOldFull, szNewFull)) {
		delete pstm;
		return NULL;
	}

	return (Stream *)pstm;
}

Stream *HostOpenSaveGameStream(int nGame, bool fDelete)
{
	char szT[PATH_MAX];
	if (!FindSaveGame(nGame, szT, sizeof(szT)))
		return NULL;

	// rename to a temporary file before opening

	char szTFull[PATH_MAX];
	PrependSavesDirectory(szT, szTFull);
	char szTempNameFull[PATH_MAX];
	PrependSavesDirectory(kszTempName, szTempNameFull);
    rename(szTFull, szTempNameFull);

	// Get stream over temp file

	FileStream *pstm = new FileStream();
	if (pstm == NULL)
		return NULL;

	// If load is successful, and fDelete is True  szTempName will be deleted
	// if fDelete is false szTempName will be renamed to szSaveGame

	if (!pstm->Init("rb", szTempNameFull, fDelete ? szTempNameFull : NULL, fDelete ? NULL : szTFull)) {
		delete pstm;
		return NULL;
	}

	return (Stream *)pstm;
}

bool HostDeleteSaveGame(char *psz, int nGame)
{
	// if nGame is > 0, delete that, otherwise if psz is non-null delete that
	// return true if we deleted something

	char szSaveGame[PATH_MAX];

	if (psz == NULL) {
		if (!FindSaveGame(nGame, szSaveGame, sizeof(szSaveGame)))
			return false;
	} else {
		strncpyz(szSaveGame, psz, sizeof(szSaveGame));
	}

	char szSaveGameFull[PATH_MAX];
	PrependSavesDirectory(szSaveGame, szSaveGameFull);

	if (psz != NULL) {
        unlink(szSaveGameFull);
		return true;
	}
	return false;
}

} // namespace wi
<commit_msg>Use the file modified time, not 0. This change requires default alignment.<commit_after>#include "../ht.h"
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
//#include <memory.h>
#include <string.h>
#include <time.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <dirent.h>

namespace wi {

class FileStream : public Stream
{
public:
	FileStream();
	~FileStream();
	bool Init(char *pszMode, char *pszFile, char *pszDelete, char *pszFinal);

	virtual void Close();
	virtual dword Read(void *pv, dword cb);
	virtual dword Write(void *pv, dword cb);
	virtual bool IsSuccess();

private:
	bool m_fSuccess;
	FILE *m_pf;
	char m_szFile[PATH_MAX];
	char m_szDelete[PATH_MAX];
	char m_szFinal[PATH_MAX];
};

FileStream::FileStream()
{
	m_fSuccess = true;
	m_pf = NULL;
	m_szFile[0] = 0;
	m_szDelete[0] = 0;
	m_szFinal[0] = 0;
}

FileStream::~FileStream()
{
	Assert(m_pf == NULL);
}

bool FileStream::Init(char *pszMode, char *pszFile, char *pszDelete, char *pszFinal)
{
	m_pf = fopen(pszFile, pszMode);
	if (m_pf == NULL)
		return false;
	strcpy(m_szFile, pszFile);
	if (pszDelete != NULL)
		strcpy(m_szDelete, pszDelete);
	if (pszFinal != NULL)
		strcpy(m_szFinal, pszFinal);
	return true;
}

void FileStream::Close()
{
	fclose(m_pf);
	m_pf = NULL;

	// Delete and rename file if no error

	if (m_fSuccess) {
		if (m_szDelete[0] != 0) {
            unlink(m_szDelete);
		}
		if (m_szFinal[0] != 0) {
            rename(m_szFile, m_szFinal);
		}
	}
}

dword FileStream::Read(void *pv, dword cb)
{
	size_t cbT = fread(pv, 1, cb, m_pf);
	if (cb != cbT)
		m_fSuccess = false;
	return cbT;
}

dword FileStream::Write(void *pv, dword cb)
{
	size_t cbT = fwrite(pv, 1, cb, m_pf);
	if (cb != cbT)
		m_fSuccess = false;
	return cbT;
}

bool FileStream::IsSuccess()
{
	return m_fSuccess;
}

void PrependSavesDirectory(char *pszIn, char *pszOut)
{
	strcpy(pszOut, IPhone::GetSaveGamesDir());
	strcat(pszOut, "/");
	strcat(pszOut, pszIn);
}

bool FindSaveGame(int nGame, char *psz, int cb, int *pc = NULL)
{
	if (pc != NULL)
		*pc = 0;

    // This is the prefix of the file being looked for

	char szCompare[PATH_MAX];
	sprintf(szCompare, "htsave%d_", nGame);
	int cchCompare = strlen(szCompare);

    // This is the special save game that is only used
    // when the game exits and reloads right away

	char szReinitializeSave[20];
	sprintf(szReinitializeSave, "htsave%d_", knGameReinitializeSave);
	int cchReinitializeSave = strlen(szReinitializeSave);

    // Enum files in this directory

	char szFileSpec[PATH_MAX];
	PrependSavesDirectory("", szFileSpec);
    DIR *pdir = opendir(szFileSpec);
    if (pdir == NULL) {
        return false;
    }

    int c = 0;
    dirent *pdent;
    while ((pdent = readdir(pdir)) != NULL) {
        // Only consider save games, because if the desired save game is
        // not found, we need to count "slots".

        if (strncmp("htsave", pdent->d_name, 6) != 0) {
            continue;
        }

        // Save game found?

        if (strncmp(szCompare, pdent->d_name, cchCompare) == 0) {
			if (psz != NULL)
				strncpyz(psz, pdent->d_name, cb);
			closedir(pdir);
			return true;
		}

        // Count save games but don't count the temporary "Reinitialize"
        // saved game

		if (strncmp(szReinitializeSave, pdent->d_name,
                cchReinitializeSave) == 0) {
			continue;
        }

        // Count this save game as a slot

		c++;
    }
    closedir(pdir);

    // Didn't find the saved game, but did count the number of occupied slots

	if (pc != NULL)
		*pc = c;
	return false;
}

int HostGetSaveGameCount()
{
	int c;
	FindSaveGame(-1, NULL, 0, &c);
	return c;
}

bool HostGetSaveGameName(int nGame, char *psz, int cb, Date *pdate, int *pnHours24, int *pnMinutes, int *pnSeconds)
{
	// Find the game

	char szT[PATH_MAX];
	if (!FindSaveGame(nGame, szT, sizeof(szT))) {
		strncpyz(psz, "-- Empty --", cb);
		return false;
	}

	char szPath[PATH_MAX];
	PrependSavesDirectory(szT, szPath);

	// Get the creation time in hours24, minutes

    struct stat st;
    if (stat(szPath, &st) > 0) {
        return false;
    }
    time_t tim = st.st_mtimespec.tv_sec;
    struct tm *ptm = localtime(&tim);
	*pnHours24 = ptm->tm_hour;
	*pnMinutes = ptm->tm_min;
	*pnSeconds = ptm->tm_sec;
	pdate->nDay = ptm->tm_mday;
	pdate->nMonth = ptm->tm_mon;
	pdate->nYear = ptm->tm_year + 1900;

	// Copy over filename, lose prefix

	char *pszName = strchr(szT, '_') + 1;
	int cbName = strlen(pszName) - 4 + 1;
	strncpyz(psz, pszName, _min(cb, cbName));

	// restore '#' to ':'

	char *pchInvalid = psz;
	do {
		pchInvalid = strchr(pchInvalid, '#');
		if (pchInvalid != 0)
			*pchInvalid = ':';
	} while (pchInvalid != 0);

	return true;
}

Stream *HostNewSaveGameStream(int nGame, char *pszName)
{
	// Get the old file name - we'll delete this if successful

	char szOld[PATH_MAX];
	char szOldFull[PATH_MAX];
	if (!FindSaveGame(nGame, szOld, sizeof(szOld))) {
		szOldFull[0] = 0;
	} else {
		PrependSavesDirectory(szOld, szOldFull);
	}

	// New file name

	char szNew[PATH_MAX];
	sprintf(szNew, "htsave%d_%s.bin", nGame, pszName);

	// windows disallows ':' in a filename, so sub those out

	char *pchInvalid = szNew;
	do {
		pchInvalid = strchr(pchInvalid, ':');
		if (pchInvalid != 0)
			*pchInvalid = '#';
	} while (pchInvalid != 0);

	char szNewFull[PATH_MAX];
	PrependSavesDirectory(szNew, szNewFull);

	// Get stream over temp file

	FileStream *pstm = new FileStream();
	if (pstm == NULL)
		return NULL;

	// If save is successful, szOld will be deleted and httempsave.bin
	// will be renamed to szNew

	char szTempSaveFull[PATH_MAX];
	PrependSavesDirectory("httempsave.bin", szTempSaveFull);

	if (!pstm->Init("wb", szTempSaveFull, szOldFull, szNewFull)) {
		delete pstm;
		return NULL;
	}

	return (Stream *)pstm;
}

Stream *HostOpenSaveGameStream(int nGame, bool fDelete)
{
	char szT[PATH_MAX];
	if (!FindSaveGame(nGame, szT, sizeof(szT)))
		return NULL;

	// rename to a temporary file before opening

	char szTFull[PATH_MAX];
	PrependSavesDirectory(szT, szTFull);
	char szTempNameFull[PATH_MAX];
	PrependSavesDirectory(kszTempName, szTempNameFull);
    rename(szTFull, szTempNameFull);

	// Get stream over temp file

	FileStream *pstm = new FileStream();
	if (pstm == NULL)
		return NULL;

	// If load is successful, and fDelete is True  szTempName will be deleted
	// if fDelete is false szTempName will be renamed to szSaveGame

	if (!pstm->Init("rb", szTempNameFull, fDelete ? szTempNameFull : NULL, fDelete ? NULL : szTFull)) {
		delete pstm;
		return NULL;
	}

	return (Stream *)pstm;
}

bool HostDeleteSaveGame(char *psz, int nGame)
{
	// if nGame is > 0, delete that, otherwise if psz is non-null delete that
	// return true if we deleted something

	char szSaveGame[PATH_MAX];

	if (psz == NULL) {
		if (!FindSaveGame(nGame, szSaveGame, sizeof(szSaveGame)))
			return false;
	} else {
		strncpyz(szSaveGame, psz, sizeof(szSaveGame));
	}

	char szSaveGameFull[PATH_MAX];
	PrependSavesDirectory(szSaveGame, szSaveGameFull);

	if (psz != NULL) {
        unlink(szSaveGameFull);
		return true;
	}
	return false;
}

} // namespace wi
<|endoftext|>


<commit_before>// Copyright 2017 Yahoo Holdings. Licensed under the terms of the Apache 2.0 license. See LICENSE in the project root.

#include "fdispatch.h"

#include <vespa/searchcore/util/log.h>
#include <vespa/searchcore/util/eventloop.h>
#include <vespa/searchcore/fdispatch/search/querycacheutil.h>
#include <vespa/searchcore/fdispatch/search/nodemanager.h>
#include <vespa/vespalib/util/exceptions.h>
#include <vespa/config/helper/configgetter.hpp>
#include "engineadapter.h"
#include "rpc.h"
#include <thread>

#include <vespa/log/log.h>
LOG_SETUP(".fdispatch");

#ifndef V_TAG
#define V_TAG "NOTAG"
#endif

using search::fs4transport::FS4PersistentPacketStreamer;
using vespa::config::search::core::FdispatchrcConfig;
using vespa::config::search::core::internal::InternalFdispatchrcType;
using document::CompressionConfig;

char FastS_VersionTag[] = V_TAG;

namespace fdispatch
{

FastS_FNETAdapter::FastS_FNETAdapter(FastS_AppContext *appCtx)
    : _appCtx(appCtx),
      _nodeManager(),
      _timeKeeper(NULL),
      _transport(NULL),
      _last_now(0.0),
      _live_counter(0),
      _task()
{ }

FastS_FNETAdapter::~FastS_FNETAdapter()
{
    fini();
}

void
FastS_FNETAdapter::init()
{
    _nodeManager = _appCtx->GetNodeManager();
    _timeKeeper  = _appCtx->GetTimeKeeper();
    _transport   = _appCtx->GetFNETTransport();
    _last_now    = _timeKeeper->GetTime();
    _task.reset(new MyTask(_transport->GetScheduler(), *this));
    _task->ScheduleNow();
}

void
FastS_FNETAdapter::perform()
{
    double now = _timeKeeper->GetTime();
    double delta = now - _last_now;
    if (delta >= 3.0) {
        LOG(warning, "FNET loop high latency: %.3f", delta);
    }
    _last_now = now;
    ++_live_counter;
    _nodeManager->CheckEvents(_timeKeeper);
}

void
FastS_FNETAdapter::fini()
{
    if (_task) {
        _task->Kill();
        _task.reset();
    }
}

Fdispatch::~Fdispatch()
{
    if (_transportServer) {
        _transportServer->shutDown(); // sync shutdown
    }
    _FNET_adapter.fini();
    if (_nodeManager) {
        _nodeManager->ShutdownConfig();
    }
    if (_transport && _transportStarted) {
        _transport->ShutDown(true); // sync shutdown
    }
    if (_rpc) {
        _rpc->ShutDown(); // sync shutdown
    }

    LOG(debug, "Will close threadpool");
    _mypool->Close();
    LOG(debug, "Has closed threadpool");
    _transportServer.reset();
    _engineAdapter.reset();
    _nodeManager.reset();
    _transport.reset();
    _rpc.reset();
    _mypool.reset();
}

FNET_Transport *
Fdispatch::GetFNETTransport()
{
    return _transport.get();
}

FNET_Scheduler *
Fdispatch::GetFNETScheduler()
{
    return (_transport) ? _transport->GetScheduler() : nullptr;
}

FastS_NodeManager *
Fdispatch::GetNodeManager()
{
    return _nodeManager.get();
}

FastS_DataSetCollection *
Fdispatch::GetDataSetCollection()
{
    return ( _nodeManager) ? _nodeManager->GetDataSetCollection() : nullptr;
}

FastOS_ThreadPool *
Fdispatch::GetThreadPool()
{
    return _mypool.get();
}

bool
Fdispatch::Failed()
{
    return ( (_transportServer && _transportServer->isFailed())) || _needRestart;
}

bool
Fdispatch::CheckTempFail()
{
    bool ret;
    bool failflag = _nodeManager->GetTempFail();
    unsigned int FNETLiveCounter;

    ret = true;

    FNETLiveCounter = _FNET_adapter.GetLiveCounter();
    if (FNETLiveCounter == _lastFNETLiveCounter) {
        if (_FNETLiveCounterFailed) {
            failflag = true;				// Still failure
        } else if (!_FNETLiveCounterDanger) {
            _FNETLiveCounterDanger = true;
            _FNETLiveCounterDangerStart.SetNow();
        } else if (_FNETLiveCounterDangerStart.MilliSecsToNow() >= 6000) {
            LOG(error, "fdispatch::Fdispatch::CheckTempFail: FNET inactive for 6 seconds, deadlock ?");
            _FNETLiveCounterFailed = true;		// Note that we failed
            failflag = true;				// Force temporary failure
        } else if (_FNETLiveCounterDangerStart.MilliSecsToNow() >= 3000 &&
                   !_FNETLiveCounterWarned) {
            _FNETLiveCounterWarned = true;
            LOG(warning, "fdispatch::Fdispatch::CheckTempFail: FNET inactive for 3 seconds");
        }
    } else {
        if (_FNETLiveCounterFailed || _FNETLiveCounterWarned) {
            LOG(warning, "fdispatch::Fdispatch::CheckTempFail: FNET active again");
        }
        _FNETLiveCounterFailed = false;
        _FNETLiveCounterWarned = false;
        _FNETLiveCounterDanger = false;
        _lastFNETLiveCounter = FNETLiveCounter;
    }

    if (failflag == _tempFail)
        return ret;

    if (_transportServer) {
        if (failflag) {
            _transportServer->setListen(false);
            LOG(error, "Disabling fnet server interface");
        } else {
            _transportServer->setListen(true);
            LOG(info, "Reenabling fnet server interface");
        }
    }
    _tempFail = failflag;
    return ret;
}

/**
 * Make the httpd and Monitor, and let a Thread execute each.
 * Set up stuff as specified in the fdispatch-rc-file.
 */
Fdispatch::Fdispatch(const config::ConfigUri &configUri)
    : _mypool(),
      _engineAdapter(),
      _transportServer(),
      _componentConfig(),
      _nodeManager(),
      _transport(),
      _FNET_adapter(this),
      _rpc(),
      _config(),
      _configUri(configUri),
      _fdispatchrcFetcher(configUri.getContext()),
      _rndGen(),
      _partition(0),
      _tempFail(false),
      _FNETLiveCounterDanger(false),
      _FNETLiveCounterWarned(false),
      _FNETLiveCounterFailed(false),
      _transportStarted(false),
      _lastFNETLiveCounter(false),
      _FNETLiveCounterDangerStart(),
      _timeouts(0u),
      _checkLimit(0u),
      _healthPort(0),
      _needRestart(false)
{
    int64_t cfgGen = -1;
    _config = config::ConfigGetter<FdispatchrcConfig>::
              getConfig(cfgGen, _configUri.getConfigId(), _configUri.getContext());
    LOG(config, "fdispatch version %s (RPC-port: %d, transport at %d)",
                FastS_VersionTag, _config->frtport, _config->ptport);

    _componentConfig.addConfig(vespalib::ComponentConfigProducer::Config("fdispatch", cfgGen,
                                       "config only obtained at startup"));
    _fdispatchrcFetcher.subscribe<FdispatchrcConfig>(configUri.getConfigId(), this);
    _fdispatchrcFetcher.start();
}

namespace {

bool needRestart(const FdispatchrcConfig & curr, const FdispatchrcConfig & next)
{
    if (curr.frtport != next.frtport) {
        LOG(warning, "FRT port has changed from %d to %d.", curr.frtport, next.frtport);
        return true;
    }
    if (curr.ptport != next.ptport) {
        LOG(warning, "PT port has changed from %d to %d.", curr.ptport, next.ptport);
        return true;
    }
    if (curr.healthport != next.healthport) {
        LOG(warning, "Health port has changed from %d to %d.", curr.healthport, next.healthport);
        return true;
    }
    return false;
}

}

void Fdispatch::configure(std::unique_ptr<FdispatchrcConfig> cfg)
{
    if (cfg && _config) {
        if ( needRestart(*_config, *cfg) ) {
            const int sleepMS = (0.100 + 2 * _rndGen.nextDouble()) * 1000;
            LOG(warning, "Will restart by abort in %d ms.", sleepMS);
            std::this_thread::sleep_for(std::chrono::milliseconds(sleepMS));
            _needRestart.store(true);
        }
    }
}

namespace {

CompressionConfig::Type
convert(InternalFdispatchrcType::Packetcompresstype type)
{
    switch (type) {
      case InternalFdispatchrcType::LZ4: return CompressionConfig::LZ4;
      default: return CompressionConfig::LZ4;
    }
}

}

bool
Fdispatch::Init()
{
    int  maxthreads;

    _tempFail = false;
    _FNETLiveCounterDanger = false;
    _FNETLiveCounterWarned = false;
    _FNETLiveCounterFailed = false;
    _lastFNETLiveCounter = 0;
    _timeouts = 0;
    _checkLimit = 60;

    FS4PersistentPacketStreamer::Instance.SetCompressionLimit(_config->packetcompresslimit);
    FS4PersistentPacketStreamer::Instance.SetCompressionLevel(_config->packetcompresslevel);
    FS4PersistentPacketStreamer::Instance.SetCompressionType(convert(_config->packetcompresstype));

    LOG(debug, "Creating FNET transport");
    _transport = std::make_unique<FNET_Transport>(_config->transportthreads);

    // grab node slowness limit defaults

    FastS_DataSetDesc::SetDefaultSlowQueryLimitFactor(_config->defaultslowquerylimitfactor);
    FastS_DataSetDesc::SetDefaultSlowQueryLimitBias(_config->defaultslowquerylimitbias);
    FastS_DataSetDesc::SetDefaultSlowDocsumLimitFactor(_config->defaultslowdocsumlimitfactor);
    FastS_DataSetDesc::SetDefaultSlowDocsumLimitBias(_config->defaultslowdocsumlimitbias);

    maxthreads = _config->maxthreads;
    _mypool = std::make_unique<FastOS_ThreadPool>(256 * 1024, maxthreads);

    // Max interval betw read from socket.
    FastS_TimeOut::_val[FastS_TimeOut::maxSockSilent] = _config->maxsocksilent;

    if (_transport) {
        _transport->SetIOCTimeOut((uint32_t) (FastS_TimeOut::_val[FastS_TimeOut::maxSockSilent] * 1000.0));
    }

    char timestr[40];
    FastS_TimeOut::WriteTime(timestr, sizeof(timestr), FastS_TimeOut::_val[FastS_TimeOut::maxSockSilent]);
    LOG(debug, "VERBOSE: Max time between successful read from a socket: %s", timestr);

    FastS_QueryCacheUtil::_systemMaxHits = std::numeric_limits<int>::max();
    LOG(debug, "VERBOSE: maxhits: %d", FastS_QueryCacheUtil::_systemMaxHits);

    FastS_QueryCacheUtil::_maxOffset = std::numeric_limits<int>::max();
    const uint32_t linesize = 1;
    if (FastS_QueryCacheUtil::_systemMaxHits < linesize
        && FastS_QueryCacheUtil::_maxOffset < linesize - FastS_QueryCacheUtil::_systemMaxHits) {
        LOG(warning, "maxoffset must be >= %d! (overriding config value)", linesize - FastS_QueryCacheUtil::_systemMaxHits);
        FastS_QueryCacheUtil::_maxOffset = linesize - FastS_QueryCacheUtil::_systemMaxHits;
    }
    LOG(debug, "VERBOSE: maxoffset: %d", FastS_QueryCacheUtil::_maxOffset);

    _partition = _config->partition;

    int ptportnum = _config->ptport;

    LOG(debug, "Using port number %d", ptportnum);

    _nodeManager = std::make_unique<FastS_NodeManager>(_componentConfig, this, _partition);

    GetFNETTransport()->SetTCPNoDelay(_config->transportnodelay);
    GetFNETTransport()->SetDirectWrite(_config->transportdirectwrite);

    if (ptportnum == 0) {
        throw vespalib::IllegalArgumentException("fdispatchrc.ptportnum must be non-zero, most likely an issue with config delivery.");
    }

    _engineAdapter = std::make_unique<fdispatch::EngineAdapter>(this, _mypool.get());
    _transportServer = std::make_unique<TransportServer>(*_engineAdapter, *_engineAdapter, *_engineAdapter, ptportnum, search::engine::TransportServer::DEBUG_ALL);
    _transportServer->setTCPNoDelay(_config->transportnodelay);
    _transportServer->setDirectWrite(_config->transportdirectwrite);

    if (!_transportServer->start()) {
        _transportServer.reset();
        _engineAdapter.reset();
        LOG(error, "CRITICAL: Failed to init upwards FNET transport on port %d", ptportnum);
        return false;
    }

    _nodeManager->SubscribePartMap(_configUri);

    if (_config->frtport != 0) {
        _rpc = std::make_unique<FastS_fdispatch_RPC>(this);
        if (!_rpc->Init(_config->frtport, _configUri.getConfigId())) {
            LOG(error, "RPC init failed");
            _rpc.reset();
        }
    } else {
        _rpc.reset();
    }

    // Kick off fdispatch administrative threads.
    if (_transport) {
        _FNET_adapter.init();
        bool rc = _transport->Start(_mypool.get());
        if (rc) {
            LOG(debug, "Started FNET transport");
            _transportStarted = true;
        } else {
            LOG(error, "Failed to start FNET transport");
        }
    }
    FastOS_Thread::Sleep(1000);
    if (_rpc) {
        _rpc->Start();
    }
    _healthPort = _config->healthport;
    return true;
}

void
Fdispatch::logPerformance()
{
    _nodeManager->logPerformance();
}

uint32_t
Fdispatch::getDispatchLevel()
{
    return _config->dispatchlevel;
}

}
<commit_msg>If you need to restart, just do it right away. You are probably taking up somebody elses port.<commit_after>// Copyright 2017 Yahoo Holdings. Licensed under the terms of the Apache 2.0 license. See LICENSE in the project root.

#include "fdispatch.h"
#include "engineadapter.h"
#include "rpc.h"
#include <vespa/searchcore/fdispatch/search/querycacheutil.h>
#include <vespa/searchcore/fdispatch/search/nodemanager.h>
#include <vespa/searchcore/util/eventloop.h>
#include <vespa/vespalib/util/exceptions.h>
#include <vespa/config/helper/configgetter.hpp>

#include <vespa/log/log.h>
LOG_SETUP(".fdispatch");

#ifndef V_TAG
#define V_TAG "NOTAG"
#endif

using search::fs4transport::FS4PersistentPacketStreamer;
using vespa::config::search::core::FdispatchrcConfig;
using vespa::config::search::core::internal::InternalFdispatchrcType;
using document::CompressionConfig;

char FastS_VersionTag[] = V_TAG;

namespace fdispatch
{

FastS_FNETAdapter::FastS_FNETAdapter(FastS_AppContext *appCtx)
    : _appCtx(appCtx),
      _nodeManager(),
      _timeKeeper(NULL),
      _transport(NULL),
      _last_now(0.0),
      _live_counter(0),
      _task()
{ }

FastS_FNETAdapter::~FastS_FNETAdapter()
{
    fini();
}

void
FastS_FNETAdapter::init()
{
    _nodeManager = _appCtx->GetNodeManager();
    _timeKeeper  = _appCtx->GetTimeKeeper();
    _transport   = _appCtx->GetFNETTransport();
    _last_now    = _timeKeeper->GetTime();
    _task.reset(new MyTask(_transport->GetScheduler(), *this));
    _task->ScheduleNow();
}

void
FastS_FNETAdapter::perform()
{
    double now = _timeKeeper->GetTime();
    double delta = now - _last_now;
    if (delta >= 3.0) {
        LOG(warning, "FNET loop high latency: %.3f", delta);
    }
    _last_now = now;
    ++_live_counter;
    _nodeManager->CheckEvents(_timeKeeper);
}

void
FastS_FNETAdapter::fini()
{
    if (_task) {
        _task->Kill();
        _task.reset();
    }
}

Fdispatch::~Fdispatch()
{
    if (_transportServer) {
        _transportServer->shutDown(); // sync shutdown
    }
    _FNET_adapter.fini();
    if (_nodeManager) {
        _nodeManager->ShutdownConfig();
    }
    if (_transport && _transportStarted) {
        _transport->ShutDown(true); // sync shutdown
    }
    if (_rpc) {
        _rpc->ShutDown(); // sync shutdown
    }

    LOG(debug, "Will close threadpool");
    _mypool->Close();
    LOG(debug, "Has closed threadpool");
    _transportServer.reset();
    _engineAdapter.reset();
    _nodeManager.reset();
    _transport.reset();
    _rpc.reset();
    _mypool.reset();
}

FNET_Transport *
Fdispatch::GetFNETTransport()
{
    return _transport.get();
}

FNET_Scheduler *
Fdispatch::GetFNETScheduler()
{
    return (_transport) ? _transport->GetScheduler() : nullptr;
}

FastS_NodeManager *
Fdispatch::GetNodeManager()
{
    return _nodeManager.get();
}

FastS_DataSetCollection *
Fdispatch::GetDataSetCollection()
{
    return ( _nodeManager) ? _nodeManager->GetDataSetCollection() : nullptr;
}

FastOS_ThreadPool *
Fdispatch::GetThreadPool()
{
    return _mypool.get();
}

bool
Fdispatch::Failed()
{
    return ( (_transportServer && _transportServer->isFailed())) || _needRestart;
}

bool
Fdispatch::CheckTempFail()
{
    bool ret;
    bool failflag = _nodeManager->GetTempFail();
    unsigned int FNETLiveCounter;

    ret = true;

    FNETLiveCounter = _FNET_adapter.GetLiveCounter();
    if (FNETLiveCounter == _lastFNETLiveCounter) {
        if (_FNETLiveCounterFailed) {
            failflag = true;				// Still failure
        } else if (!_FNETLiveCounterDanger) {
            _FNETLiveCounterDanger = true;
            _FNETLiveCounterDangerStart.SetNow();
        } else if (_FNETLiveCounterDangerStart.MilliSecsToNow() >= 6000) {
            LOG(error, "fdispatch::Fdispatch::CheckTempFail: FNET inactive for 6 seconds, deadlock ?");
            _FNETLiveCounterFailed = true;		// Note that we failed
            failflag = true;				// Force temporary failure
        } else if (_FNETLiveCounterDangerStart.MilliSecsToNow() >= 3000 &&
                   !_FNETLiveCounterWarned) {
            _FNETLiveCounterWarned = true;
            LOG(warning, "fdispatch::Fdispatch::CheckTempFail: FNET inactive for 3 seconds");
        }
    } else {
        if (_FNETLiveCounterFailed || _FNETLiveCounterWarned) {
            LOG(warning, "fdispatch::Fdispatch::CheckTempFail: FNET active again");
        }
        _FNETLiveCounterFailed = false;
        _FNETLiveCounterWarned = false;
        _FNETLiveCounterDanger = false;
        _lastFNETLiveCounter = FNETLiveCounter;
    }

    if (failflag == _tempFail)
        return ret;

    if (_transportServer) {
        if (failflag) {
            _transportServer->setListen(false);
            LOG(error, "Disabling fnet server interface");
        } else {
            _transportServer->setListen(true);
            LOG(info, "Reenabling fnet server interface");
        }
    }
    _tempFail = failflag;
    return ret;
}

/**
 * Make the httpd and Monitor, and let a Thread execute each.
 * Set up stuff as specified in the fdispatch-rc-file.
 */
Fdispatch::Fdispatch(const config::ConfigUri &configUri)
    : _mypool(),
      _engineAdapter(),
      _transportServer(),
      _componentConfig(),
      _nodeManager(),
      _transport(),
      _FNET_adapter(this),
      _rpc(),
      _config(),
      _configUri(configUri),
      _fdispatchrcFetcher(configUri.getContext()),
      _rndGen(),
      _partition(0),
      _tempFail(false),
      _FNETLiveCounterDanger(false),
      _FNETLiveCounterWarned(false),
      _FNETLiveCounterFailed(false),
      _transportStarted(false),
      _lastFNETLiveCounter(false),
      _FNETLiveCounterDangerStart(),
      _timeouts(0u),
      _checkLimit(0u),
      _healthPort(0),
      _needRestart(false)
{
    int64_t cfgGen = -1;
    _config = config::ConfigGetter<FdispatchrcConfig>::
              getConfig(cfgGen, _configUri.getConfigId(), _configUri.getContext());
    LOG(config, "fdispatch version %s (RPC-port: %d, transport at %d)",
                FastS_VersionTag, _config->frtport, _config->ptport);

    _componentConfig.addConfig(vespalib::ComponentConfigProducer::Config("fdispatch", cfgGen,
                                       "config only obtained at startup"));
    _fdispatchrcFetcher.subscribe<FdispatchrcConfig>(configUri.getConfigId(), this);
    _fdispatchrcFetcher.start();
}

namespace {

bool needRestart(const FdispatchrcConfig & curr, const FdispatchrcConfig & next)
{
    if (curr.frtport != next.frtport) {
        LOG(warning, "FRT port has changed from %d to %d.", curr.frtport, next.frtport);
        return true;
    }
    if (curr.ptport != next.ptport) {
        LOG(warning, "PT port has changed from %d to %d.", curr.ptport, next.ptport);
        return true;
    }
    if (curr.healthport != next.healthport) {
        LOG(warning, "Health port has changed from %d to %d.", curr.healthport, next.healthport);
        return true;
    }
    return false;
}

}

void Fdispatch::configure(std::unique_ptr<FdispatchrcConfig> cfg)
{
    if (cfg && _config) {
        if ( needRestart(*_config, *cfg) ) {
            LOG(warning, "Will restart by abort now.");
            _needRestart.store(true);
        }
    }
}

namespace {

CompressionConfig::Type
convert(InternalFdispatchrcType::Packetcompresstype type)
{
    switch (type) {
      case InternalFdispatchrcType::LZ4: return CompressionConfig::LZ4;
      default: return CompressionConfig::LZ4;
    }
}

}

bool
Fdispatch::Init()
{
    int  maxthreads;

    _tempFail = false;
    _FNETLiveCounterDanger = false;
    _FNETLiveCounterWarned = false;
    _FNETLiveCounterFailed = false;
    _lastFNETLiveCounter = 0;
    _timeouts = 0;
    _checkLimit = 60;

    FS4PersistentPacketStreamer::Instance.SetCompressionLimit(_config->packetcompresslimit);
    FS4PersistentPacketStreamer::Instance.SetCompressionLevel(_config->packetcompresslevel);
    FS4PersistentPacketStreamer::Instance.SetCompressionType(convert(_config->packetcompresstype));

    LOG(debug, "Creating FNET transport");
    _transport = std::make_unique<FNET_Transport>(_config->transportthreads);

    // grab node slowness limit defaults

    FastS_DataSetDesc::SetDefaultSlowQueryLimitFactor(_config->defaultslowquerylimitfactor);
    FastS_DataSetDesc::SetDefaultSlowQueryLimitBias(_config->defaultslowquerylimitbias);
    FastS_DataSetDesc::SetDefaultSlowDocsumLimitFactor(_config->defaultslowdocsumlimitfactor);
    FastS_DataSetDesc::SetDefaultSlowDocsumLimitBias(_config->defaultslowdocsumlimitbias);

    maxthreads = _config->maxthreads;
    _mypool = std::make_unique<FastOS_ThreadPool>(256 * 1024, maxthreads);

    // Max interval betw read from socket.
    FastS_TimeOut::_val[FastS_TimeOut::maxSockSilent] = _config->maxsocksilent;

    if (_transport) {
        _transport->SetIOCTimeOut((uint32_t) (FastS_TimeOut::_val[FastS_TimeOut::maxSockSilent] * 1000.0));
    }

    char timestr[40];
    FastS_TimeOut::WriteTime(timestr, sizeof(timestr), FastS_TimeOut::_val[FastS_TimeOut::maxSockSilent]);
    LOG(debug, "VERBOSE: Max time between successful read from a socket: %s", timestr);

    FastS_QueryCacheUtil::_systemMaxHits = std::numeric_limits<int>::max();
    LOG(debug, "VERBOSE: maxhits: %d", FastS_QueryCacheUtil::_systemMaxHits);

    FastS_QueryCacheUtil::_maxOffset = std::numeric_limits<int>::max();
    const uint32_t linesize = 1;
    if (FastS_QueryCacheUtil::_systemMaxHits < linesize
        && FastS_QueryCacheUtil::_maxOffset < linesize - FastS_QueryCacheUtil::_systemMaxHits) {
        LOG(warning, "maxoffset must be >= %d! (overriding config value)", linesize - FastS_QueryCacheUtil::_systemMaxHits);
        FastS_QueryCacheUtil::_maxOffset = linesize - FastS_QueryCacheUtil::_systemMaxHits;
    }
    LOG(debug, "VERBOSE: maxoffset: %d", FastS_QueryCacheUtil::_maxOffset);

    _partition = _config->partition;

    int ptportnum = _config->ptport;

    LOG(debug, "Using port number %d", ptportnum);

    _nodeManager = std::make_unique<FastS_NodeManager>(_componentConfig, this, _partition);

    GetFNETTransport()->SetTCPNoDelay(_config->transportnodelay);
    GetFNETTransport()->SetDirectWrite(_config->transportdirectwrite);

    if (ptportnum == 0) {
        throw vespalib::IllegalArgumentException("fdispatchrc.ptportnum must be non-zero, most likely an issue with config delivery.");
    }

    _engineAdapter = std::make_unique<fdispatch::EngineAdapter>(this, _mypool.get());
    _transportServer = std::make_unique<TransportServer>(*_engineAdapter, *_engineAdapter, *_engineAdapter, ptportnum, search::engine::TransportServer::DEBUG_ALL);
    _transportServer->setTCPNoDelay(_config->transportnodelay);
    _transportServer->setDirectWrite(_config->transportdirectwrite);

    if (!_transportServer->start()) {
        _transportServer.reset();
        _engineAdapter.reset();
        LOG(error, "CRITICAL: Failed to init upwards FNET transport on port %d", ptportnum);
        return false;
    }

    _nodeManager->SubscribePartMap(_configUri);

    if (_config->frtport != 0) {
        _rpc = std::make_unique<FastS_fdispatch_RPC>(this);
        if (!_rpc->Init(_config->frtport, _configUri.getConfigId())) {
            LOG(error, "RPC init failed");
            _rpc.reset();
        }
    } else {
        _rpc.reset();
    }

    // Kick off fdispatch administrative threads.
    if (_transport) {
        _FNET_adapter.init();
        bool rc = _transport->Start(_mypool.get());
        if (rc) {
            LOG(debug, "Started FNET transport");
            _transportStarted = true;
        } else {
            LOG(error, "Failed to start FNET transport");
        }
    }
    FastOS_Thread::Sleep(1000);
    if (_rpc) {
        _rpc->Start();
    }
    _healthPort = _config->healthport;
    return true;
}

void
Fdispatch::logPerformance()
{
    _nodeManager->logPerformance();
}

uint32_t
Fdispatch::getDispatchLevel()
{
    return _config->dispatchlevel;
}

}
<|endoftext|>