<commit_before>/*

Copyright (c) 2003, Arvid Norberg
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 author 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 OWNER 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 <vector>
#include <iostream>
#include <cctype>
#include <iomanip>
#include <sstream>

#include "zlib.h"

#include "libtorrent/tracker_manager.hpp"
#include "libtorrent/http_tracker_connection.hpp"
#include "libtorrent/udp_tracker_connection.hpp"
#include "libtorrent/entry.hpp"
#include "libtorrent/bencode.hpp"
#include "libtorrent/torrent.hpp"

using namespace libtorrent;

namespace
{
	enum
	{
		minimum_tracker_response_length = 3,
		http_buffer_size = 2048
	};

	enum
	{
		FTEXT = 0x01,
		FHCRC = 0x02,
		FEXTRA = 0x04,
		FNAME = 0x08,
		FCOMMENT = 0x10,
		FRESERVED = 0xe0,

		GZIP_MAGIC0 = 0x1f,
		GZIP_MAGIC1 = 0x8b
	};

}

namespace libtorrent
{

	// returns -1 if gzip header is invalid or the header size in bytes
	int gzip_header(const char* buf, int size)
	{
		assert(buf != 0);
		assert(size > 0);

		const unsigned char* buffer = reinterpret_cast<const unsigned char*>(buf);
		const int total_size = size;

		// The zip header cannot be shorter than 10 bytes
		if (size < 10) return -1;

		// check the magic header of gzip
		if ((buffer[0] != GZIP_MAGIC0) || (buffer[1] != GZIP_MAGIC1)) return -1;

		int method = buffer[2];
		int flags = buffer[3];

		// check for reserved flag and make sure it's compressed with the correct metod
		if (method != Z_DEFLATED || (flags & FRESERVED) != 0) return -1;

		// skip time, xflags, OS code
		size -= 10;
		buffer += 10;

		if (flags & FEXTRA)
		{
			int extra_len;

			if (size < 2) return -1;

			extra_len = (buffer[1] << 8) | buffer[0];

			if (size < (extra_len+2)) return -1;
			size -= (extra_len + 2);
			buffer += (extra_len + 2);
		}

		if (flags & FNAME)
		{
			while (size && *buffer)
			{
				--size;
				++buffer;
			}
			if (!size || *buffer) return -1;

			--size;
			++buffer;
		}

		if (flags & FCOMMENT)
		{
			while (size && *buffer)
			{
				--size;
				++buffer;
			}
			if (!size || *buffer) return -1;

			--size;
			++buffer;
		}

		if (flags & FHCRC)
		{
			if (size < 2) return -1;

			size -= 2;
			buffer += 2;
		}

		return total_size - size;
	}

	bool inflate_gzip(
		std::vector<char>& buffer
		, request_callback* requester
		, int maximum_tracker_response_length)
	{
		assert(maximum_tracker_response_length > 0);

		int header_len = gzip_header(&buffer[0], (int)buffer.size());
		if (header_len < 0)
		{
			requester->tracker_request_error(200, "invalid gzip header in tracker response");
			return true;
		}

		// start off wth one kilobyte and grow
		// if needed
		std::vector<char> inflate_buffer(1024);

		// initialize the zlib-stream
		z_stream str;

		// subtract 8 from the end of the buffer since that's CRC32 and input size
		// and those belong to the gzip file
		str.avail_in = (int)buffer.size() - header_len - 8;
		str.next_in = reinterpret_cast<Bytef*>(&buffer[header_len]);
		str.next_out = reinterpret_cast<Bytef*>(&inflate_buffer[0]);
		str.avail_out = (int)inflate_buffer.size();
		str.zalloc = Z_NULL;
		str.zfree = Z_NULL;
		str.opaque = 0;
		// -15 is really important. It will make inflate() not look for a zlib header
		// and just deflate the buffer
		if (inflateInit2(&str, -15) != Z_OK)
		{
			requester->tracker_request_error(200, "gzip out of memory");
			return true;
		}

		// inflate and grow inflate_buffer as needed
		int ret = inflate(&str, Z_SYNC_FLUSH);
		while (ret == Z_OK)
		{
			if (str.avail_out == 0)
			{
				if (inflate_buffer.size() >= (unsigned)maximum_tracker_response_length)
				{
					inflateEnd(&str);
					requester->tracker_request_error(200, "tracker response too large");
					return true;
				}
				int new_size = (int)inflate_buffer.size() * 2;
				if (new_size > maximum_tracker_response_length) new_size = maximum_tracker_response_length;
				int old_size = (int)inflate_buffer.size();

				inflate_buffer.resize(new_size);
				str.next_out = reinterpret_cast<Bytef*>(&inflate_buffer[old_size]);
				str.avail_out = new_size - old_size;
			}

			ret = inflate(&str, Z_SYNC_FLUSH);
		}

		inflate_buffer.resize(inflate_buffer.size() - str.avail_out);
		inflateEnd(&str);

		if (ret != Z_STREAM_END)
		{
			requester->tracker_request_error(200, "gzip error");
			return true;
		}

		// commit the resulting buffer
		std::swap(buffer, inflate_buffer);
		return false;
	}

	std::string base64encode(const std::string& s)
	{
		static const char base64_table[] =
		{
			'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H',
			'I', 'J', 'K', 'L', 'M', 'N', 'O', 'P',
			'Q', 'R', 'S', 'T', 'U', 'V', 'W', 'X',
			'Y', 'Z', 'a', 'b', 'c', 'd', 'e', 'f',
			'g', 'h', 'i', 'j', 'k', 'l', 'm', 'n',
			'o', 'p', 'q', 'r', 's', 't', 'u', 'v',
			'w', 'x', 'y', 'z', '0', '1', '2', '3',
			'4', '5', '6', '7', '8', '9', '+', '/'
		};

		unsigned char inbuf[3];
		unsigned char outbuf[4];

		std::string ret;
		for (std::string::const_iterator i = s.begin(); i != s.end();)
		{
			// available input is 1,2 or 3 bytes
			// since we read 3 bytes at a time at most
			int available_input = std::min(3, (int)std::distance(i, s.end()));

			// clear input buffer
			std::fill(inbuf, inbuf+3, 0);

			// read a chunk of input into inbuf
			for (int j = 0; j < available_input; ++j)
			{
				inbuf[j] = *i;
				++i;
			}

			// encode inbuf to outbuf
			outbuf[0] = (inbuf[0] & 0xfc) >> 2;
			outbuf[1] = ((inbuf[0] & 0x03) << 4) | ((inbuf [1] & 0xf0) >> 4);
			outbuf[2] = ((inbuf[1] & 0x0f) << 2) | ((inbuf [2] & 0xc0) >> 6);
			outbuf[3] = inbuf[2] & 0x3f;

			// write output
			for (int j = 0; j < available_input+1; ++j)
			{
				ret += base64_table[outbuf[j]];
			}

			// write pad
			for (int j = 0; j < 3 - available_input; ++j)
			{
				ret += '=';
			}
		}
		return ret;
	}

	void tracker_manager::tick()
	{
		std::vector<boost::shared_ptr<tracker_connection> >::iterator i;
		for (i = m_connections.begin(); i != m_connections.end(); ++i)
		{
			boost::shared_ptr<tracker_connection>& c = *i;
			try
			{
				if (!c->tick()) continue;
			}
			catch (const std::exception& e)
			{
				if (c->requester())
					c->requester()->tracker_request_error(-1, e.what());
			}
			if (c->requester()) c->requester()->m_manager = 0;
			m_connections.erase(i);
			--i; // compensate for the remove
		}
	}

	void tracker_manager::queue_request(
		tracker_request req
		, request_callback* c
		, std::string const& password)
	{
		assert(req.num_want >= 0);
		if (req.event == tracker_request::stopped)
			req.num_want = 0;

		try
		{
			std::string hostname; // hostname only
			std::string protocol; // should be http
			int port = 80;

			// PARSE URL
			std::string::iterator start = req.url.begin();
			std::string::iterator end
				= std::find(req.url.begin(), req.url.end(), ':');
			protocol = std::string(start, end);

			if (end == req.url.end()) throw std::runtime_error("invalid url");
			++end;
			if (end == req.url.end()) throw std::runtime_error("invalid url");
			if (*end != '/') throw std::runtime_error("invalid url");
			++end;
			if (end == req.url.end()) throw std::runtime_error("invalid url");
			if (*end != '/') throw std::runtime_error("invalid url");
			++end;
			start = end;

			end = std::find(start, req.url.end(), '/');
			std::string::const_iterator port_pos
				= std::find(start, req.url.end(), ':');

			if (port_pos < end)
			{
				hostname.assign(start, port_pos);
				++port_pos;
				try
				{
					port = boost::lexical_cast<int>(std::string(port_pos, end));
				}
				catch(boost::bad_lexical_cast&)
				{
					throw std::runtime_error("invalid url");
				}
			}
			else
			{
				hostname.assign(start, end);
			}

			start = end;
			std::string request_string = std::string(start, req.url.end());

			boost::shared_ptr<tracker_connection> con;

			if (protocol == "http")
			{
				con.reset(new http_tracker_connection(
					req
					, hostname
					, port
					, request_string
					, c
					, m_settings
					, password));
			}
			else if (protocol == "udp")
			{
				con.reset(new udp_tracker_connection(
					req
					, hostname
					, port
					, c
					, m_settings));
			}
			else
			{
				throw std::runtime_error("unkown protocol in tracker url");
			}

			m_connections.push_back(con);

			if (con->requester()) con->requester()->m_manager = this;
		}
		catch (std::exception& e)
		{
			if (c) c->tracker_request_error(-1, e.what());
		}
	}

	void tracker_manager::abort_request(request_callback* c)
	{
		assert(c != 0);
		std::vector<boost::shared_ptr<tracker_connection> >::iterator i;
		for (i = m_connections.begin(); i != m_connections.end(); ++i)
		{
			if ((*i)->requester() == c)
			{
				m_connections.erase(i);
				break;
			}
		}
	}

	void tracker_manager::abort_all_requests()
	{
		// removes all connections from m_connections
		// except those with a requester == 0 (since those are
		// 'event=stopped'-requests)

		std::vector<boost::shared_ptr<tracker_connection> > keep_connections;

		for (std::vector<boost::shared_ptr<tracker_connection> >::const_iterator i =
				m_connections.begin();
			i != m_connections.end();
			++i)
		{
			if ((*i)->requester() == 0) keep_connections.push_back(*i);
		}

		std::swap(m_connections, keep_connections);
	}

	bool tracker_manager::send_finished() const
	{
		for (std::vector<boost::shared_ptr<tracker_connection> >::const_iterator i =
				m_connections.begin();
			i != m_connections.end();
			++i)
		{
			if (!(*i)->send_finished()) return false;
		}
		return true;
	}

}
<commit_msg>*** empty log message ***<commit_after>/*

Copyright (c) 2003, Arvid Norberg
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 author 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 OWNER 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 <vector>
#include <iostream>
#include <cctype>
#include <iomanip>
#include <sstream>

#include "zlib.h"

#include "libtorrent/tracker_manager.hpp"
#include "libtorrent/http_tracker_connection.hpp"
#include "libtorrent/udp_tracker_connection.hpp"
#include "libtorrent/entry.hpp"
#include "libtorrent/bencode.hpp"
#include "libtorrent/torrent.hpp"

using namespace libtorrent;

namespace
{
	enum
	{
		minimum_tracker_response_length = 3,
		http_buffer_size = 2048
	};

	enum
	{
		FTEXT = 0x01,
		FHCRC = 0x02,
		FEXTRA = 0x04,
		FNAME = 0x08,
		FCOMMENT = 0x10,
		FRESERVED = 0xe0,

		GZIP_MAGIC0 = 0x1f,
		GZIP_MAGIC1 = 0x8b
	};

}

namespace libtorrent
{

	// returns -1 if gzip header is invalid or the header size in bytes
	int gzip_header(const char* buf, int size)
	{
		assert(buf != 0);
		assert(size > 0);

		const unsigned char* buffer = reinterpret_cast<const unsigned char*>(buf);
		const int total_size = size;

		// The zip header cannot be shorter than 10 bytes
		if (size < 10) return -1;

		// check the magic header of gzip
		if ((buffer[0] != GZIP_MAGIC0) || (buffer[1] != GZIP_MAGIC1)) return -1;

		int method = buffer[2];
		int flags = buffer[3];

		// check for reserved flag and make sure it's compressed with the correct metod
		if (method != Z_DEFLATED || (flags & FRESERVED) != 0) return -1;

		// skip time, xflags, OS code
		size -= 10;
		buffer += 10;

		if (flags & FEXTRA)
		{
			int extra_len;

			if (size < 2) return -1;

			extra_len = (buffer[1] << 8) | buffer[0];

			if (size < (extra_len+2)) return -1;
			size -= (extra_len + 2);
			buffer += (extra_len + 2);
		}

		if (flags & FNAME)
		{
			while (size && *buffer)
			{
				--size;
				++buffer;
			}
			if (!size || *buffer) return -1;

			--size;
			++buffer;
		}

		if (flags & FCOMMENT)
		{
			while (size && *buffer)
			{
				--size;
				++buffer;
			}
			if (!size || *buffer) return -1;

			--size;
			++buffer;
		}

		if (flags & FHCRC)
		{
			if (size < 2) return -1;

			size -= 2;
			buffer += 2;
		}

		return total_size - size;
	}

	bool inflate_gzip(
		std::vector<char>& buffer
		, request_callback* requester
		, int maximum_tracker_response_length)
	{
		assert(maximum_tracker_response_length > 0);

		int header_len = gzip_header(&buffer[0], (int)buffer.size());
		if (header_len < 0)
		{
			requester->tracker_request_error(200, "invalid gzip header in tracker response");
			return true;
		}

		// start off wth one kilobyte and grow
		// if needed
		std::vector<char> inflate_buffer(1024);

		// initialize the zlib-stream
		z_stream str;

		// subtract 8 from the end of the buffer since that's CRC32 and input size
		// and those belong to the gzip file
		str.avail_in = (int)buffer.size() - header_len - 8;
		str.next_in = reinterpret_cast<Bytef*>(&buffer[header_len]);
		str.next_out = reinterpret_cast<Bytef*>(&inflate_buffer[0]);
		str.avail_out = (int)inflate_buffer.size();
		str.zalloc = Z_NULL;
		str.zfree = Z_NULL;
		str.opaque = 0;
		// -15 is really important. It will make inflate() not look for a zlib header
		// and just deflate the buffer
		if (inflateInit2(&str, -15) != Z_OK)
		{
			requester->tracker_request_error(200, "gzip out of memory");
			return true;
		}

		// inflate and grow inflate_buffer as needed
		int ret = inflate(&str, Z_SYNC_FLUSH);
		while (ret == Z_OK)
		{
			if (str.avail_out == 0)
			{
				if (inflate_buffer.size() >= (unsigned)maximum_tracker_response_length)
				{
					inflateEnd(&str);
					requester->tracker_request_error(200, "tracker response too large");
					return true;
				}
				int new_size = (int)inflate_buffer.size() * 2;
				if (new_size > maximum_tracker_response_length) new_size = maximum_tracker_response_length;
				int old_size = (int)inflate_buffer.size();

				inflate_buffer.resize(new_size);
				str.next_out = reinterpret_cast<Bytef*>(&inflate_buffer[old_size]);
				str.avail_out = new_size - old_size;
			}

			ret = inflate(&str, Z_SYNC_FLUSH);
		}

		inflate_buffer.resize(inflate_buffer.size() - str.avail_out);
		inflateEnd(&str);

		if (ret != Z_STREAM_END)
		{
			requester->tracker_request_error(200, "gzip error");
			return true;
		}

		// commit the resulting buffer
		std::swap(buffer, inflate_buffer);
		return false;
	}

	std::string base64encode(const std::string& s)
	{
		static const char base64_table[] =
		{
			'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H',
			'I', 'J', 'K', 'L', 'M', 'N', 'O', 'P',
			'Q', 'R', 'S', 'T', 'U', 'V', 'W', 'X',
			'Y', 'Z', 'a', 'b', 'c', 'd', 'e', 'f',
			'g', 'h', 'i', 'j', 'k', 'l', 'm', 'n',
			'o', 'p', 'q', 'r', 's', 't', 'u', 'v',
			'w', 'x', 'y', 'z', '0', '1', '2', '3',
			'4', '5', '6', '7', '8', '9', '+', '/'
		};

		unsigned char inbuf[3];
		unsigned char outbuf[4];

		std::string ret;
		for (std::string::const_iterator i = s.begin(); i != s.end();)
		{
			// available input is 1,2 or 3 bytes
			// since we read 3 bytes at a time at most
			int available_input = std::min(3, (int)std::distance(i, s.end()));

			// clear input buffer
			std::fill(inbuf, inbuf+3, 0);

			// read a chunk of input into inbuf
			for (int j = 0; j < available_input; ++j)
			{
				inbuf[j] = *i;
				++i;
			}

			// encode inbuf to outbuf
			outbuf[0] = (inbuf[0] & 0xfc) >> 2;
			outbuf[1] = ((inbuf[0] & 0x03) << 4) | ((inbuf [1] & 0xf0) >> 4);
			outbuf[2] = ((inbuf[1] & 0x0f) << 2) | ((inbuf [2] & 0xc0) >> 6);
			outbuf[3] = inbuf[2] & 0x3f;

			// write output
			for (int j = 0; j < available_input+1; ++j)
			{
				ret += base64_table[outbuf[j]];
			}

			// write pad
			for (int j = 0; j < 3 - available_input; ++j)
			{
				ret += '=';
			}
		}
		return ret;
	}

	void tracker_manager::tick()
	{
		std::vector<boost::shared_ptr<tracker_connection> >::iterator i;
		for (i = m_connections.begin(); i != m_connections.end(); ++i)
		{
			boost::shared_ptr<tracker_connection>& c = *i;
			try
			{
				if (!c->tick()) continue;
			}
			catch (const std::exception& e)
			{
				if (c->requester())
					c->requester()->tracker_request_error(-1, e.what());
			}
			if (c->requester()) c->requester()->m_manager = 0;
			m_connections.erase(i);
			--i; // compensate for the remove
		}
	}

	void tracker_manager::queue_request(
		tracker_request req
		, request_callback* c
		, std::string const& password)
	{
		assert(req.num_want >= 0);
		if (req.event == tracker_request::stopped)
			req.num_want = 0;

		try
		{
			std::string hostname; // hostname only
			std::string protocol; // should be http
			int port = 80;

			// PARSE URL
			std::string::iterator start = req.url.begin();
			std::string::iterator end
				= std::find(req.url.begin(), req.url.end(), ':');
			protocol = std::string(start, end);

			if (end == req.url.end()) throw std::runtime_error("invalid url");
			++end;
			if (end == req.url.end()) throw std::runtime_error("invalid url");
			if (*end != '/') throw std::runtime_error("invalid url");
			++end;
			if (end == req.url.end()) throw std::runtime_error("invalid url");
			if (*end != '/') throw std::runtime_error("invalid url");
			++end;
			start = end;

			end = std::find(start, req.url.end(), '/');
			std::string::iterator port_pos
				= std::find(start, req.url.end(), ':');

			if (port_pos < end)
			{
				hostname.assign(start, port_pos);
				++port_pos;
				try
				{
					port = boost::lexical_cast<int>(std::string(port_pos, end));
				}
				catch(boost::bad_lexical_cast&)
				{
					throw std::runtime_error("invalid url");
				}
			}
			else
			{
				hostname.assign(start, end);
			}

			start = end;
			std::string request_string = std::string(start, req.url.end());

			boost::shared_ptr<tracker_connection> con;

			if (protocol == "http")
			{
				con.reset(new http_tracker_connection(
					req
					, hostname
					, port
					, request_string
					, c
					, m_settings
					, password));
			}
			else if (protocol == "udp")
			{
				con.reset(new udp_tracker_connection(
					req
					, hostname
					, port
					, c
					, m_settings));
			}
			else
			{
				throw std::runtime_error("unkown protocol in tracker url");
			}

			m_connections.push_back(con);

			if (con->requester()) con->requester()->m_manager = this;
		}
		catch (std::exception& e)
		{
			if (c) c->tracker_request_error(-1, e.what());
		}
	}

	void tracker_manager::abort_request(request_callback* c)
	{
		assert(c != 0);
		std::vector<boost::shared_ptr<tracker_connection> >::iterator i;
		for (i = m_connections.begin(); i != m_connections.end(); ++i)
		{
			if ((*i)->requester() == c)
			{
				m_connections.erase(i);
				break;
			}
		}
	}

	void tracker_manager::abort_all_requests()
	{
		// removes all connections from m_connections
		// except those with a requester == 0 (since those are
		// 'event=stopped'-requests)

		std::vector<boost::shared_ptr<tracker_connection> > keep_connections;

		for (std::vector<boost::shared_ptr<tracker_connection> >::const_iterator i =
				m_connections.begin();
			i != m_connections.end();
			++i)
		{
			if ((*i)->requester() == 0) keep_connections.push_back(*i);
		}

		std::swap(m_connections, keep_connections);
	}

	bool tracker_manager::send_finished() const
	{
		for (std::vector<boost::shared_ptr<tracker_connection> >::const_iterator i =
				m_connections.begin();
			i != m_connections.end();
			++i)
		{
			if (!(*i)->send_finished()) return false;
		}
		return true;
	}

}
<|endoftext|>


<commit_before>/******************************************************************************/
/* Copyright (c) 2013-2014 VectorChief (at github, bitbucket, sourceforge)    */
/* Distributed under the MIT software license, see the accompanying           */
/* file COPYING or http://www.opensource.org/licenses/mit-license.php         */
/******************************************************************************/

#include <malloc.h>
#include <string.h>

#include "engine.h"

#define RUN_LEVEL       9
#define VERBOSE         RT_FALSE
#define CYC_SIZE        10

#define RT_X_RES        800
#define RT_Y_RES        480

rt_cell     x_res   = RT_X_RES;
rt_cell     y_res   = RT_Y_RES;
rt_cell     x_row   = RT_X_RES;
rt_word     frame[RT_X_RES * RT_Y_RES];

rt_cell     fsaa    = RT_FSAA_NO;

rt_Scene   *scene   = RT_NULL;

static
rt_void frame_cpy(rt_word *fd, rt_word *fs)
{
    rt_cell i;

    for (i = 0; i < y_res * x_row; i++)
    {
       *fd++ = *fs++;
    }
}

static
rt_cell frame_cmp(rt_word *f1, rt_word *f2)
{
    rt_cell i;

    for (i = 0; i < y_res * x_row; i++)
    {
        if (f1[i] != f2[i])
        {
            RT_LOGI("Frames differ\n");
            return 1;
        }
    }

    if (VERBOSE)
    {
        RT_LOGI("Frames are identical\n");
    }

    return 0;
}

/******************************************************************************/
/******************************   RUN LEVEL  1   ******************************/
/******************************************************************************/

#if RUN_LEVEL >=  1

#include "scn_test01.h"

rt_void test01(rt_cell opts)
{
    scene = new rt_Scene(&scn_test01::sc_root,
                        x_res, y_res, x_row, RT_NULL,
                        malloc, free,
                        RT_NULL, RT_NULL,
                        RT_NULL, RT_NULL);

    scene->set_opts(opts);
}

#endif /* RUN_LEVEL  1 */

/******************************************************************************/
/******************************   RUN LEVEL  2   ******************************/
/******************************************************************************/

#if RUN_LEVEL >=  2

#include "scn_test02.h"

rt_void test02(rt_cell opts)
{
    scene = new rt_Scene(&scn_test02::sc_root,
                        x_res, y_res, x_row, RT_NULL,
                        malloc, free,
                        RT_NULL, RT_NULL,
                        RT_NULL, RT_NULL);

    scene->set_opts(opts);
}

#endif /* RUN_LEVEL  2 */

/******************************************************************************/
/******************************   RUN LEVEL  3   ******************************/
/******************************************************************************/

#if RUN_LEVEL >=  3

#include "scn_test03.h"

rt_void test03(rt_cell opts)
{
    scene = new rt_Scene(&scn_test03::sc_root,
                        x_res, y_res, x_row, RT_NULL,
                        malloc, free,
                        RT_NULL, RT_NULL,
                        RT_NULL, RT_NULL);

    scene->set_opts(opts);
}

#endif /* RUN_LEVEL  3 */

/******************************************************************************/
/******************************   RUN LEVEL  4   ******************************/
/******************************************************************************/

#if RUN_LEVEL >=  4

#include "scn_test04.h"

rt_void test04(rt_cell opts)
{
    scene = new rt_Scene(&scn_test04::sc_root,
                        x_res, y_res, x_row, RT_NULL,
                        malloc, free,
                        RT_NULL, RT_NULL,
                        RT_NULL, RT_NULL);

    scene->set_opts(opts);
}

#endif /* RUN_LEVEL  4 */

/******************************************************************************/
/******************************   RUN LEVEL  5   ******************************/
/******************************************************************************/

#if RUN_LEVEL >=  5

#include "scn_test05.h"

rt_void test05(rt_cell opts)
{
    scene = new rt_Scene(&scn_test05::sc_root,
                        x_res, y_res, x_row, RT_NULL,
                        malloc, free,
                        RT_NULL, RT_NULL,
                        RT_NULL, RT_NULL);

    scene->set_opts(opts);
}

#endif /* RUN_LEVEL  5 */

/******************************************************************************/
/******************************   RUN LEVEL  6   ******************************/
/******************************************************************************/

#if RUN_LEVEL >=  6

#include "scn_test06.h"

rt_void test06(rt_cell opts)
{
    scene = new rt_Scene(&scn_test06::sc_root,
                        x_res, y_res, x_row, RT_NULL,
                        malloc, free,
                        RT_NULL, RT_NULL,
                        RT_NULL, RT_NULL);

    scene->set_opts(opts);
}

#endif /* RUN_LEVEL  6 */

/******************************************************************************/
/******************************   RUN LEVEL  7   ******************************/
/******************************************************************************/

#if RUN_LEVEL >=  7

#include "scn_test07.h"

rt_void test07(rt_cell opts)
{
    scene = new rt_Scene(&scn_test07::sc_root,
                        x_res, y_res, x_row, RT_NULL,
                        malloc, free,
                        RT_NULL, RT_NULL,
                        RT_NULL, RT_NULL);

    scene->set_opts(opts);
}

#endif /* RUN_LEVEL  7 */

/******************************************************************************/
/******************************   RUN LEVEL  8   ******************************/
/******************************************************************************/

#if RUN_LEVEL >=  8

#include "scn_test08.h"

rt_void test08(rt_cell opts)
{
    scene = new rt_Scene(&scn_test08::sc_root,
                        x_res, y_res, x_row, RT_NULL,
                        malloc, free,
                        RT_NULL, RT_NULL,
                        RT_NULL, RT_NULL);

    scene->set_opts(opts);
}

#endif /* RUN_LEVEL  8 */

/******************************************************************************/
/******************************   RUN LEVEL  9   ******************************/
/******************************************************************************/

#if RUN_LEVEL >=  9

#include "scn_test09.h"

rt_void test09(rt_cell opts)
{
    scene = new rt_Scene(&scn_test09::sc_root,
                        x_res, y_res, x_row, RT_NULL,
                        malloc, free,
                        RT_NULL, RT_NULL,
                        RT_NULL, RT_NULL);

    scene->set_opts(opts);
}

#endif /* RUN_LEVEL  9 */

/******************************************************************************/
/*********************************   TABLES   *********************************/
/******************************************************************************/

typedef rt_void (*testXX)(rt_cell);

testXX test[RUN_LEVEL] =
{
#if RUN_LEVEL >=  1
    test01,
#endif /* RUN_LEVEL  1 */

#if RUN_LEVEL >=  2
    test02,
#endif /* RUN_LEVEL  2 */

#if RUN_LEVEL >=  3
    test03,
#endif /* RUN_LEVEL  3 */

#if RUN_LEVEL >=  4
    test04,
#endif /* RUN_LEVEL  4 */

#if RUN_LEVEL >=  5
    test05,
#endif /* RUN_LEVEL  5 */

#if RUN_LEVEL >=  6
    test06,
#endif /* RUN_LEVEL  6 */

#if RUN_LEVEL >=  7
    test07,
#endif /* RUN_LEVEL  7 */

#if RUN_LEVEL >=  8
    test08,
#endif /* RUN_LEVEL  8 */

#if RUN_LEVEL >=  9
    test09,
#endif /* RUN_LEVEL  9 */
};

/******************************************************************************/
/**********************************   MAIN   **********************************/
/******************************************************************************/

rt_long get_time();

rt_cell main()
{
    rt_long time1 = 0;
    rt_long time2 = 0;
    rt_long tN = 0;
    rt_long tF = 0;

    rt_cell i, j;

    for (i = 0; i < RUN_LEVEL; i++)
    {
        RT_LOGI("-----------------  RUN LEVEL = %2d  -----------------\n", i+1);
        try
        {
            scene = RT_NULL;
            test[i](RT_OPTS_NONE);

            time1 = get_time();

            for (j = 0; j < CYC_SIZE; j++)
            {
                scene->render(j * 16);
            }

            time2 = get_time();
            tN = time2 - time1;
            RT_LOGI("Time N = %d\n", (rt_cell)tN);

            frame_cpy(frame, scene->get_frame());
            delete scene;

            scene = RT_NULL;
            test[i](RT_OPTS_FULL);

            time1 = get_time();

            for (j = 0; j < CYC_SIZE; j++)
            {
                scene->render(j * 16);
            }

            time2 = get_time();
            tF = time2 - time1;
            RT_LOGI("Time F = %d\n", (rt_cell)tF);

            frame_cmp(frame, scene->get_frame());
            delete scene;
        }
        catch (rt_Exception e)
        {
            RT_LOGE("Exception: %s\n", e.err);
        }
        RT_LOGI("----------------------------------------------------\n");
    }

#if   defined (WIN32) /* Win32, MSVC ---------------------------------------- */

    RT_LOGI("Type any letter and press ENTER to exit:");
    rt_char str[256]; /* not secure, do not inherit this practice */
    scanf("%s", str); /* not secure, do not inherit this practice */

#endif /* ------------- OS specific ----------------------------------------- */

    return 0;
}

#if   defined (WIN32) /* Win32, MSVC ---------------------------------------- */

#include <windows.h>

rt_long get_time()
{
    LARGE_INTEGER fr;
    QueryPerformanceFrequency(&fr);
    LARGE_INTEGER tm;
    QueryPerformanceCounter(&tm);
    return (rt_long)(tm.QuadPart * 1000 / fr.QuadPart);
}

#elif defined (linux) /* Linux, GCC ----------------------------------------- */

#include <sys/time.h>

rt_long get_time()
{
    timeval tm;
    gettimeofday(&tm, NULL);
    return (rt_long)(tm.tv_sec * 1000 + tm.tv_usec / 1000);
}

#endif /* ------------- OS specific ----------------------------------------- */

/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
<commit_msg>extend verbose mode in core test to print all diffs<commit_after>/******************************************************************************/
/* Copyright (c) 2013-2014 VectorChief (at github, bitbucket, sourceforge)    */
/* Distributed under the MIT software license, see the accompanying           */
/* file COPYING or http://www.opensource.org/licenses/mit-license.php         */
/******************************************************************************/

#include <malloc.h>
#include <string.h>

#include "engine.h"

#define RUN_LEVEL       9
#define VERBOSE         RT_FALSE
#define CYC_SIZE        5

#define RT_X_RES        800
#define RT_Y_RES        480

rt_cell     x_res   = RT_X_RES;
rt_cell     y_res   = RT_Y_RES;
rt_cell     x_row   = RT_X_RES;
rt_word     frame[RT_X_RES * RT_Y_RES];

rt_cell     fsaa    = RT_FSAA_NO;

rt_Scene   *scene   = RT_NULL;

static
rt_void frame_cpy(rt_word *fd, rt_word *fs)
{
    rt_cell i;

    for (i = 0; i < y_res * x_row; i++)
    {
       *fd++ = *fs++;
    }
}

static
rt_cell frame_cmp(rt_word *f1, rt_word *f2)
{
    rt_cell i, ret = 0;

    for (i = 0; i < y_res * x_row; i++)
    {
        if (f1[i] != f2[i])
        {
            ret = 1;

            RT_LOGI("Frames differ (%06X %06X) at x = %d, y = %d\n",
                        f1[i], f2[i], i % x_row, i / x_row);

            if (!VERBOSE)
            {
                break;
            }
        }
    }

    if (VERBOSE && ret == 0)
    {
        RT_LOGI("Frames are identical\n");
    }

    return ret;
}

/******************************************************************************/
/******************************   RUN LEVEL  1   ******************************/
/******************************************************************************/

#if RUN_LEVEL >=  1

#include "scn_test01.h"

rt_void test01(rt_cell opts)
{
    scene = new rt_Scene(&scn_test01::sc_root,
                        x_res, y_res, x_row, RT_NULL,
                        malloc, free,
                        RT_NULL, RT_NULL,
                        RT_NULL, RT_NULL);

    scene->set_opts(opts);
}

#endif /* RUN_LEVEL  1 */

/******************************************************************************/
/******************************   RUN LEVEL  2   ******************************/
/******************************************************************************/

#if RUN_LEVEL >=  2

#include "scn_test02.h"

rt_void test02(rt_cell opts)
{
    scene = new rt_Scene(&scn_test02::sc_root,
                        x_res, y_res, x_row, RT_NULL,
                        malloc, free,
                        RT_NULL, RT_NULL,
                        RT_NULL, RT_NULL);

    scene->set_opts(opts);
}

#endif /* RUN_LEVEL  2 */

/******************************************************************************/
/******************************   RUN LEVEL  3   ******************************/
/******************************************************************************/

#if RUN_LEVEL >=  3

#include "scn_test03.h"

rt_void test03(rt_cell opts)
{
    scene = new rt_Scene(&scn_test03::sc_root,
                        x_res, y_res, x_row, RT_NULL,
                        malloc, free,
                        RT_NULL, RT_NULL,
                        RT_NULL, RT_NULL);

    scene->set_opts(opts);
}

#endif /* RUN_LEVEL  3 */

/******************************************************************************/
/******************************   RUN LEVEL  4   ******************************/
/******************************************************************************/

#if RUN_LEVEL >=  4

#include "scn_test04.h"

rt_void test04(rt_cell opts)
{
    scene = new rt_Scene(&scn_test04::sc_root,
                        x_res, y_res, x_row, RT_NULL,
                        malloc, free,
                        RT_NULL, RT_NULL,
                        RT_NULL, RT_NULL);

    scene->set_opts(opts);
}

#endif /* RUN_LEVEL  4 */

/******************************************************************************/
/******************************   RUN LEVEL  5   ******************************/
/******************************************************************************/

#if RUN_LEVEL >=  5

#include "scn_test05.h"

rt_void test05(rt_cell opts)
{
    scene = new rt_Scene(&scn_test05::sc_root,
                        x_res, y_res, x_row, RT_NULL,
                        malloc, free,
                        RT_NULL, RT_NULL,
                        RT_NULL, RT_NULL);

    scene->set_opts(opts);
}

#endif /* RUN_LEVEL  5 */

/******************************************************************************/
/******************************   RUN LEVEL  6   ******************************/
/******************************************************************************/

#if RUN_LEVEL >=  6

#include "scn_test06.h"

rt_void test06(rt_cell opts)
{
    scene = new rt_Scene(&scn_test06::sc_root,
                        x_res, y_res, x_row, RT_NULL,
                        malloc, free,
                        RT_NULL, RT_NULL,
                        RT_NULL, RT_NULL);

    scene->set_opts(opts);
}

#endif /* RUN_LEVEL  6 */

/******************************************************************************/
/******************************   RUN LEVEL  7   ******************************/
/******************************************************************************/

#if RUN_LEVEL >=  7

#include "scn_test07.h"

rt_void test07(rt_cell opts)
{
    scene = new rt_Scene(&scn_test07::sc_root,
                        x_res, y_res, x_row, RT_NULL,
                        malloc, free,
                        RT_NULL, RT_NULL,
                        RT_NULL, RT_NULL);

    scene->set_opts(opts);
}

#endif /* RUN_LEVEL  7 */

/******************************************************************************/
/******************************   RUN LEVEL  8   ******************************/
/******************************************************************************/

#if RUN_LEVEL >=  8

#include "scn_test08.h"

rt_void test08(rt_cell opts)
{
    scene = new rt_Scene(&scn_test08::sc_root,
                        x_res, y_res, x_row, RT_NULL,
                        malloc, free,
                        RT_NULL, RT_NULL,
                        RT_NULL, RT_NULL);

    scene->set_opts(opts);
}

#endif /* RUN_LEVEL  8 */

/******************************************************************************/
/******************************   RUN LEVEL  9   ******************************/
/******************************************************************************/

#if RUN_LEVEL >=  9

#include "scn_test09.h"

rt_void test09(rt_cell opts)
{
    scene = new rt_Scene(&scn_test09::sc_root,
                        x_res, y_res, x_row, RT_NULL,
                        malloc, free,
                        RT_NULL, RT_NULL,
                        RT_NULL, RT_NULL);

    scene->set_opts(opts);
}

#endif /* RUN_LEVEL  9 */

/******************************************************************************/
/*********************************   TABLES   *********************************/
/******************************************************************************/

typedef rt_void (*testXX)(rt_cell);

testXX test[RUN_LEVEL] =
{
#if RUN_LEVEL >=  1
    test01,
#endif /* RUN_LEVEL  1 */

#if RUN_LEVEL >=  2
    test02,
#endif /* RUN_LEVEL  2 */

#if RUN_LEVEL >=  3
    test03,
#endif /* RUN_LEVEL  3 */

#if RUN_LEVEL >=  4
    test04,
#endif /* RUN_LEVEL  4 */

#if RUN_LEVEL >=  5
    test05,
#endif /* RUN_LEVEL  5 */

#if RUN_LEVEL >=  6
    test06,
#endif /* RUN_LEVEL  6 */

#if RUN_LEVEL >=  7
    test07,
#endif /* RUN_LEVEL  7 */

#if RUN_LEVEL >=  8
    test08,
#endif /* RUN_LEVEL  8 */

#if RUN_LEVEL >=  9
    test09,
#endif /* RUN_LEVEL  9 */
};

/******************************************************************************/
/**********************************   MAIN   **********************************/
/******************************************************************************/

rt_long get_time();

rt_cell main()
{
    rt_long time1 = 0;
    rt_long time2 = 0;
    rt_long tN = 0;
    rt_long tF = 0;

    rt_cell i, j;

    for (i = 0; i < RUN_LEVEL; i++)
    {
        RT_LOGI("-----------------  RUN LEVEL = %2d  -----------------\n", i+1);
        try
        {
            scene = RT_NULL;
            test[i](RT_OPTS_NONE);

            time1 = get_time();

            for (j = 0; j < CYC_SIZE; j++)
            {
                scene->render(j * 16);
            }

            time2 = get_time();
            tN = time2 - time1;
            RT_LOGI("Time N = %d\n", (rt_cell)tN);

            frame_cpy(frame, scene->get_frame());
            delete scene;

            scene = RT_NULL;
            test[i](RT_OPTS_FULL);

            time1 = get_time();

            for (j = 0; j < CYC_SIZE; j++)
            {
                scene->render(j * 16);
            }

            time2 = get_time();
            tF = time2 - time1;
            RT_LOGI("Time F = %d\n", (rt_cell)tF);

            frame_cmp(frame, scene->get_frame());
            delete scene;
        }
        catch (rt_Exception e)
        {
            RT_LOGE("Exception: %s\n", e.err);
        }
        RT_LOGI("----------------------------------------------------\n");
    }

#if   defined (WIN32) /* Win32, MSVC ---------------------------------------- */

    RT_LOGI("Type any letter and press ENTER to exit:");
    rt_char str[256]; /* not secure, do not inherit this practice */
    scanf("%s", str); /* not secure, do not inherit this practice */

#endif /* ------------- OS specific ----------------------------------------- */

    return 0;
}

#if   defined (WIN32) /* Win32, MSVC ---------------------------------------- */

#include <windows.h>

rt_long get_time()
{
    LARGE_INTEGER fr;
    QueryPerformanceFrequency(&fr);
    LARGE_INTEGER tm;
    QueryPerformanceCounter(&tm);
    return (rt_long)(tm.QuadPart * 1000 / fr.QuadPart);
}

#elif defined (linux) /* Linux, GCC ----------------------------------------- */

#include <sys/time.h>

rt_long get_time()
{
    timeval tm;
    gettimeofday(&tm, NULL);
    return (rt_long)(tm.tv_sec * 1000 + tm.tv_usec / 1000);
}

#endif /* ------------- OS specific ----------------------------------------- */

/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
<|endoftext|>


<commit_before>/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Rice University
*  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 Rice University 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 OWNER 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.
*********************************************************************/

/* Author: Ioan Sucan */

#include "ompl/base/GoalLazySamples.h"
#include "ompl/base/ScopedState.h"
#include "ompl/util/Time.h"

ompl::base::GoalLazySamples::GoalLazySamples(const SpaceInformationPtr &si, const GoalSamplingFn &samplerFunc, bool autoStart, double minDist) :
    GoalStates(si), samplerFunc_(samplerFunc), terminateSamplingThread_(false), samplingThread_(NULL), samplingAttempts_(0), minDist_(minDist)
{
    type_ = GOAL_LAZY_SAMPLES;
    if (autoStart)
        startSampling();
}

ompl::base::GoalLazySamples::~GoalLazySamples(void)
{
    stopSampling();
}

void ompl::base::GoalLazySamples::startSampling(void)
{
    if (samplingThread_ == NULL)
    {
        msg_.debug("Starting goal sampling thread");
        terminateSamplingThread_ = false;
        samplingThread_ = new boost::thread(&GoalLazySamples::goalSamplingThread, this);
    }
}

void ompl::base::GoalLazySamples::stopSampling(void)
{
    if (isSampling())
    {
        msg_.debug("Attempting to stop goal sampling thread...");
        terminateSamplingThread_ = true;
        samplingThread_->join();
        delete samplingThread_;
        samplingThread_ = NULL;
    }
    else
        if (samplingThread_)
        { // join a finished thread
            samplingThread_->join();
            delete samplingThread_;
            samplingThread_ = NULL;
        }
}

void ompl::base::GoalLazySamples::goalSamplingThread(void)
{
    if (!si_->isSetup())
    {
        msg_.debug("Waiting for space information to be set up before the sampling thread can begin computation...");
        // wait for everything to be set up before performing computation
        while (!terminateSamplingThread_ && !si_->isSetup())
            boost::this_thread::sleep(time::seconds(0.01));
    }

    if (!terminateSamplingThread_ && samplerFunc_)
    {
        msg_.debug("Beginning sampling thread computation");
        ScopedState<> s(si_);
        while (!terminateSamplingThread_ && samplerFunc_(this, s.get()))
        {
            ++samplingAttempts_;
            if (si_->satisfiesBounds(s.get()) && si_->isValid(s.get()))
                addStateIfDifferent(s.get(), minDist_);
        }
    }
    else
            msg_.warn("Goal sampling thread never did any work.%s",
                  samplerFunc_ ? (si_->isSetup() ? "" : " Space information not set up.") : " No sampling function set.");
    terminateSamplingThread_ = true;
    msg_.debug("Stopped goal sampling thread");
}

bool ompl::base::GoalLazySamples::isSampling(void) const
{
    return terminateSamplingThread_ == false && samplingThread_ != NULL;
}

bool ompl::base::GoalLazySamples::couldSample(void) const
{
    return canSample() || isSampling();
}

void ompl::base::GoalLazySamples::clear(void)
{
    boost::mutex::scoped_lock slock(lock_);
    GoalStates::clear();
}

double ompl::base::GoalLazySamples::distanceGoal(const State *st) const
{
    boost::mutex::scoped_lock slock(lock_);
    return GoalStates::distanceGoal(st);
}

void ompl::base::GoalLazySamples::sampleGoal(base::State *st) const
{
    boost::mutex::scoped_lock slock(lock_);
    GoalStates::sampleGoal(st);
}

void ompl::base::GoalLazySamples::setNewStateCallback(const NewStateCallbackFn &callback)
{
    callback_ = callback;
}

void ompl::base::GoalLazySamples::addState(const State* st)
{
    boost::mutex::scoped_lock slock(lock_);
    GoalStates::addState(st);
}

const ompl::base::State* ompl::base::GoalLazySamples::getState(unsigned int index) const
{
    boost::mutex::scoped_lock slock(lock_);
    return GoalStates::getState(index);
}

bool ompl::base::GoalLazySamples::hasStates(void) const
{
    boost::mutex::scoped_lock slock(lock_);
    return GoalStates::hasStates();
}

std::size_t ompl::base::GoalLazySamples::getStateCount(void) const
{
    boost::mutex::scoped_lock slock(lock_);
    return GoalStates::getStateCount();
}

bool ompl::base::GoalLazySamples::addStateIfDifferent(const State* st, double minDistance)
{
    const base::State *newState = NULL;
    bool added = false;
    {
        boost::mutex::scoped_lock slock(lock_);
        if (GoalStates::distanceGoal(st) > minDistance)
        {
            GoalStates::addState(st);
            added = true;
            if (callback_)
                newState = states_.back();
        }
    }

    // the lock is released at this; if needed, issue a call to the callback
    if (newState)
        callback_(newState);
    return added;
}
<commit_msg>report sampling attempts as debug msg<commit_after>/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Rice University
*  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 Rice University 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 OWNER 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.
*********************************************************************/

/* Author: Ioan Sucan */

#include "ompl/base/GoalLazySamples.h"
#include "ompl/base/ScopedState.h"
#include "ompl/util/Time.h"

ompl::base::GoalLazySamples::GoalLazySamples(const SpaceInformationPtr &si, const GoalSamplingFn &samplerFunc, bool autoStart, double minDist) :
    GoalStates(si), samplerFunc_(samplerFunc), terminateSamplingThread_(false), samplingThread_(NULL), samplingAttempts_(0), minDist_(minDist)
{
    type_ = GOAL_LAZY_SAMPLES;
    if (autoStart)
        startSampling();
}

ompl::base::GoalLazySamples::~GoalLazySamples(void)
{
    stopSampling();
}

void ompl::base::GoalLazySamples::startSampling(void)
{
    if (samplingThread_ == NULL)
    {
        msg_.debug("Starting goal sampling thread");
        terminateSamplingThread_ = false;
        samplingThread_ = new boost::thread(&GoalLazySamples::goalSamplingThread, this);
    }
}

void ompl::base::GoalLazySamples::stopSampling(void)
{
    if (isSampling())
    {
        msg_.debug("Attempting to stop goal sampling thread...");
        terminateSamplingThread_ = true;
        samplingThread_->join();
        delete samplingThread_;
        samplingThread_ = NULL;
    }
    else
        if (samplingThread_)
        { // join a finished thread
            samplingThread_->join();
            delete samplingThread_;
            samplingThread_ = NULL;
        }
}

void ompl::base::GoalLazySamples::goalSamplingThread(void)
{
    if (!si_->isSetup())
    {
        msg_.debug("Waiting for space information to be set up before the sampling thread can begin computation...");
        // wait for everything to be set up before performing computation
        while (!terminateSamplingThread_ && !si_->isSetup())
            boost::this_thread::sleep(time::seconds(0.01));
    }
    unsigned int prevsa = samplingAttempts_;
    if (!terminateSamplingThread_ && samplerFunc_)
    {
        msg_.debug("Beginning sampling thread computation");
        ScopedState<> s(si_);
        while (!terminateSamplingThread_ && samplerFunc_(this, s.get()))
        {
            ++samplingAttempts_;
            if (si_->satisfiesBounds(s.get()) && si_->isValid(s.get()))
                addStateIfDifferent(s.get(), minDist_);
        }
    }
    else
        msg_.warn("Goal sampling thread never did any work.%s",
                  samplerFunc_ ? (si_->isSetup() ? "" : " Space information not set up.") : " No sampling function set.");
    terminateSamplingThread_ = true;
    msg_.debug("Stopped goal sampling thread after %u sampling attempts", samplingAttempts_ - prevsa);
}

bool ompl::base::GoalLazySamples::isSampling(void) const
{
    return terminateSamplingThread_ == false && samplingThread_ != NULL;
}

bool ompl::base::GoalLazySamples::couldSample(void) const
{
    return canSample() || isSampling();
}

void ompl::base::GoalLazySamples::clear(void)
{
    boost::mutex::scoped_lock slock(lock_);
    GoalStates::clear();
}

double ompl::base::GoalLazySamples::distanceGoal(const State *st) const
{
    boost::mutex::scoped_lock slock(lock_);
    return GoalStates::distanceGoal(st);
}

void ompl::base::GoalLazySamples::sampleGoal(base::State *st) const
{
    boost::mutex::scoped_lock slock(lock_);
    GoalStates::sampleGoal(st);
}

void ompl::base::GoalLazySamples::setNewStateCallback(const NewStateCallbackFn &callback)
{
    callback_ = callback;
}

void ompl::base::GoalLazySamples::addState(const State* st)
{
    boost::mutex::scoped_lock slock(lock_);
    GoalStates::addState(st);
}

const ompl::base::State* ompl::base::GoalLazySamples::getState(unsigned int index) const
{
    boost::mutex::scoped_lock slock(lock_);
    return GoalStates::getState(index);
}

bool ompl::base::GoalLazySamples::hasStates(void) const
{
    boost::mutex::scoped_lock slock(lock_);
    return GoalStates::hasStates();
}

std::size_t ompl::base::GoalLazySamples::getStateCount(void) const
{
    boost::mutex::scoped_lock slock(lock_);
    return GoalStates::getStateCount();
}

bool ompl::base::GoalLazySamples::addStateIfDifferent(const State* st, double minDistance)
{
    const base::State *newState = NULL;
    bool added = false;
    {
        boost::mutex::scoped_lock slock(lock_);
        if (GoalStates::distanceGoal(st) > minDistance)
        {
            GoalStates::addState(st);
            added = true;
            if (callback_)
                newState = states_.back();
        }
    }

    // the lock is released at this; if needed, issue a call to the callback
    if (newState)
        callback_(newState);
    return added;
}
<|endoftext|>


<commit_before>#pragma once

#include <memory>
#include <systemd/sd-bus.h>
#include <sdbusplus/message.hpp>

namespace sdbusplus
{
namespace bus
{

using busp_t = sd_bus*;
class bus;

/** @brief Get an instance of the 'default' bus. */
bus new_default();
/** @brief Get an instance of the 'user' session bus. */
bus new_user();
/** @brief Get an instance of the 'system' bus. */
bus new_system();

namespace details
{

/** @brief unique_ptr functor to release a bus reference. */
struct BusDeleter
{
    void operator()(sd_bus* ptr) const
    {
        sd_bus_flush_close_unref(ptr);
    }
};

/* @brief Alias 'bus' to a unique_ptr type for auto-release. */
using bus = std::unique_ptr<sd_bus, BusDeleter>;

} // namespace details

/** @class bus
 *  @brief Provides C++ bindings to the sd_bus_* class functions.
 */
struct bus
{
        /* Define all of the basic class operations:
         *     Not allowed:
         *         - Default constructor to avoid nullptrs.
         *         - Copy operations due to internal unique_ptr.
         *     Allowed:
         *         - Move operations.
         *         - Destructor.
         */
    bus() = delete;
    bus(const bus&) = delete;
    bus& operator=(const bus&) = delete;
    bus(bus&&) = default;
    bus& operator=(bus&&) = default;
    ~bus() = default;

    /** @brief Conversion constructor from 'busp_t'.
     *
     *  Takes ownership of the bus-pointer and releases it when done.
     */
    explicit bus(busp_t b) : _bus(b) {}

    /** @brief Release ownership of the stored bus-pointer. */
    busp_t release() { return _bus.release(); }

    /** @brief Wait for new dbus messages or signals.
     *
     *  @param[in] timeout_us - Timeout in usec.
     */
    void wait(uint64_t timeout_us = 0)
    {
        sd_bus_wait(_bus.get(), timeout_us);
    }

    /** @brief Process waiting dbus messages or signals. */
    auto process()
    {
        sd_bus_message* m = nullptr;
        sd_bus_process(_bus.get(), &m);

        return message::message(m);
    }

    /** @brief Claim a service name on the dbus.
     *
     *  @param[in] service - The service name to claim.
     */
    void request_name(const char* service)
    {
        sd_bus_request_name(_bus.get(), service, 0);
    }

    /** @brief Create a method_call message.
     *
     *  @param[in] service - The service to call.
     *  @param[in] objpath - The object's path for the call.
     *  @param[in] interf - The object's interface to call.
     *  @param[in] method - The object's method to call.
     *
     *  @return A newly constructed message.
     */
    auto new_method_call(const char* service, const char* objpath,
                         const char* interf, const char* method)
    {
        sd_bus_message* m = nullptr;
        sd_bus_message_new_method_call(_bus.get(), &m, service, objpath,
                                       interf, method);

        return message::message(m);
    }

    /** @brief Perform a message call.
     *
     *  @param[in] m - The method_call message.
     *  @param[in] timeout_us - The timeout for the method call.
     *
     *  @return The response message.
     */
    auto call(message::message& m, uint64_t timeout_us = 0)
    {
        sd_bus_message* reply = nullptr;
        sd_bus_call(_bus.get(), m.get(), timeout_us, nullptr, &reply);

        return reply;
    }

    /** @brief Perform a message call, ignoring the reply.
     *
     *  @param[in] m - The method_call message.
     *  @param[in] timeout_us - The timeout for the method call.
     */
    void call_noreply(message::message& m, uint64_t timeout_us = 0)
    {
        sd_bus_call(_bus.get(), m.get(), timeout_us, nullptr, nullptr);
    }

    private:
        details::bus _bus;
};

bus new_default()
{
    sd_bus* b = nullptr;
    sd_bus_open(&b);
    return bus(b);
}

bus new_user()
{
    sd_bus* b = nullptr;
    sd_bus_open_user(&b);
    return bus(b);
}

bus new_system()
{
    sd_bus* b = nullptr;
    sd_bus_open_system(&b);
    return bus(b);
}

} // namespace bus

} // namespace sdbusplus
<commit_msg>bus: inline getters<commit_after>#pragma once

#include <memory>
#include <systemd/sd-bus.h>
#include <sdbusplus/message.hpp>

namespace sdbusplus
{
namespace bus
{

using busp_t = sd_bus*;
class bus;

/** @brief Get an instance of the 'default' bus. */
bus new_default();
/** @brief Get an instance of the 'user' session bus. */
bus new_user();
/** @brief Get an instance of the 'system' bus. */
bus new_system();

namespace details
{

/** @brief unique_ptr functor to release a bus reference. */
struct BusDeleter
{
    void operator()(sd_bus* ptr) const
    {
        sd_bus_flush_close_unref(ptr);
    }
};

/* @brief Alias 'bus' to a unique_ptr type for auto-release. */
using bus = std::unique_ptr<sd_bus, BusDeleter>;

} // namespace details

/** @class bus
 *  @brief Provides C++ bindings to the sd_bus_* class functions.
 */
struct bus
{
        /* Define all of the basic class operations:
         *     Not allowed:
         *         - Default constructor to avoid nullptrs.
         *         - Copy operations due to internal unique_ptr.
         *     Allowed:
         *         - Move operations.
         *         - Destructor.
         */
    bus() = delete;
    bus(const bus&) = delete;
    bus& operator=(const bus&) = delete;
    bus(bus&&) = default;
    bus& operator=(bus&&) = default;
    ~bus() = default;

    /** @brief Conversion constructor from 'busp_t'.
     *
     *  Takes ownership of the bus-pointer and releases it when done.
     */
    explicit bus(busp_t b) : _bus(b) {}

    /** @brief Release ownership of the stored bus-pointer. */
    busp_t release() { return _bus.release(); }

    /** @brief Wait for new dbus messages or signals.
     *
     *  @param[in] timeout_us - Timeout in usec.
     */
    void wait(uint64_t timeout_us = 0)
    {
        sd_bus_wait(_bus.get(), timeout_us);
    }

    /** @brief Process waiting dbus messages or signals. */
    auto process()
    {
        sd_bus_message* m = nullptr;
        sd_bus_process(_bus.get(), &m);

        return message::message(m);
    }

    /** @brief Claim a service name on the dbus.
     *
     *  @param[in] service - The service name to claim.
     */
    void request_name(const char* service)
    {
        sd_bus_request_name(_bus.get(), service, 0);
    }

    /** @brief Create a method_call message.
     *
     *  @param[in] service - The service to call.
     *  @param[in] objpath - The object's path for the call.
     *  @param[in] interf - The object's interface to call.
     *  @param[in] method - The object's method to call.
     *
     *  @return A newly constructed message.
     */
    auto new_method_call(const char* service, const char* objpath,
                         const char* interf, const char* method)
    {
        sd_bus_message* m = nullptr;
        sd_bus_message_new_method_call(_bus.get(), &m, service, objpath,
                                       interf, method);

        return message::message(m);
    }

    /** @brief Perform a message call.
     *
     *  @param[in] m - The method_call message.
     *  @param[in] timeout_us - The timeout for the method call.
     *
     *  @return The response message.
     */
    auto call(message::message& m, uint64_t timeout_us = 0)
    {
        sd_bus_message* reply = nullptr;
        sd_bus_call(_bus.get(), m.get(), timeout_us, nullptr, &reply);

        return reply;
    }

    /** @brief Perform a message call, ignoring the reply.
     *
     *  @param[in] m - The method_call message.
     *  @param[in] timeout_us - The timeout for the method call.
     */
    void call_noreply(message::message& m, uint64_t timeout_us = 0)
    {
        sd_bus_call(_bus.get(), m.get(), timeout_us, nullptr, nullptr);
    }

    private:
        details::bus _bus;
};

inline bus new_default()
{
    sd_bus* b = nullptr;
    sd_bus_open(&b);
    return bus(b);
}

inline bus new_user()
{
    sd_bus* b = nullptr;
    sd_bus_open_user(&b);
    return bus(b);
}

inline bus new_system()
{
    sd_bus* b = nullptr;
    sd_bus_open_system(&b);
    return bus(b);
}

} // namespace bus

} // namespace sdbusplus
<|endoftext|>


<commit_before><commit_msg>g++ 2.95 has no substring compare function, so use less efficient way that should work for everybody<commit_after><|endoftext|>


<commit_before>#ifndef AABOX_HPP
#define AABOX_HPP

#include <vector>

#include <glm/glm.hpp>

#include "Platform.hpp"

namespace Diggler {

///
/// @brief Axis-Aligned Bounding Box.
///
template<typename T = typename glm::mediump_vec3::value_type>
class AABB {
public:
  using Vec3 = glm::tvec3<T, glm::mediump>;

  Vec3 v1, v2;

  AABB(const Vec3 &vec1 = Vec3(), const Vec3 &vec2 = Vec3()) {
    set(vec1, vec2);
  }

  ///
  /// @brief Sets the AABB points.
  /// @param vec1 First point.
  /// @param vec2 Second point.
  /// @note Points coordinates are sorted so that each individual coordinate of vec2 is greater than vec1.
  ///
  void set(const Vec3 &vec1, const Vec3 &vec2) {
    v1 = vec1;
    v2 = vec2;
    if (v1.x > v2.x)
      std::swap(v1.x, v2.x);
    if (v1.y > v2.y)
      std::swap(v1.y, v2.y);
    if (v1.z > v2.z)
      std::swap(v1.z, v2.z);
  }

  ///
  /// @brief Checks if a point is in the AABB.
  /// @param point Point to check intersection with.
  /// @returns `true` if point is in AABB, `false` otherwise.
  ///
  bool intersects(const Vec3 &point) const {
    if (point.x > v1.x && point.x < v2.x &&
      point.y > v1.y && point.y < v2.y &&
      point.z > v1.z && point.z < v2.z)
      return true;
    return false;
  }

  ///
  /// @brief Checks if another AABB intersects this one.
  /// @param other AABB to check intersection with.
  /// @returns `true` if AABB intersects, `false` otherwise.
  ///
  bool intersects(const AABB<T> &other) const {
    bool xOverlap = !(other.v1.x > v2.x || other.v2.x < v1.x);
    bool yOverlap = !(other.v1.y > v2.y || other.v2.y < v1.y);
    bool zOverlap = !(other.v1.z > v2.z || other.v2.z < v1.z);
    return xOverlap && yOverlap && zOverlap;
  }

  ///
  /// @brief Does swept AABB collision computation.
  /// @param other Other AABB to check swept collsion with.
  /// @param vx,vy,vz Velocity.
  /// @param[out] nx,ny,nz Collision normal.
  ///
  float sweptCollision(AABB<T> other, float vx, float vy, float vz, float &nx, float &ny, float &nz) {
    T xInvEntry, yInvEntry, zInvEntry;
    T xInvExit, yInvExit, zInvExit;

    // find the distance between the objects on the near and far sides for both x and y
    if (vx > 0.0f) {
      xInvEntry = other.v1.x - v2.x;
      xInvExit =  other.v2.x - v1.x;
    } else {
      xInvEntry = other.v2.x - v1.x;
      xInvExit =  other.v1.x - v2.x;
    }

    if (vy > 0.0f) {
      yInvEntry = other.v1.y - v2.y;
      yInvExit =  other.v2.y - v1.y;
    } else {
      yInvEntry = other.v2.y - v1.y;
      yInvExit =  other.v1.y - v2.y;
    }

    if (vz > 0.0f) {
      zInvEntry = other.v1.z - v2.z;
      zInvExit =  other.v2.z - v1.z;
    } else {
      zInvEntry = other.v2.z - v1.z;
      zInvExit =  other.v1.z - v2.z;
    }

    // find time of collision and time of leaving for each axis (if statement is to prevent divide by zero)
    float xEntry, yEntry, zEntry;
    float xExit, yExit, zExit;

    if (vx == 0.0f) {
      xEntry = -std::numeric_limits<float>::infinity();
      xExit = std::numeric_limits<float>::infinity();
    } else {
      xEntry = xInvEntry / vx;
      xExit = xInvExit / vx;
    }

    if (vy == 0.0f) {
      yEntry = -std::numeric_limits<float>::infinity();
      yExit = std::numeric_limits<float>::infinity();
    } else {
      yEntry = yInvEntry / vy;
      yExit = yInvExit / vy;
    }

    if (vz == 0.0f) {
      zEntry = -std::numeric_limits<float>::infinity();
      zExit = std::numeric_limits<float>::infinity();
    } else {
      zEntry = zInvEntry / vz;
      zExit = zInvExit / vz;
    }

    // find the earliest/latest times of collision
    float entryTime = std::max(std::max(xEntry, yEntry), zEntry);
    float exitTime = std::min(std::min(xExit, yExit), zExit);

    if ( entryTime > exitTime ||
        (xEntry < 0.0f && yEntry < 0.0f && zEntry < 0.0f) ||
        (xEntry > 1.0f || yEntry > 1.0f || zEntry > 1.0f)) {
      // No collision
      nx = ny = nz = 0.0f;
      return 1.0f;
    } else { // O noes I haz hit!
      if (xEntry > yEntry && yEntry > zEntry) {
        ny = nz = 0.0f;
        nx = xInvEntry < 0.0f ? 1.0f : -1.0f;
      } else if (yEntry > zEntry) {
        nx = nz = 0.0f;
        ny = yInvEntry < 0.0f ? 1.0f : -1.0f;
      } else {
        nx = ny = 0.0f;
        nz = zInvEntry < 0.0f ? 1.0f : -1.0f;
      }

      // return the time of collision
      return entryTime;
    }
  }
};

extern template class AABB<>;

template<typename T = float>
class AABBVector : public std::vector<AABB<T>> {
public:
  using Vec3 = glm::tvec3<T, glm::mediump>;

  ///
  /// @brief Checks if a point is in the AABB vector.
  /// @param point Point to check intersection with.
  /// @returns `true` if point is in one AABB or more, `false` otherwise.
  ///
  bool intersects(const Vec3 &point) const {
    for (const AABB<T> &box : *this)
      if (box.intersects(point))
        return true;
    return false;
  }

  ///
  /// @brief Checks if an AABB intersects any of this vector.
  /// @param other AABB to check intersection with.
  /// @returns `true` if AABBs intersects, `false` otherwise.
  ///
  bool intersects(const AABB<T> &other) const {
    for (const AABB<T> &box : *this)
      if (box.intersects(other))
        return true;
    return false;
  }

  ///
  /// @brief Checks if an AABB vector intersects this one.
  /// @param other AABB vector to check intersection with.
  /// @returns `true` if AABBs intersects, `false` otherwise.
  ///
  bool intersects(const AABBVector<T> &other) const {
    for (const AABB<T> &obox : other)
      for (const AABB<T> &box : *this)
        if (box.intersects(obox))
          return true;
    return false;
  }
};

}

#endif<commit_msg>AABB: don't use glm detail types<commit_after>#ifndef AABOX_HPP
#define AABOX_HPP

#include <vector>

#include <glm/glm.hpp>

#include "Platform.hpp"

namespace Diggler {

///
/// @brief Axis-Aligned Bounding Box.
///
template<typename Vec3 = typename glm::vec3>
class AABB {
public:
  using T = typename Vec3::value_type;

  Vec3 v1, v2;

  AABB(const Vec3 &vec1 = Vec3(), const Vec3 &vec2 = Vec3()) {
    set(vec1, vec2);
  }

  ///
  /// @brief Sets the AABB points.
  /// @param vec1 First point.
  /// @param vec2 Second point.
  /// @note Points coordinates are sorted so that each individual coordinate of vec2 is greater than vec1.
  ///
  void set(const Vec3 &vec1, const Vec3 &vec2) {
    v1 = vec1;
    v2 = vec2;
    if (v1.x > v2.x)
      std::swap(v1.x, v2.x);
    if (v1.y > v2.y)
      std::swap(v1.y, v2.y);
    if (v1.z > v2.z)
      std::swap(v1.z, v2.z);
  }

  ///
  /// @brief Checks if a point is in the AABB.
  /// @param point Point to check intersection with.
  /// @returns `true` if point is in AABB, `false` otherwise.
  ///
  bool intersects(const Vec3 &point) const {
    if (point.x > v1.x && point.x < v2.x &&
      point.y > v1.y && point.y < v2.y &&
      point.z > v1.z && point.z < v2.z)
      return true;
    return false;
  }

  ///
  /// @brief Checks if another AABB intersects this one.
  /// @param other AABB to check intersection with.
  /// @returns `true` if AABB intersects, `false` otherwise.
  ///
  bool intersects(const AABB<Vec3> &other) const {
    bool xOverlap = !(other.v1.x > v2.x || other.v2.x < v1.x);
    bool yOverlap = !(other.v1.y > v2.y || other.v2.y < v1.y);
    bool zOverlap = !(other.v1.z > v2.z || other.v2.z < v1.z);
    return xOverlap && yOverlap && zOverlap;
  }

  ///
  /// @brief Does swept AABB collision computation.
  /// @param other Other AABB to check swept collsion with.
  /// @param vx,vy,vz Velocity.
  /// @param[out] nx,ny,nz Collision normal.
  ///
  float sweptCollision(AABB<Vec3> other, float vx, float vy, float vz, float &nx, float &ny, float &nz) {
    T xInvEntry, yInvEntry, zInvEntry;
    T xInvExit, yInvExit, zInvExit;

    // find the distance between the objects on the near and far sides for both x and y
    if (vx > 0.0f) {
      xInvEntry = other.v1.x - v2.x;
      xInvExit =  other.v2.x - v1.x;
    } else {
      xInvEntry = other.v2.x - v1.x;
      xInvExit =  other.v1.x - v2.x;
    }

    if (vy > 0.0f) {
      yInvEntry = other.v1.y - v2.y;
      yInvExit =  other.v2.y - v1.y;
    } else {
      yInvEntry = other.v2.y - v1.y;
      yInvExit =  other.v1.y - v2.y;
    }

    if (vz > 0.0f) {
      zInvEntry = other.v1.z - v2.z;
      zInvExit =  other.v2.z - v1.z;
    } else {
      zInvEntry = other.v2.z - v1.z;
      zInvExit =  other.v1.z - v2.z;
    }

    // find time of collision and time of leaving for each axis (if statement is to prevent divide by zero)
    float xEntry, yEntry, zEntry;
    float xExit, yExit, zExit;

    if (vx == 0.0f) {
      xEntry = -std::numeric_limits<float>::infinity();
      xExit = std::numeric_limits<float>::infinity();
    } else {
      xEntry = xInvEntry / vx;
      xExit = xInvExit / vx;
    }

    if (vy == 0.0f) {
      yEntry = -std::numeric_limits<float>::infinity();
      yExit = std::numeric_limits<float>::infinity();
    } else {
      yEntry = yInvEntry / vy;
      yExit = yInvExit / vy;
    }

    if (vz == 0.0f) {
      zEntry = -std::numeric_limits<float>::infinity();
      zExit = std::numeric_limits<float>::infinity();
    } else {
      zEntry = zInvEntry / vz;
      zExit = zInvExit / vz;
    }

    // find the earliest/latest times of collision
    float entryTime = std::max(std::max(xEntry, yEntry), zEntry);
    float exitTime = std::min(std::min(xExit, yExit), zExit);

    if ( entryTime > exitTime ||
        (xEntry < 0.0f && yEntry < 0.0f && zEntry < 0.0f) ||
        (xEntry > 1.0f || yEntry > 1.0f || zEntry > 1.0f)) {
      // No collision
      nx = ny = nz = 0.0f;
      return 1.0f;
    } else { // O noes I haz hit!
      if (xEntry > yEntry && yEntry > zEntry) {
        ny = nz = 0.0f;
        nx = xInvEntry < 0.0f ? 1.0f : -1.0f;
      } else if (yEntry > zEntry) {
        nx = nz = 0.0f;
        ny = yInvEntry < 0.0f ? 1.0f : -1.0f;
      } else {
        nx = ny = 0.0f;
        nz = zInvEntry < 0.0f ? 1.0f : -1.0f;
      }

      // return the time of collision
      return entryTime;
    }
  }
};

extern template class AABB<>;

template<typename Vec3 = typename glm::vec3>
class AABBVector : public std::vector<AABB<Vec3>> {
public:
  using T = typename Vec3::value_type;

  ///
  /// @brief Checks if a point is in the AABB vector.
  /// @param point Point to check intersection with.
  /// @returns `true` if point is in one AABB or more, `false` otherwise.
  ///
  bool intersects(const Vec3 &point) const {
    for (const AABB<Vec3> &box : *this)
      if (box.intersects(point))
        return true;
    return false;
  }

  ///
  /// @brief Checks if an AABB intersects any of this vector.
  /// @param other AABB to check intersection with.
  /// @returns `true` if AABBs intersects, `false` otherwise.
  ///
  bool intersects(const AABB<Vec3> &other) const {
    for (const AABB<Vec3> &box : *this)
      if (box.intersects(other))
        return true;
    return false;
  }

  ///
  /// @brief Checks if an AABB vector intersects this one.
  /// @param other AABB vector to check intersection with.
  /// @returns `true` if AABBs intersects, `false` otherwise.
  ///
  bool intersects(const AABBVector<Vec3> &other) const {
    for (const AABB<Vec3> &obox : other)
      for (const AABB<Vec3> &box : *this)
        if (box.intersects(obox))
          return true;
    return false;
  }
};

}

#endif
<|endoftext|>


<commit_before><commit_msg>renderSceneToFbo ok, warping ok<commit_after><|endoftext|>