<commit_before>/*
 * $Id$
 *
 * Copyright(C) 1998-2005 Satoshi Nakamura
 * All rights reserved.
 *
 */

#include <qmapplication.h>
#include <qmmessage.h>
#include <qmmessagewindow.h>

#include "actionid.h"
#include "resourceinc.h"
#include "statusbar.h"
#include "uiutil.h"
#include "../uimodel/encodingmodel.h"

using namespace qm;
using namespace qs;

/****************************************************************************
 *
 * StatusBar
 *
 */

qm::StatusBar::StatusBar() :
	WindowBase(true)
{
	setWindowHandler(this, false);
}

qm::StatusBar::~StatusBar()
{
}

int qm::StatusBar::getParts(int nParts,
							int* pnWidth) const
{
	return static_cast<int>(const_cast<StatusBar*>(this)->sendMessage(
		SB_GETPARTS, nParts, reinterpret_cast<LPARAM>(pnWidth)));
}

bool qm::StatusBar::setParts(int* pnWidth,
							 size_t nCount)
{
	return sendMessage(SB_SETPARTS, nCount, reinterpret_cast<LPARAM>(pnWidth)) != 0;
}

bool qm::StatusBar::setText(int nPart,
							const WCHAR* pwszText)
{
	W2T(pwszText, ptszText);
	return sendMessage(SB_SETTEXT, nPart, reinterpret_cast<LPARAM>(ptszText)) != 0;
}

#ifndef _WIN32_WCE
bool qm::StatusBar::setIcon(int nPart,
							HICON hIcon)
{
	return sendMessage(SB_SETICON, nPart, reinterpret_cast<LPARAM>(hIcon)) != 0;
}
#endif

void qm::StatusBar::setSimple(bool bSimple)
{
	sendMessage(SB_SIMPLE, bSimple);
}

bool qm::StatusBar::getRect(int nPart,
							RECT* pRect) const
{
	return const_cast<StatusBar*>(this)->sendMessage(
		SB_GETRECT, nPart, reinterpret_cast<LPARAM>(pRect)) != 0;
}

/****************************************************************************
 *
 * MessageStatusBar
 *
 */

qm::MessageStatusBar::MessageStatusBar(MessageWindow* pMessageWindow,
									   EncodingModel* pEncodingModel,
									   int nOffset,
									   MenuManager* pMenuManager) :
	pMessageWindow_(pMessageWindow),
	pEncodingModel_(pEncodingModel),
	nOffset_(nOffset),
	pMenuManager_(pMenuManager)
{
	assert(pMessageWindow);
	assert(pEncodingModel);
	assert(pMenuManager);

#ifdef _WIN32_WCE_PSPC
	nOffset_ -= 2;
#endif
}

qm::MessageStatusBar::~MessageStatusBar()
{
}

void qm::MessageStatusBar::updateMessageParts(MessageHolder* pmh,
											  Message& msg,
											  const ContentTypeParser* pContentType)
{
	if (pmh) {
#ifndef _WIN32_WCE_PSPC
		const WCHAR* pwszEncoding = pEncodingModel_->getEncoding();
		wstring_ptr wstrCharset;
		if (!pwszEncoding) {
			if (!pContentType) {
				if (msg.isMultipart()) {
					const Part::PartList& listPart = msg.getPartList();
					if (!listPart.empty())
						pContentType = listPart.front()->getContentType();
				}
				else {
					pContentType = msg.getContentType();
				}
			}
			if (pContentType)
				wstrCharset = pContentType->getParameter(L"charset");
			pwszEncoding = wstrCharset.get();
		}
		if (!pwszEncoding)
			pwszEncoding = L"us-ascii";
		setText(nOffset_ + 1, pwszEncoding);

		const WCHAR* pwszTemplate = pMessageWindow_->getTemplate();
		wstring_ptr wstrNone;
		if (pwszTemplate) {
			pwszTemplate += 5;
		}
		else {
			wstrNone = loadString(Application::getApplication().getResourceHandle(), IDS_STATUS_NONE);
			pwszTemplate = wstrNone.get();
		}
		setText(nOffset_ + 2, pwszTemplate);
#endif

		unsigned int nSecurity = msg.getSecurity();
		if (nSecurity & Message::SECURITY_DECRYPTED)
			setIconOrText(nOffset_ + 3, IDI_DECRYPTED, L"D");
		else
			setIconOrText(nOffset_ + 3, 0, L"");

		HICON hIconVerified = 0;
		if ((nSecurity & Message::SECURITY_VERIFICATIONFAILED) ||
			((nSecurity & Message::SECURITY_ADDRESSNOTMATCH) &&
			 !(nSecurity & Message::SECURITY_ADDRESSNOTMATCHNOERROR)))
			setIconOrText(nOffset_ + 4, IDI_UNVERIFIED, L"X");
		else if (nSecurity & Message::SECURITY_VERIFIED)
			setIconOrText(nOffset_ + 4, IDI_VERIFIED, L"V");
		else
			setIconOrText(nOffset_ + 4, 0, L"");
	}
	else {
#ifdef _WIN32_WCE_PSPC
		int nMax = 3;
#else
		int nMax = 5;
#endif
		for (int n = 1; n < nMax; ++n)
			setText(nOffset_ + n, L"");
	}
}

LRESULT qm::MessageStatusBar::windowProc(UINT uMsg,
										 WPARAM wParam,
										 LPARAM lParam)
{
	BEGIN_MESSAGE_HANDLER()
		HANDLE_CONTEXTMENU()
		HANDLE_LBUTTONDOWN()
	END_MESSAGE_HANDLER()
	return StatusBar::windowProc(uMsg, wParam, lParam);
}

LRESULT qm::MessageStatusBar::onContextMenu(HWND hwnd,
											const POINT& pt)
{
	POINT ptMenu = UIUtil::getContextMenuPosition(getHandle(), pt);
#ifndef _WIN32_WCE_PSPC
	POINT ptClient = pt;
	screenToClient(&ptClient);
	int nPart = getPart(ptClient);
	if (nPart != -1) {
		HMENU hmenu = pMenuManager_->getMenu(getMenuName(nPart), false, false);
		if (hmenu) {
			UINT nFlags = TPM_LEFTALIGN | TPM_TOPALIGN;
#ifndef _WIN32_WCE
			nFlags |= TPM_LEFTBUTTON | TPM_RIGHTBUTTON;
#endif
			::TrackPopupMenu(hmenu, nFlags, ptMenu.x, ptMenu.y, 0, getParentFrame(), 0);
		}
	}
#endif

	return 0;
}

LRESULT qm::MessageStatusBar::onLButtonDown(UINT nFlags,
											const POINT& pt)
{
	int nPart = getPart(pt);
	if (nPart != -1) {
		UINT nId = getClickActionId(nPart);
		if (nId != -1)
			Window(getParentFrame()).postMessage(WM_COMMAND, MAKEWPARAM(nId, 0), 0);
	}

	return StatusBar::onLButtonDown(nFlags, pt);
}

int qm::MessageStatusBar::getPart(const POINT& pt) const
{
	typedef std::vector<int> PartList;
	PartList listPart;
	listPart.resize(10);
	int nParts = getParts(static_cast<int>(listPart.size()), &listPart[0]);
	for (int n = 0; n < nParts; ++n) {
		RECT rect;
		getRect(n, &rect);
		if (::PtInRect(&rect, pt))
			return n;
	}
	return -1;
}

void qm::MessageStatusBar::setIconOrText(int nPart,
										 UINT nIcon,
										 const WCHAR* pwszText)
{
#ifndef _WIN32_WCE
	setIconId(nPart, nIcon);
#else
	setText(nPart, pwszText);
#endif
}

#ifndef _WIN32_WCE
void qm::MessageStatusBar::setIconId(int nPart,
									 UINT nIcon)
{
	HICON hIcon = 0;
	if (nIcon != 0)
		hIcon = reinterpret_cast<HICON>(::LoadImage(
			Application::getApplication().getResourceHandle(),
			MAKEINTRESOURCE(nIcon), IMAGE_ICON, 16, 16, LR_SHARED));
	setIcon(nPart, hIcon);
}
#endif

const WCHAR* qm::MessageStatusBar::getMenuName(int nPart)
{
	if (nPart == nOffset_ + 1)
		return L"encoding";
	else if (nPart == nOffset_ + 2)
		return L"template";
	else
		return 0;
}

UINT qm::MessageStatusBar::getClickActionId(int nPart)
{
	if (nPart == nOffset_ + 4)
		return IDM_MESSAGE_CERTIFICATE;
	else
		return -1;
}
<commit_msg>Fix Pocket PC versions don't compile.<commit_after>/*
 * $Id$
 *
 * Copyright(C) 1998-2005 Satoshi Nakamura
 * All rights reserved.
 *
 */

#include <qmapplication.h>
#include <qmmessage.h>
#include <qmmessagewindow.h>

#include "actionid.h"
#include "resourceinc.h"
#include "statusbar.h"
#include "uiutil.h"
#include "../uimodel/encodingmodel.h"

using namespace qm;
using namespace qs;

/****************************************************************************
 *
 * StatusBar
 *
 */

qm::StatusBar::StatusBar() :
	WindowBase(true)
{
	setWindowHandler(this, false);
}

qm::StatusBar::~StatusBar()
{
}

int qm::StatusBar::getParts(int nParts,
							int* pnWidth) const
{
	return static_cast<int>(const_cast<StatusBar*>(this)->sendMessage(
		SB_GETPARTS, nParts, reinterpret_cast<LPARAM>(pnWidth)));
}

bool qm::StatusBar::setParts(int* pnWidth,
							 size_t nCount)
{
	return sendMessage(SB_SETPARTS, nCount, reinterpret_cast<LPARAM>(pnWidth)) != 0;
}

bool qm::StatusBar::setText(int nPart,
							const WCHAR* pwszText)
{
	W2T(pwszText, ptszText);
	return sendMessage(SB_SETTEXT, nPart, reinterpret_cast<LPARAM>(ptszText)) != 0;
}

#ifndef _WIN32_WCE
bool qm::StatusBar::setIcon(int nPart,
							HICON hIcon)
{
	return sendMessage(SB_SETICON, nPart, reinterpret_cast<LPARAM>(hIcon)) != 0;
}
#endif

void qm::StatusBar::setSimple(bool bSimple)
{
	sendMessage(SB_SIMPLE, bSimple);
}

bool qm::StatusBar::getRect(int nPart,
							RECT* pRect) const
{
	return const_cast<StatusBar*>(this)->sendMessage(
		SB_GETRECT, nPart, reinterpret_cast<LPARAM>(pRect)) != 0;
}

/****************************************************************************
 *
 * MessageStatusBar
 *
 */

qm::MessageStatusBar::MessageStatusBar(MessageWindow* pMessageWindow,
									   EncodingModel* pEncodingModel,
									   int nOffset,
									   MenuManager* pMenuManager) :
	pMessageWindow_(pMessageWindow),
	pEncodingModel_(pEncodingModel),
	nOffset_(nOffset),
	pMenuManager_(pMenuManager)
{
	assert(pMessageWindow);
	assert(pEncodingModel);
	assert(pMenuManager);

#ifdef _WIN32_WCE_PSPC
	nOffset_ -= 2;
#endif
}

qm::MessageStatusBar::~MessageStatusBar()
{
}

void qm::MessageStatusBar::updateMessageParts(MessageHolder* pmh,
											  Message& msg,
											  const ContentTypeParser* pContentType)
{
	if (pmh) {
#ifndef _WIN32_WCE_PSPC
		const WCHAR* pwszEncoding = pEncodingModel_->getEncoding();
		wstring_ptr wstrCharset;
		if (!pwszEncoding) {
			if (!pContentType) {
				if (msg.isMultipart()) {
					const Part::PartList& listPart = msg.getPartList();
					if (!listPart.empty())
						pContentType = listPart.front()->getContentType();
				}
				else {
					pContentType = msg.getContentType();
				}
			}
			if (pContentType)
				wstrCharset = pContentType->getParameter(L"charset");
			pwszEncoding = wstrCharset.get();
		}
		if (!pwszEncoding)
			pwszEncoding = L"us-ascii";
		setText(nOffset_ + 1, pwszEncoding);

		const WCHAR* pwszTemplate = pMessageWindow_->getTemplate();
		wstring_ptr wstrNone;
		if (pwszTemplate) {
			pwszTemplate += 5;
		}
		else {
			wstrNone = loadString(Application::getApplication().getResourceHandle(), IDS_STATUS_NONE);
			pwszTemplate = wstrNone.get();
		}
		setText(nOffset_ + 2, pwszTemplate);
#endif

		unsigned int nSecurity = msg.getSecurity();
		if (nSecurity & Message::SECURITY_DECRYPTED)
			setIconOrText(nOffset_ + 3, IDI_DECRYPTED, L"D");
		else
			setIconOrText(nOffset_ + 3, 0, L"");

		HICON hIconVerified = 0;
		if ((nSecurity & Message::SECURITY_VERIFICATIONFAILED) ||
			((nSecurity & Message::SECURITY_ADDRESSNOTMATCH) &&
			 !(nSecurity & Message::SECURITY_ADDRESSNOTMATCHNOERROR)))
			setIconOrText(nOffset_ + 4, IDI_UNVERIFIED, L"X");
		else if (nSecurity & Message::SECURITY_VERIFIED)
			setIconOrText(nOffset_ + 4, IDI_VERIFIED, L"V");
		else
			setIconOrText(nOffset_ + 4, 0, L"");
	}
	else {
#ifdef _WIN32_WCE_PSPC
		int nMax = 3;
#else
		int nMax = 5;
#endif
		for (int n = 1; n < nMax; ++n)
			setText(nOffset_ + n, L"");
	}
}

LRESULT qm::MessageStatusBar::windowProc(UINT uMsg,
										 WPARAM wParam,
										 LPARAM lParam)
{
	BEGIN_MESSAGE_HANDLER()
		HANDLE_CONTEXTMENU()
		HANDLE_LBUTTONDOWN()
	END_MESSAGE_HANDLER()
	return StatusBar::windowProc(uMsg, wParam, lParam);
}

LRESULT qm::MessageStatusBar::onContextMenu(HWND hwnd,
											const POINT& pt)
{
#ifndef _WIN32_WCE_PSPC
	POINT ptMenu = UIUtil::getContextMenuPosition(getHandle(), pt);
	POINT ptClient = pt;
	screenToClient(&ptClient);
	int nPart = getPart(ptClient);
	if (nPart != -1) {
		HMENU hmenu = pMenuManager_->getMenu(getMenuName(nPart), false, false);
		if (hmenu) {
			UINT nFlags = TPM_LEFTALIGN | TPM_TOPALIGN;
#ifndef _WIN32_WCE
			nFlags |= TPM_LEFTBUTTON | TPM_RIGHTBUTTON;
#endif
			::TrackPopupMenu(hmenu, nFlags, ptMenu.x, ptMenu.y, 0, getParentFrame(), 0);
		}
	}
#endif

	return 0;
}

LRESULT qm::MessageStatusBar::onLButtonDown(UINT nFlags,
											const POINT& pt)
{
	int nPart = getPart(pt);
	if (nPart != -1) {
		UINT nId = getClickActionId(nPart);
		if (nId != -1)
			Window(getParentFrame()).postMessage(WM_COMMAND, MAKEWPARAM(nId, 0), 0);
	}

	return StatusBar::onLButtonDown(nFlags, pt);
}

int qm::MessageStatusBar::getPart(const POINT& pt) const
{
	typedef std::vector<int> PartList;
	PartList listPart;
	listPart.resize(10);
	int nParts = getParts(static_cast<int>(listPart.size()), &listPart[0]);
	for (int n = 0; n < nParts; ++n) {
		RECT rect;
		getRect(n, &rect);
		if (::PtInRect(&rect, pt))
			return n;
	}
	return -1;
}

void qm::MessageStatusBar::setIconOrText(int nPart,
										 UINT nIcon,
										 const WCHAR* pwszText)
{
#ifndef _WIN32_WCE
	setIconId(nPart, nIcon);
#else
	setText(nPart, pwszText);
#endif
}

#ifndef _WIN32_WCE
void qm::MessageStatusBar::setIconId(int nPart,
									 UINT nIcon)
{
	HICON hIcon = 0;
	if (nIcon != 0)
		hIcon = reinterpret_cast<HICON>(::LoadImage(
			Application::getApplication().getResourceHandle(),
			MAKEINTRESOURCE(nIcon), IMAGE_ICON, 16, 16, LR_SHARED));
	setIcon(nPart, hIcon);
}
#endif

const WCHAR* qm::MessageStatusBar::getMenuName(int nPart)
{
	if (nPart == nOffset_ + 1)
		return L"encoding";
	else if (nPart == nOffset_ + 2)
		return L"template";
	else
		return 0;
}

UINT qm::MessageStatusBar::getClickActionId(int nPart)
{
	if (nPart == nOffset_ + 4)
		return IDM_MESSAGE_CERTIFICATE;
	else
		return -1;
}
<|endoftext|>


<commit_before>/*
Copyright (c) 2010-2014, 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 <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>

#include <tf/transform_listener.h>

#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/CameraInfo.h>
#include <stereo_msgs/DisparityImage.h>

#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>

#include <image_geometry/pinhole_camera_model.h>

#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/subscriber.h>

#include <cv_bridge/cv_bridge.h>
#include <opencv2/highgui/highgui.hpp>

#include <rtabmap_ros/MsgConversion.h>

#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_filtering.h"
#include "rtabmap/core/util3d_mapping.h"
#include "rtabmap/core/util3d_transforms.h"

namespace rtabmap_ros
{

class ObstaclesDetection : public nodelet::Nodelet
{
public:
	ObstaclesDetection() :
		frameId_("base_link"),
		normalEstimationRadius_(0.05),
		groundNormalAngle_(M_PI_4),
		minClusterSize_(20),
		maxFloorHeight_(-1),
		maxObstaclesHeight_(0),
		waitForTransform_(false)
	{}

	virtual ~ObstaclesDetection()
	{}

private:
	virtual void onInit()
	{
		ros::NodeHandle & nh = getNodeHandle();
		ros::NodeHandle & pnh = getPrivateNodeHandle();

		int queueSize = 10;
		pnh.param("queue_size", queueSize, queueSize);
		pnh.param("frame_id", frameId_, frameId_);
		pnh.param("normal_estimation_radius", normalEstimationRadius_, normalEstimationRadius_);
		pnh.param("ground_normal_angle", groundNormalAngle_, groundNormalAngle_);
		pnh.param("min_cluster_size", minClusterSize_, minClusterSize_);
		pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_);
		pnh.param("max_floor_height", maxFloorHeight_, maxFloorHeight_);
		pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);

		cloudSub_ = nh.subscribe("cloud", 1, &ObstaclesDetection::callback, this);

		groundPub_ = nh.advertise<sensor_msgs::PointCloud2>("ground", 1);
		obstaclesPub_ = nh.advertise<sensor_msgs::PointCloud2>("obstacles", 1);

		this->_lastFrameTime = ros::Time::now();
	}

	void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg)
	{
		if (groundPub_.getNumSubscribers() == 0 && obstaclesPub_.getNumSubscribers() == 0)
		{
			// no one wants the results
			return;
		}

		rtabmap::Transform localTransform;
		try
		{
			if(waitForTransform_)
			{
				if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1)))
				{
					ROS_ERROR("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str());
					return;
				}
			}
			tf::StampedTransform tmp;
			tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp);
			localTransform = rtabmap_ros::transformFromTF(tmp);
		}
		catch(tf::TransformException & ex)
		{
			ROS_ERROR("%s",ex.what());
			return;
		}

		pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::fromROSMsg(*cloudMsg, *originalCloud);
		if(originalCloud->size() == 0)
		{
			ROS_ERROR("Recieved empty point cloud!");
			return;
		}
		originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform);

		/////////////////////////////////////////////////////////////////////////////

		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::copyPointCloud(*originalCloud, *cloud);

		if(maxObstaclesHeight_ > 0)
		{
			cloud = rtabmap::util3d::passThrough(cloud, "z", std::numeric_limits<int>::min(), maxObstaclesHeight_);
		}

		ros::Time lasttime = ros::Time::now();

		pcl::IndicesPtr ground, obstacles;
		rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(cloud,
				ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_);

		ros::Time curtime = ros::Time::now();
		ros::Duration process_duration = curtime - lasttime;
		ros::Duration between_frames = curtime - this->_lastFrameTime;
		this->_lastFrameTime = curtime;
		std::stringstream buffer;
		buffer << "acloudsize=" << cloud->size() << " t=" << process_duration.toSec() << "s; " << (1./between_frames.toSec()) << "Hz";
		ROS_ERROR("3%s: %s", this->getName().c_str(), buffer.str().c_str());

		pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
		if(ground.get() && ground->size())
		{
			pcl::copyPointCloud(*cloud, *ground, *groundCloud);
		}

		pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
		if(obstaclesPub_.getNumSubscribers() && obstacles.get() && obstacles->size())
		{
			pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
		}

		if(maxFloorHeight_ > 0)
		{
			pcl::PointCloud<pcl::PointXYZ>::Ptr flatObstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
			flatObstaclesCloud = rtabmap::util3d::passThrough(groundCloud, "z", maxFloorHeight_, std::numeric_limits<int>::max());
			*obstaclesCloud += *flatObstaclesCloud;

			groundCloud = rtabmap::util3d::passThrough(groundCloud, "z", std::numeric_limits<int>::min(), maxFloorHeight_);
		}

		if(groundPub_.getNumSubscribers())
		{
			sensor_msgs::PointCloud2 rosCloud;
			pcl::toROSMsg(*groundCloud, rosCloud);
			rosCloud.header.stamp = cloudMsg->header.stamp;
			rosCloud.header.frame_id = frameId_;

			//publish the message
			groundPub_.publish(rosCloud);
		}

		if(obstaclesPub_.getNumSubscribers())
		{
			sensor_msgs::PointCloud2 rosCloud;
			pcl::toROSMsg(*obstaclesCloud, rosCloud);
			rosCloud.header.stamp = cloudMsg->header.stamp;
			rosCloud.header.frame_id = frameId_;

			//publish the message
			obstaclesPub_.publish(rosCloud);
		}

	}

private:
	std::string frameId_;
	double normalEstimationRadius_;
	double groundNormalAngle_;
	int minClusterSize_;
	double maxObstaclesHeight_;
	double maxFloorHeight_;
	bool waitForTransform_;

	tf::TransformListener tfListener_;

	ros::Publisher groundPub_;
	ros::Publisher obstaclesPub_;

	ros::Subscriber cloudSub_;
	ros::Time _lastFrameTime;
};

PLUGINLIB_EXPORT_CLASS(rtabmap_ros::ObstaclesDetection, nodelet::Nodelet);
}

<commit_msg>print sizes<commit_after>/*
Copyright (c) 2010-2014, 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 <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>

#include <tf/transform_listener.h>

#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/CameraInfo.h>
#include <stereo_msgs/DisparityImage.h>

#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>

#include <image_geometry/pinhole_camera_model.h>

#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/subscriber.h>

#include <cv_bridge/cv_bridge.h>
#include <opencv2/highgui/highgui.hpp>

#include <rtabmap_ros/MsgConversion.h>

#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_filtering.h"
#include "rtabmap/core/util3d_mapping.h"
#include "rtabmap/core/util3d_transforms.h"

namespace rtabmap_ros
{

class ObstaclesDetection : public nodelet::Nodelet
{
public:
	ObstaclesDetection() :
		frameId_("base_link"),
		normalEstimationRadius_(0.05),
		groundNormalAngle_(M_PI_4),
		minClusterSize_(20),
		maxFloorHeight_(-1),
		maxObstaclesHeight_(0),
		waitForTransform_(false)
	{}

	virtual ~ObstaclesDetection()
	{}

private:
	virtual void onInit()
	{
		ros::NodeHandle & nh = getNodeHandle();
		ros::NodeHandle & pnh = getPrivateNodeHandle();

		int queueSize = 10;
		pnh.param("queue_size", queueSize, queueSize);
		pnh.param("frame_id", frameId_, frameId_);
		pnh.param("normal_estimation_radius", normalEstimationRadius_, normalEstimationRadius_);
		pnh.param("ground_normal_angle", groundNormalAngle_, groundNormalAngle_);
		pnh.param("min_cluster_size", minClusterSize_, minClusterSize_);
		pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_);
		pnh.param("max_floor_height", maxFloorHeight_, maxFloorHeight_);
		pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);

		cloudSub_ = nh.subscribe("cloud", 1, &ObstaclesDetection::callback, this);

		groundPub_ = nh.advertise<sensor_msgs::PointCloud2>("ground", 1);
		obstaclesPub_ = nh.advertise<sensor_msgs::PointCloud2>("obstacles", 1);

		this->_lastFrameTime = ros::Time::now();
	}

	void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg)
	{
		if (groundPub_.getNumSubscribers() == 0 && obstaclesPub_.getNumSubscribers() == 0)
		{
			// no one wants the results
			return;
		}

		rtabmap::Transform localTransform;
		try
		{
			if(waitForTransform_)
			{
				if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1)))
				{
					ROS_ERROR("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str());
					return;
				}
			}
			tf::StampedTransform tmp;
			tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp);
			localTransform = rtabmap_ros::transformFromTF(tmp);
		}
		catch(tf::TransformException & ex)
		{
			ROS_ERROR("%s",ex.what());
			return;
		}

		pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::fromROSMsg(*cloudMsg, *originalCloud);
		if(originalCloud->size() == 0)
		{
			ROS_ERROR("Recieved empty point cloud!");
			return;
		}
		originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform);

		/////////////////////////////////////////////////////////////////////////////

		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::copyPointCloud(*originalCloud, *cloud);

		if(maxObstaclesHeight_ > 0)
		{
			cloud = rtabmap::util3d::passThrough(cloud, "z", std::numeric_limits<int>::min(), maxObstaclesHeight_);
		}

		ros::Time lasttime = ros::Time::now();

		pcl::IndicesPtr ground, obstacles;
		rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(cloud,
				ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_);

		ros::Time curtime = ros::Time::now();
		ros::Duration process_duration = curtime - lasttime;
		ros::Duration between_frames = curtime - this->_lastFrameTime;
		this->_lastFrameTime = curtime;

		pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
		if(ground.get() && ground->size())
		{
			pcl::copyPointCloud(*cloud, *ground, *groundCloud);
		}

		pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
		if(obstaclesPub_.getNumSubscribers() && obstacles.get() && obstacles->size())
		{
			pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
		}

		std::stringstream buffer;
		buffer << "cloud=" << cloud->size() << " floor=" << ground->size() << " obst=" << obstacles->size();
		buffer << " t=" << process_duration.toSec() << "s; " << (1./between_frames.toSec()) << "Hz";
		ROS_ERROR("3%s: %s", this->getName().c_str(), buffer.str().c_str());

//		if(maxFloorHeight_ > 0)
//		{
//			pcl::PointCloud<pcl::PointXYZ>::Ptr flatObstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
//			flatObstaclesCloud = rtabmap::util3d::passThrough(groundCloud, "z", maxFloorHeight_, std::numeric_limits<int>::max());
//			*obstaclesCloud += *flatObstaclesCloud;
//
//			groundCloud = rtabmap::util3d::passThrough(groundCloud, "z", std::numeric_limits<int>::min(), maxFloorHeight_);
//		}

		if(groundPub_.getNumSubscribers())
		{
			sensor_msgs::PointCloud2 rosCloud;
			pcl::toROSMsg(*groundCloud, rosCloud);
			rosCloud.header.stamp = cloudMsg->header.stamp;
			rosCloud.header.frame_id = frameId_;

			//publish the message
			groundPub_.publish(rosCloud);
		}

		if(obstaclesPub_.getNumSubscribers())
		{
			sensor_msgs::PointCloud2 rosCloud;
			pcl::toROSMsg(*obstaclesCloud, rosCloud);
			rosCloud.header.stamp = cloudMsg->header.stamp;
			rosCloud.header.frame_id = frameId_;

			//publish the message
			obstaclesPub_.publish(rosCloud);
		}

	}

private:
	std::string frameId_;
	double normalEstimationRadius_;
	double groundNormalAngle_;
	int minClusterSize_;
	double maxObstaclesHeight_;
	double maxFloorHeight_;
	bool waitForTransform_;

	tf::TransformListener tfListener_;

	ros::Publisher groundPub_;
	ros::Publisher obstaclesPub_;

	ros::Subscriber cloudSub_;
	ros::Time _lastFrameTime;
};

PLUGINLIB_EXPORT_CLASS(rtabmap_ros::ObstaclesDetection, nodelet::Nodelet);
}

<|endoftext|>


<commit_before>/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield
 *
 * This library is open source and may be redistributed and/or modified under
 * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
 * (at your option) any later version.  The full license is in LICENSE file
 * included with this distribution, and on the openscenegraph.org website.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * OpenSceneGraph Public License for more details.
*/

#include <osgTerrain/GeometryTechnique>
#include <osgTerrain/TerrainNode>

#include <osgUtil/SmoothingVisitor>

#include <osgDB/FileUtils>

#include <osg/io_utils>
#include <osg/Texture2D>
#include <osg/Texture1D>
#include <osg/TexEnvCombine>
#include <osg/Program>

using namespace osgTerrain;

GeometryTechnique::GeometryTechnique()
{
}

GeometryTechnique::GeometryTechnique(const GeometryTechnique& gt,const osg::CopyOp& copyop):
    TerrainTechnique(gt,copyop)
{
}

GeometryTechnique::~GeometryTechnique()
{
}

void GeometryTechnique::init()
{
    osg::notify(osg::NOTICE)<<"Doing init()"<<std::endl;

    if (!_terrainNode) return;

    osgTerrain::Layer* elevationLayer = _terrainNode->getElevationLayer();
    osgTerrain::Layer* colorLayer = _terrainNode->getColorLayer(0);
    osg::TransferFunction* colorTF = _terrainNode->getColorTransferFunction(0);

    // if the elevationLayer and colorLayer are the same, and there is colorTF then
    // simply assing as a texture coordinate.
    if ((elevationLayer==colorLayer) && colorTF) colorLayer = 0;

    osg::notify(osg::NOTICE)<<"elevationLayer = "<<elevationLayer<<std::endl;
    osg::notify(osg::NOTICE)<<"colorLayer = "<<colorLayer<<std::endl;
    osg::notify(osg::NOTICE)<<"colorTF = "<<colorTF<<std::endl;

    Locator* elevationLocator = elevationLayer ? elevationLayer->getLocator() : 0;
    Locator* colorLocator = colorLayer ? colorLayer->getLocator() : 0;

    Locator* masterLocator = elevationLocator ? elevationLocator : colorLocator;
    if (!masterLocator)
    {
        osg::notify(osg::NOTICE)<<"Problem, no locator found in any of the terrain layers"<<std::endl;
        return;
    }

    if (!elevationLocator) elevationLocator = masterLocator;
    if (!colorLocator) colorLocator = masterLocator;

    osg::Vec3d bottomLeftNDC(DBL_MAX, DBL_MAX, 0.0);
    osg::Vec3d topRightNDC(-DBL_MAX, -DBL_MAX, 0.0);

    if (elevationLayer)
    {
        if (elevationLocator!= masterLocator)
        {
            masterLocator->computeLocalBounds(*elevationLocator, bottomLeftNDC, topRightNDC);
        }
        else
        {
            bottomLeftNDC.x() = osg::minimum(bottomLeftNDC.x(), 0.0);
            bottomLeftNDC.y() = osg::minimum(bottomLeftNDC.y(), 0.0);
            topRightNDC.x() = osg::maximum(topRightNDC.x(), 1.0);
            topRightNDC.y() = osg::maximum(topRightNDC.y(), 1.0);
        }
    }

    if (colorLayer)
    {
        if (colorLocator!= masterLocator)
        {
            masterLocator->computeLocalBounds(*colorLocator, bottomLeftNDC, topRightNDC);
        }
        else
        {
            bottomLeftNDC.x() = osg::minimum(bottomLeftNDC.x(), 0.0);
            bottomLeftNDC.y() = osg::minimum(bottomLeftNDC.y(), 0.0);
            topRightNDC.x() = osg::maximum(topRightNDC.x(), 1.0);
            topRightNDC.y() = osg::maximum(topRightNDC.y(), 1.0);
        }
    }

    osg::notify(osg::NOTICE)<<"bottomLeftNDC = "<<bottomLeftNDC<<std::endl;
    osg::notify(osg::NOTICE)<<"topRightNDC = "<<topRightNDC<<std::endl;

    _geode = new osg::Geode;
    _geometry = new osg::Geometry;
    _geode->addDrawable(_geometry.get());
    _geode->addDrawable(new osgTerrain::TerrainGeometry);

    unsigned int numRows = 100;
    unsigned int numColumns = 100;

    if (elevationLayer)
    {
        numColumns = elevationLayer->getNumColumns();
        numRows = elevationLayer->getNumRows();
    }

    unsigned int numVertices = numRows * numColumns;

    // allocate and assign vertices
    osg::Vec3Array* _vertices = new osg::Vec3Array(numVertices);
    _geometry->setVertexArray(_vertices);

    // allocate and assign normals
    osg::Vec3Array* _normals = new osg::Vec3Array(numVertices);
    _geometry->setNormalArray(_normals);
    _geometry->setNormalBinding(osg::Geometry::BIND_PER_VERTEX);

    int texcoord_index = 0;
    int color_index = -1;
    int tf_index = -1;

    float minHeight = 0.0;
    float scaleHeight = 1.0;

    // allocate and assign tex coords
    osg::Vec2Array* _texcoords = 0;
    if (colorLayer)
    {
        color_index = texcoord_index;
        ++texcoord_index;

        _texcoords = new osg::Vec2Array(numVertices);
        _geometry->setTexCoordArray(color_index, _texcoords);
    }

    osg::FloatArray* _elevations = 0;
    osg::TransferFunction1D* tf = dynamic_cast<osg::TransferFunction1D*>(colorTF);
    if (tf)
    {
        tf_index = texcoord_index;
        ++texcoord_index;

        if (!colorLayer)
        {
            _elevations = new osg::FloatArray(numVertices);
            _geometry->setTexCoordArray(tf_index, _elevations);

            minHeight = tf->getMinimum();
            scaleHeight = 1.0f/(tf->getMaximum()-tf->getMinimum());
        }
    }

    // allocate and assign color
    osg::Vec4Array* _colors = new osg::Vec4Array(1);
    _geometry->setColorArray(_colors);
    _geometry->setColorBinding(osg::Geometry::BIND_OVERALL);
    (*_colors)[0].set(1.0f,1.0f,1.0f,1.0f);

    // populate vertex and tex coord arrays
    unsigned int j;
    for(j=0; j<numRows; ++j)
    {
        for(unsigned int i=0; i<numColumns; ++i)
        {
            unsigned int iv = j*numColumns + i;
            osg::Vec3d ndc( ((double)i)/(double)(numColumns-1), ((double)j)/(double)(numRows-1), 0.0);

            if (elevationLayer)
            {
                float value = 0.0f;
                elevationLayer->getValue(i,j, value);
                // osg::notify(osg::NOTICE)<<"i="<<i<<" j="<<j<<" z="<<value<<std::endl;
                ndc.z() = value;
            }

            osg::Vec3d model;
            masterLocator->convertLocalToModel(ndc, model);

            (*_vertices)[iv] = model;

            if (colorLayer)
            {
                if (colorLocator!= masterLocator)
                {
                    osg::Vec3d color_ndc;
                    Locator::convertLocalCoordBetween(*masterLocator, ndc, *colorLocator, color_ndc);
                    (*_texcoords)[iv].set(color_ndc.x(), color_ndc.y());
                }
                else
                {
                    (*_texcoords)[iv].set(ndc.x(), ndc.y());
                }

            }

            if (_elevations)
            {
                (*_elevations)[iv] = (ndc.z()-minHeight)*scaleHeight;
            }

            // compute the local normal
            osg::Vec3d ndc_one( (double)i/(double)(numColumns-1), (double)j/(double)(numColumns-1), 1.0);
            osg::Vec3d model_one;
            masterLocator->convertLocalToModel(ndc_one, model_one);
            model_one -= model;
            model_one.normalize();
            (*_normals)[iv] = model_one;
        }
    }

    // populate primitive sets
    // _primitiveSets.clear();
    for(j=0; j<numRows-1; ++j)
    {
        osg::DrawElementsUInt* elements = new osg::DrawElementsUInt(GL_TRIANGLE_STRIP, numColumns*2);
        for(unsigned int i=0; i<numColumns; ++i)
        {
            unsigned int iv = j*numColumns + i;
            (*elements)[i*2] = iv + numColumns;
            (*elements)[i*2+1] = iv;
        }

        //_primitiveSets.push_back(elements);

        _geometry->addPrimitiveSet(elements);
    }

    osgUtil::SmoothingVisitor smoother;
    _geode->accept(smoother);

    if (colorLayer)
    {
        osgTerrain::ImageLayer* imageLayer = dynamic_cast<osgTerrain::ImageLayer*>(colorLayer);
        if (imageLayer)
        {
            osg::Image* image = imageLayer->getImage();
            osg::StateSet* stateset = _geode->getOrCreateStateSet();

            osg::Texture2D* texture2D = new osg::Texture2D;
            texture2D->setImage(image);
            texture2D->setResizeNonPowerOfTwoHint(false);
            stateset->setTextureAttributeAndModes(color_index, texture2D, osg::StateAttribute::ON);

            if (tf)
            {
                // up the precision of hte internal texture format to its maximum.
                //image->setInternalTextureFormat(GL_LUMINANCE32F_ARB);
                image->setInternalTextureFormat(GL_LUMINANCE16);
            }
        }

    }

    if (tf)
    {
        osg::notify(osg::NOTICE)<<"Requires TransferFunction"<<std::endl;
        osg::Image* image = tf->getImage();
        osg::StateSet* stateset = _geode->getOrCreateStateSet();
        osg::Texture1D* texture1D = new osg::Texture1D;
        texture1D->setImage(image);
        texture1D->setResizeNonPowerOfTwoHint(false);
        texture1D->setFilter(osg::Texture::MIN_FILTER, osg::Texture::NEAREST);
        texture1D->setFilter(osg::Texture::MAG_FILTER, osg::Texture::NEAREST);
        stateset->setTextureAttributeAndModes(tf_index, texture1D, osg::StateAttribute::ON);

        if (colorLayer)
        {
            osg::notify(osg::NOTICE)<<"Using fragment program"<<std::endl;

            osg::Program* program = new osg::Program;
            stateset->setAttribute(program);

            // get shaders from source
            std::string vertexShaderFile = osgDB::findDataFile("lookup.vert");
            if (!vertexShaderFile.empty())
            {
                program->addShader(osg::Shader::readShaderFile(osg::Shader::VERTEX, vertexShaderFile));
            }
            else
            {
                osg::notify(osg::NOTICE)<<"Not found lookup.vert"<<std::endl;
            }

            std::string fragmentShaderFile = osgDB::findDataFile("lookup.frag");
            if (!fragmentShaderFile.empty())
            {
                program->addShader(osg::Shader::readShaderFile(osg::Shader::FRAGMENT, fragmentShaderFile));
            }
            else
            {
                osg::notify(osg::NOTICE)<<"Not found lookup.frag"<<std::endl;
            }

            osg::Uniform* sourceSampler = new osg::Uniform("sourceTexture",color_index);
            stateset->addUniform(sourceSampler);

            osg::Uniform* lookupTexture = new osg::Uniform("lookupTexture",tf_index);
            stateset->addUniform(lookupTexture);

            osg::Uniform* minValue = new osg::Uniform("minValue", tf->getMinimum());
            stateset->addUniform(minValue);

            osg::Uniform* inverseRange = new osg::Uniform("inverseRange", 1.0f/(tf->getMaximum()-tf->getMinimum()));
            stateset->addUniform(inverseRange);
        }
        else
        {
            osg::notify(osg::NOTICE)<<"Using standard OpenGL fixed function pipeline"<<std::endl;
        }
    }

    // _geometry->setUseDisplayList(false);

    _dirty = false;
}

void GeometryTechnique::update(osgUtil::UpdateVisitor* nv)
{
}

void GeometryTechnique::cull(osgUtil::CullVisitor* nv)
{
    if (_geode.valid())
    {
        _geode->accept(*nv);
    }
}

void GeometryTechnique::cleanSceneGraph()
{
}

void GeometryTechnique::dirty()
{
    TerrainTechnique::dirty();
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//  TerrainGeometry
//
TerrainGeometry::TerrainGeometry()
{
}

TerrainGeometry::TerrainGeometry(const TerrainGeometry& geometry,const osg::CopyOp& copyop):
    osg::Drawable(geometry, copyop),
    _vertices(geometry._vertices),
    _normals(geometry._normals),
    _colors(geometry._colors),
    _texcoords(geometry._texcoords),
    _primitiveSets(geometry._primitiveSets)

{
}

osg::BoundingBox TerrainGeometry::computeBound() const
{
    osg::BoundingBox bb;

    if (_vertices.first.valid())
    {
        for(osg::Vec3Array::const_iterator itr = _vertices.first->begin();
            itr != _vertices.first->end();
            ++itr)
        {
            bb.expandBy(*itr);
        }
    }
    return bb;
}

void TerrainGeometry::drawImplementation(osg::RenderInfo& renderInfo) const
{
    osg::notify(osg::NOTICE)<<"TerrainGeometry::drawImplementation"<<std::endl;
}
<commit_msg>Added some debugging<commit_after>/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield
 *
 * This library is open source and may be redistributed and/or modified under
 * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
 * (at your option) any later version.  The full license is in LICENSE file
 * included with this distribution, and on the openscenegraph.org website.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * OpenSceneGraph Public License for more details.
*/

#include <osgTerrain/GeometryTechnique>
#include <osgTerrain/TerrainNode>

#include <osgUtil/SmoothingVisitor>

#include <osgDB/FileUtils>

#include <osg/io_utils>
#include <osg/Texture2D>
#include <osg/Texture1D>
#include <osg/TexEnvCombine>
#include <osg/Program>

using namespace osgTerrain;

GeometryTechnique::GeometryTechnique()
{
}

GeometryTechnique::GeometryTechnique(const GeometryTechnique& gt,const osg::CopyOp& copyop):
    TerrainTechnique(gt,copyop)
{
}

GeometryTechnique::~GeometryTechnique()
{
}

void GeometryTechnique::init()
{
    osg::notify(osg::NOTICE)<<"Doing init()"<<std::endl;

    if (!_terrainNode) return;

    osgTerrain::Layer* elevationLayer = _terrainNode->getElevationLayer();
    osgTerrain::Layer* colorLayer = _terrainNode->getColorLayer(0);
    osg::TransferFunction* colorTF = _terrainNode->getColorTransferFunction(0);

    // if the elevationLayer and colorLayer are the same, and there is colorTF then
    // simply assing as a texture coordinate.
    if ((elevationLayer==colorLayer) && colorTF) colorLayer = 0;

    osg::notify(osg::NOTICE)<<"elevationLayer = "<<elevationLayer<<std::endl;
    osg::notify(osg::NOTICE)<<"colorLayer = "<<colorLayer<<std::endl;
    osg::notify(osg::NOTICE)<<"colorTF = "<<colorTF<<std::endl;

    Locator* elevationLocator = elevationLayer ? elevationLayer->getLocator() : 0;
    Locator* colorLocator = colorLayer ? colorLayer->getLocator() : 0;

    Locator* masterLocator = elevationLocator ? elevationLocator : colorLocator;
    if (!masterLocator)
    {
        osg::notify(osg::NOTICE)<<"Problem, no locator found in any of the terrain layers"<<std::endl;
        return;
    }

    if (!elevationLocator) elevationLocator = masterLocator;
    if (!colorLocator) colorLocator = masterLocator;

    osg::Vec3d bottomLeftNDC(DBL_MAX, DBL_MAX, 0.0);
    osg::Vec3d topRightNDC(-DBL_MAX, -DBL_MAX, 0.0);

    if (elevationLayer)
    {
        if (elevationLocator!= masterLocator)
        {
            masterLocator->computeLocalBounds(*elevationLocator, bottomLeftNDC, topRightNDC);
        }
        else
        {
            bottomLeftNDC.x() = osg::minimum(bottomLeftNDC.x(), 0.0);
            bottomLeftNDC.y() = osg::minimum(bottomLeftNDC.y(), 0.0);
            topRightNDC.x() = osg::maximum(topRightNDC.x(), 1.0);
            topRightNDC.y() = osg::maximum(topRightNDC.y(), 1.0);
        }
    }

    if (colorLayer)
    {
        if (colorLocator!= masterLocator)
        {
            masterLocator->computeLocalBounds(*colorLocator, bottomLeftNDC, topRightNDC);
        }
        else
        {
            bottomLeftNDC.x() = osg::minimum(bottomLeftNDC.x(), 0.0);
            bottomLeftNDC.y() = osg::minimum(bottomLeftNDC.y(), 0.0);
            topRightNDC.x() = osg::maximum(topRightNDC.x(), 1.0);
            topRightNDC.y() = osg::maximum(topRightNDC.y(), 1.0);
        }
    }

    osg::notify(osg::NOTICE)<<"bottomLeftNDC = "<<bottomLeftNDC<<std::endl;
    osg::notify(osg::NOTICE)<<"topRightNDC = "<<topRightNDC<<std::endl;

    _geode = new osg::Geode;
    _geometry = new osg::Geometry;
    _geode->addDrawable(_geometry.get());
    _geode->addDrawable(new osgTerrain::TerrainGeometry);

    unsigned int numRows = 100;
    unsigned int numColumns = 100;

    if (elevationLayer)
    {
        numColumns = elevationLayer->getNumColumns();
        numRows = elevationLayer->getNumRows();
    }

    unsigned int numVertices = numRows * numColumns;

    // allocate and assign vertices
    osg::Vec3Array* _vertices = new osg::Vec3Array(numVertices);
    _geometry->setVertexArray(_vertices);

    // allocate and assign normals
    osg::Vec3Array* _normals = new osg::Vec3Array(numVertices);
    _geometry->setNormalArray(_normals);
    _geometry->setNormalBinding(osg::Geometry::BIND_PER_VERTEX);

    int texcoord_index = 0;
    int color_index = -1;
    int tf_index = -1;

    float minHeight = 0.0;
    float scaleHeight = 1.0;

    // allocate and assign tex coords
    osg::Vec2Array* _texcoords = 0;
    if (colorLayer)
    {
        color_index = texcoord_index;
        ++texcoord_index;

        _texcoords = new osg::Vec2Array(numVertices);
        _geometry->setTexCoordArray(color_index, _texcoords);
    }

    osg::FloatArray* _elevations = 0;
    osg::TransferFunction1D* tf = dynamic_cast<osg::TransferFunction1D*>(colorTF);
    if (tf)
    {
        tf_index = texcoord_index;
        ++texcoord_index;

        if (!colorLayer)
        {
            _elevations = new osg::FloatArray(numVertices);
            _geometry->setTexCoordArray(tf_index, _elevations);

            minHeight = tf->getMinimum();
            scaleHeight = 1.0f/(tf->getMaximum()-tf->getMinimum());
        }
    }

    // allocate and assign color
    osg::Vec4Array* _colors = new osg::Vec4Array(1);
    _geometry->setColorArray(_colors);
    _geometry->setColorBinding(osg::Geometry::BIND_OVERALL);
    (*_colors)[0].set(1.0f,1.0f,1.0f,1.0f);

    // populate vertex and tex coord arrays
    unsigned int j;
    for(j=0; j<numRows; ++j)
    {
        for(unsigned int i=0; i<numColumns; ++i)
        {
            unsigned int iv = j*numColumns + i;
            osg::Vec3d ndc( ((double)i)/(double)(numColumns-1), ((double)j)/(double)(numRows-1), 0.0);

            if (elevationLayer)
            {
                float value = 0.0f;
                elevationLayer->getValue(i,j, value);
                // osg::notify(osg::NOTICE)<<"i="<<i<<" j="<<j<<" z="<<value<<std::endl;
                ndc.z() = value;
            }

            osg::Vec3d model;
            masterLocator->convertLocalToModel(ndc, model);

            (*_vertices)[iv] = model;

            if (colorLayer)
            {
                if (colorLocator!= masterLocator)
                {
                    osg::Vec3d color_ndc;
                    Locator::convertLocalCoordBetween(*masterLocator, ndc, *colorLocator, color_ndc);
                    (*_texcoords)[iv].set(color_ndc.x(), color_ndc.y());
                }
                else
                {
                    (*_texcoords)[iv].set(ndc.x(), ndc.y());
                }

            }

            if (_elevations)
            {
                (*_elevations)[iv] = (ndc.z()-minHeight)*scaleHeight;
            }

            // compute the local normal
            osg::Vec3d ndc_one( (double)i/(double)(numColumns-1), (double)j/(double)(numColumns-1), 1.0);
            osg::Vec3d model_one;
            masterLocator->convertLocalToModel(ndc_one, model_one);
            model_one -= model;
            model_one.normalize();
            (*_normals)[iv] = model_one;
        }
    }

    // populate primitive sets
    // _primitiveSets.clear();
    for(j=0; j<numRows-1; ++j)
    {
        osg::DrawElementsUInt* elements = new osg::DrawElementsUInt(GL_TRIANGLE_STRIP, numColumns*2);
        for(unsigned int i=0; i<numColumns; ++i)
        {
            unsigned int iv = j*numColumns + i;
            (*elements)[i*2] = iv + numColumns;
            (*elements)[i*2+1] = iv;
        }

        //_primitiveSets.push_back(elements);

        _geometry->addPrimitiveSet(elements);
    }

    osgUtil::SmoothingVisitor smoother;
    _geode->accept(smoother);

    if (colorLayer)
    {
        osgTerrain::ImageLayer* imageLayer = dynamic_cast<osgTerrain::ImageLayer*>(colorLayer);
        if (imageLayer)
        {
            osg::Image* image = imageLayer->getImage();
            osg::StateSet* stateset = _geode->getOrCreateStateSet();

            osg::Texture2D* texture2D = new osg::Texture2D;
            texture2D->setImage(image);
            texture2D->setResizeNonPowerOfTwoHint(false);
            stateset->setTextureAttributeAndModes(color_index, texture2D, osg::StateAttribute::ON);

            if (tf)
            {
                // up the precision of hte internal texture format to its maximum.
                //image->setInternalTextureFormat(GL_LUMINANCE32F_ARB);
                image->setInternalTextureFormat(GL_LUMINANCE16);
            }
        }

    }

    if (tf)
    {
        osg::notify(osg::NOTICE)<<"Requires TransferFunction"<<std::endl;
        osg::Image* image = tf->getImage();
        osg::StateSet* stateset = _geode->getOrCreateStateSet();
        osg::Texture1D* texture1D = new osg::Texture1D;
        texture1D->setImage(image);
        texture1D->setResizeNonPowerOfTwoHint(false);
        texture1D->setFilter(osg::Texture::MIN_FILTER, osg::Texture::NEAREST);
        texture1D->setFilter(osg::Texture::MAG_FILTER, osg::Texture::NEAREST);
        stateset->setTextureAttributeAndModes(tf_index, texture1D, osg::StateAttribute::ON);

        if (colorLayer)
        {
            osg::notify(osg::NOTICE)<<"Using fragment program"<<std::endl;

            osg::Program* program = new osg::Program;
            stateset->setAttribute(program);

            // get shaders from source
            std::string vertexShaderFile = osgDB::findDataFile("lookup.vert");
            if (!vertexShaderFile.empty())
            {
                program->addShader(osg::Shader::readShaderFile(osg::Shader::VERTEX, vertexShaderFile));
            }
            else
            {
                osg::notify(osg::NOTICE)<<"Not found lookup.vert"<<std::endl;
            }

            std::string fragmentShaderFile = osgDB::findDataFile("lookup.frag");
            if (!fragmentShaderFile.empty())
            {
                program->addShader(osg::Shader::readShaderFile(osg::Shader::FRAGMENT, fragmentShaderFile));
            }
            else
            {
                osg::notify(osg::NOTICE)<<"Not found lookup.frag"<<std::endl;
            }

            osg::Uniform* sourceSampler = new osg::Uniform("sourceTexture",color_index);
            stateset->addUniform(sourceSampler);

            osg::Uniform* lookupTexture = new osg::Uniform("lookupTexture",tf_index);
            stateset->addUniform(lookupTexture);

            osg::Uniform* minValue = new osg::Uniform("minValue", tf->getMinimum());
            stateset->addUniform(minValue);

            osg::Uniform* inverseRange = new osg::Uniform("inverseRange", 1.0f/(tf->getMaximum()-tf->getMinimum()));
            stateset->addUniform(inverseRange);
        }
        else
        {
            osg::notify(osg::NOTICE)<<"Using standard OpenGL fixed function pipeline"<<std::endl;
        }
    }

    // _geometry->setUseDisplayList(false);

    _dirty = false;
}

void GeometryTechnique::update(osgUtil::UpdateVisitor* nv)
{
}

void GeometryTechnique::cull(osgUtil::CullVisitor* nv)
{
    if (_geode.valid())
    {
        _geode->accept(*nv);
    }
}

void GeometryTechnique::cleanSceneGraph()
{
}

void GeometryTechnique::dirty()
{
    TerrainTechnique::dirty();
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//  TerrainGeometry
//
TerrainGeometry::TerrainGeometry()
{
}

TerrainGeometry::TerrainGeometry(const TerrainGeometry& geometry,const osg::CopyOp& copyop):
    osg::Drawable(geometry, copyop),
    _vertices(geometry._vertices),
    _normals(geometry._normals),
    _colors(geometry._colors),
    _texcoords(geometry._texcoords),
    _primitiveSets(geometry._primitiveSets)

{
}

osg::BoundingBox TerrainGeometry::computeBound() const
{
    osg::BoundingBox bb;

    if (_vertices.first.valid())
    {
        for(osg::Vec3Array::const_iterator itr = _vertices.first->begin();
            itr != _vertices.first->end();
            ++itr)
        {
            bb.expandBy(*itr);
        }
    }
    return bb;
}

void TerrainGeometry::drawImplementation(osg::RenderInfo& renderInfo) const
{
    osg::notify(osg::NOTICE)<<"TerrainGeometry::drawImplementation"<<std::endl;

    if (_vertices.first.valid() && _vertices.first->getDataVariance()==DYNAMIC)
    {
        osg::notify(osg::NOTICE)<<"  Vertices DYNAMIC"<<std::endl;
    }
    else
    {
        osg::notify(osg::NOTICE)<<"  Vertices STATIC"<<std::endl;
    }
}
<|endoftext|>


<commit_before>#include "./constraint_updater.h"
#include <Eigen/Geometry>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <vector>
#include <utility>
#include "../graphics/vertex_array.h"
#include "../graphics/render_data.h"
BOOST_GEOMETRY_REGISTER_POINT_2D(Eigen::Vector2i, int, cs::cartesian, x(), y())
#include "./boost_polygon_concepts.h"

ConstraintUpdater::ConstraintUpdater(
    Graphics::Gl *gl, std::shared_ptr<Graphics::ShaderManager> shaderManager,
    int width, int height)
  : gl(gl), shaderManager(shaderManager), width(width), height(height)
{
  shaderId = shaderManager->addShader(":/shader/constraint.vert",
                                      ":/shader/colorImmediate.frag");

  Eigen::Affine3f pixelToNDCTransform(
      Eigen::Translation3f(Eigen::Vector3f(-1, 1, 0)) *
      Eigen::Scaling(Eigen::Vector3f(2.0f / width, -2.0f / height, 1)));
  pixelToNDC = pixelToNDCTransform.matrix();
}

polygon createBoxPolygon(Eigen::Vector2i center, Eigen::Vector2i size)
{
  polygon p;
  p.outer().push_back(
      Eigen::Vector2i(center.x() - size.x(), center.y() - size.y()));
  p.outer().push_back(
      Eigen::Vector2i(center.x() + size.x(), center.y() - size.y()));
  p.outer().push_back(
      Eigen::Vector2i(center.x() + size.x(), center.y() + size.y()));
  p.outer().push_back(
      Eigen::Vector2i(center.x() - size.x(), center.y() + size.y()));

  return p;
}

void ConstraintUpdater::convolveTwoSegements(const edge &a, const edge &b)
{
  point p = a.first;
  convolve(p, b.second);
  positions.push_back(p.x());
  positions.push_back(height - p.y());
  positions.push_back(p.x());
  positions.push_back(height - p.y());

  p = a.first;
  convolve(p, b.first);
  positions.push_back(p.x());
  positions.push_back(height - p.y());

  p = a.second;
  convolve(p, b.first);
  positions.push_back(p.x());
  positions.push_back(height - p.y());

  p = a.second;
  convolve(p, b.second);
  positions.push_back(p.x());
  positions.push_back(height - p.y());
}

template <typename itrT1, typename itrT2>
void ConstraintUpdater::convolveTwoPointSequences(itrT1 ab, itrT1 ae, itrT2 bb,
                                                  itrT2 be)
{
  if (ab == ae || bb == be)
    return;
  point prev_a = *ab;
  ++ab;
  for (; ab != ae; ++ab)
  {
    point prev_b = *bb;
    itrT2 tmpb = bb;
    ++tmpb;
    for (; tmpb != be; ++tmpb)
    {
      convolveTwoSegements(std::make_pair(prev_b, *tmpb),
                           std::make_pair(prev_a, *ab));
      prev_b = *tmpb;
    }
    prev_a = *ab;
  }
}

void ConstraintUpdater::convolveTwoPolygons(const ppolygon &a,
                                            const ppolygon &b)
{
  convolveTwoPointSequences(begin_points(a), end_points(a), begin_points(b),
                            end_points(b));

  ppolygon tmp_poly = a;
  addPolygonToPositions(convolve(tmp_poly, *(begin_points(b))));
  tmp_poly = b;
  addPolygonToPositions(convolve(tmp_poly, *(begin_points(a))));
}

void ConstraintUpdater::addPolygonToPositions(const ppolygon &polygon)
{
  std::vector<boost::polygon::point_data<int>> p(polygon.begin(), polygon.end());
  auto iteratorBegin = p.begin();
  positions.push_back(iteratorBegin->x());
  positions.push_back(height - iteratorBegin->y());
  for (auto iterator = iteratorBegin; iterator != p.end(); ++iterator)
  {
    positions.push_back(iterator->x());
    positions.push_back(height - iterator->y());
  }
}

void ConstraintUpdater::minkowskiSum(const polygon &a, const polygon &b)
{
  ppolygon aPoly;
  boost::polygon::set_points(aPoly, a.outer().begin(), a.outer().end());

  ppolygon bPoly;
  boost::polygon::set_points(bPoly, b.outer().begin(), b.outer().end());

  convolveTwoPolygons(aPoly, bPoly);
}

void ConstraintUpdater::addLabel(Eigen::Vector2i anchorPosition,
                                 Eigen::Vector2i labelSize,
                                 Eigen::Vector2i lastAnchorPosition,
                                 Eigen::Vector2i lastLabelPosition,
                                 Eigen::Vector2i lastLabelSize)
{
  polygon oldLabel = createBoxPolygon(lastLabelPosition, lastLabelSize / 2);

  polygon oldLabelExtruded(oldLabel);
  for (auto point : oldLabel.outer())
  {
    Eigen::Vector2i p = anchorPosition + 1000 * (point - anchorPosition);
    oldLabelExtruded.outer().push_back(p);
  }

  polygon oldLabelExtrudedConvexHull;
  boost::geometry::convex_hull(oldLabelExtruded, oldLabelExtrudedConvexHull);

  int border = 2;
  polygon newLabel = createBoxPolygon(
      Eigen::Vector2i(0, 0), labelSize / 2 + Eigen::Vector2i(border, border));

  positions.clear();
  minkowskiSum(oldLabelExtrudedConvexHull, newLabel);

  polygon connectorPolygon;
  connectorPolygon.outer().push_back(lastAnchorPosition);
  Eigen::Vector2i throughLastAnchor =
      anchorPosition + 1000 * (lastAnchorPosition - anchorPosition);
  connectorPolygon.outer().push_back(throughLastAnchor);

  Eigen::Vector2i throughLastLabel =
      anchorPosition + 1000 * (lastLabelPosition - anchorPosition);
  connectorPolygon.outer().push_back(throughLastLabel);
  connectorPolygon.outer().push_back(lastLabelPosition);

  minkowskiSum(connectorPolygon, newLabel);

  drawElementVector(positions);
}

void ConstraintUpdater::clear()
{
  gl->glViewport(0, 0, width, height);
  gl->glClearColor(0, 0, 0, 0);
  gl->glClear(GL_COLOR_BUFFER_BIT);
}

template <class T> void ConstraintUpdater::drawPolygon(std::vector<T> polygon)
{
  std::vector<float> positions;
  if (polygon.size() > 0)
  {
    auto point = polygon[0];
    positions.push_back(point.x());
    positions.push_back(height - point.y());
  }
  for (auto point : polygon)
  {
    positions.push_back(point.x());
    positions.push_back(height - point.y());
  }

  drawElementVector(positions);
}

void ConstraintUpdater::drawElementVector(std::vector<float> positions)
{
  Graphics::VertexArray *vertexArray =
      new Graphics::VertexArray(gl, GL_TRIANGLE_FAN, 2);
  vertexArray->addStream(positions, 2);

  RenderData renderData;
  renderData.modelMatrix = Eigen::Matrix4f::Identity();
  renderData.viewMatrix = pixelToNDC;
  renderData.projectionMatrix = Eigen::Matrix4f::Identity();
  shaderManager->bind(shaderId, renderData);
  vertexArray->draw();
}

<commit_msg>Try to fix polygons by sorting vertices according to angle.<commit_after>#include "./constraint_updater.h"
#include <Eigen/Geometry>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <vector>
#include <cmath>
#include <utility>
#include "../graphics/vertex_array.h"
#include "../graphics/render_data.h"
BOOST_GEOMETRY_REGISTER_POINT_2D(Eigen::Vector2i, int, cs::cartesian, x(), y())
#include "./boost_polygon_concepts.h"

ConstraintUpdater::ConstraintUpdater(
    Graphics::Gl *gl, std::shared_ptr<Graphics::ShaderManager> shaderManager,
    int width, int height)
  : gl(gl), shaderManager(shaderManager), width(width), height(height)
{
  shaderId = shaderManager->addShader(":/shader/constraint.vert",
                                      ":/shader/colorImmediate.frag");

  Eigen::Affine3f pixelToNDCTransform(
      Eigen::Translation3f(Eigen::Vector3f(-1, 1, 0)) *
      Eigen::Scaling(Eigen::Vector3f(2.0f / width, -2.0f / height, 1)));
  pixelToNDC = pixelToNDCTransform.matrix();
}

polygon createBoxPolygon(Eigen::Vector2i center, Eigen::Vector2i size)
{
  polygon p;
  p.outer().push_back(
      Eigen::Vector2i(center.x() - size.x(), center.y() - size.y()));
  p.outer().push_back(
      Eigen::Vector2i(center.x() + size.x(), center.y() - size.y()));
  p.outer().push_back(
      Eigen::Vector2i(center.x() + size.x(), center.y() + size.y()));
  p.outer().push_back(
      Eigen::Vector2i(center.x() - size.x(), center.y() + size.y()));

  return p;
}

void ConstraintUpdater::convolveTwoSegements(const edge &a, const edge &b)
{
  point p = a.first;
  convolve(p, b.second);
  positions.push_back(p.x());
  positions.push_back(height - p.y());
  positions.push_back(p.x());
  positions.push_back(height - p.y());

  p = a.first;
  convolve(p, b.first);
  positions.push_back(p.x());
  positions.push_back(height - p.y());

  p = a.second;
  convolve(p, b.first);
  positions.push_back(p.x());
  positions.push_back(height - p.y());

  p = a.second;
  convolve(p, b.second);
  positions.push_back(p.x());
  positions.push_back(height - p.y());
}

template <typename itrT1, typename itrT2>
void ConstraintUpdater::convolveTwoPointSequences(itrT1 ab, itrT1 ae, itrT2 bb,
                                                  itrT2 be)
{
  if (ab == ae || bb == be)
    return;
  point prev_a = *ab;
  ++ab;
  for (; ab != ae; ++ab)
  {
    point prev_b = *bb;
    itrT2 tmpb = bb;
    ++tmpb;
    for (; tmpb != be; ++tmpb)
    {
      convolveTwoSegements(std::make_pair(prev_b, *tmpb),
                           std::make_pair(prev_a, *ab));
      prev_b = *tmpb;
    }
    prev_a = *ab;
  }
}

void ConstraintUpdater::convolveTwoPolygons(const ppolygon &a,
                                            const ppolygon &b)
{
  convolveTwoPointSequences(begin_points(a), end_points(a), begin_points(b),
                            end_points(b));

  ppolygon tmp_poly = a;
  addPolygonToPositions(convolve(tmp_poly, *(begin_points(b))));
  tmp_poly = b;
  addPolygonToPositions(convolve(tmp_poly, *(begin_points(a))));
}

bool hasSmallerAngle(float dir1X, float dir1Y, float dir2X, float dir2Y)
{
  float angle1 = std::atan2(dir1Y, dir1X);
  float angle2 = std::atan2(dir2Y, dir2X);

  return angle1 <= angle2;
}

void ConstraintUpdater::addPolygonToPositions(const ppolygon &polygon)
{
  auto iteratorBegin = polygon.begin();
  float referenceX = iteratorBegin->x();
  positions.push_back(referenceX);
  float referenceY = iteratorBegin->y();
  positions.push_back(height - referenceY);
  positions.push_back(referenceX);
  positions.push_back(height - referenceY);

  std::list<std::pair<float, float>> temp;
  for (auto iterator = ++iteratorBegin; iterator != polygon.end(); ++iterator)
  {
    float diffX = iterator->x() - referenceX;
    float diffY = iterator->y() - referenceY;
    auto inner = temp.begin();
    while (inner != temp.end() &&
           hasSmallerAngle(inner->first - referenceX,
                           inner->second - referenceY, diffX, diffY))
      ++inner;

    temp.insert(inner, 1, std::make_pair(iterator->x(), iterator->y()));
  }

  for (auto iterator = temp.cbegin(); iterator != temp.cend(); ++iterator)
  {
    positions.push_back(iterator->first);
    positions.push_back(height - iterator->second);
  }
}

void ConstraintUpdater::minkowskiSum(const polygon &a, const polygon &b)
{
  ppolygon aPoly;
  boost::polygon::set_points(aPoly, a.outer().begin(), a.outer().end());

  ppolygon bPoly;
  boost::polygon::set_points(bPoly, b.outer().begin(), b.outer().end());

  convolveTwoPolygons(aPoly, bPoly);
}

void ConstraintUpdater::addLabel(Eigen::Vector2i anchorPosition,
                                 Eigen::Vector2i labelSize,
                                 Eigen::Vector2i lastAnchorPosition,
                                 Eigen::Vector2i lastLabelPosition,
                                 Eigen::Vector2i lastLabelSize)
{
  polygon oldLabel = createBoxPolygon(lastLabelPosition, lastLabelSize / 2);

  polygon oldLabelExtruded(oldLabel);
  for (auto point : oldLabel.outer())
  {
    Eigen::Vector2i p = anchorPosition + 1000 * (point - anchorPosition);
    oldLabelExtruded.outer().push_back(p);
  }

  polygon oldLabelExtrudedConvexHull;
  boost::geometry::convex_hull(oldLabelExtruded, oldLabelExtrudedConvexHull);

  int border = 2;
  polygon newLabel = createBoxPolygon(
      Eigen::Vector2i(0, 0), labelSize / 2 + Eigen::Vector2i(border, border));

  positions.clear();
  minkowskiSum(oldLabelExtrudedConvexHull, newLabel);

  polygon connectorPolygon;
  connectorPolygon.outer().push_back(lastAnchorPosition);
  Eigen::Vector2i throughLastAnchor =
      anchorPosition + 1000 * (lastAnchorPosition - anchorPosition);
  connectorPolygon.outer().push_back(throughLastAnchor);

  Eigen::Vector2i throughLastLabel =
      anchorPosition + 1000 * (lastLabelPosition - anchorPosition);
  connectorPolygon.outer().push_back(throughLastLabel);
  connectorPolygon.outer().push_back(lastLabelPosition);

  minkowskiSum(connectorPolygon, newLabel);

  drawElementVector(positions);
}

void ConstraintUpdater::clear()
{
  gl->glViewport(0, 0, width, height);
  gl->glClearColor(0, 0, 0, 0);
  gl->glClear(GL_COLOR_BUFFER_BIT);
}

template <class T> void ConstraintUpdater::drawPolygon(std::vector<T> polygon)
{
  std::vector<float> positions;
  if (polygon.size() > 0)
  {
    auto point = polygon[0];
    positions.push_back(point.x());
    positions.push_back(height - point.y());
  }
  for (auto point : polygon)
  {
    positions.push_back(point.x());
    positions.push_back(height - point.y());
  }

  drawElementVector(positions);
}

void ConstraintUpdater::drawElementVector(std::vector<float> positions)
{
  Graphics::VertexArray *vertexArray =
      new Graphics::VertexArray(gl, GL_TRIANGLE_FAN, 2);
  vertexArray->addStream(positions, 2);

  RenderData renderData;
  renderData.modelMatrix = Eigen::Matrix4f::Identity();
  renderData.viewMatrix = pixelToNDC;
  renderData.projectionMatrix = Eigen::Matrix4f::Identity();
  shaderManager->bind(shaderId, renderData);
  vertexArray->draw();
}

<|endoftext|>


<commit_before>#include "private_window.h"

#include <game/DirectWindow.h>

// We really shouldn't be compiled with an older compiler to begin with...we need epic TCO to work properly...
#if __cplusplus > 199711L
    #define ATHENA_OVERRIDE override
#else
    #define ATHENA_OVERRIDE
#endif

#include <vector>
#include <algorithm>

class Athena_Window : public BDirectWindow{

    /*
    typedef struct {
       direct_buffer_state   buffer_state;
       direct_driver_state   driver_state;
       void*                 bits;
       void*                 pci_bits;
       int32                 bytes_per_row;
       uint32                bits_per_pixel;
       color_space           pixel_format;
       buffer_layout         layout;
       buffer_orientation    orientation;
       uint32                _reserved[9];
       uint32                _dd_type_;
       uint32                _dd_token_;
       uint32                clip_list_count;
       clipping_rect         window_bounds;
       clipping_rect         clip_bounds;
       clipping_rect         clip_list[1];
    } direct_buffer_info;
    */

    uint8_t *screen;
    uint32_t pitch, depth; // both in bytes.
    color_space format;
    clipping_rect bounds;

    std::vector<clipping_rect> clip_list;

    bool connected, connection_disabled;

    static void ConvertColorSpaces(const uint32_t *in, void *out, size_t num_pixels, color_space c);

public:
    Athena_Window(int x, int y, unsigned w, unsigned h, const char *title)
      : BDirectWindow(BRect(x, y, x + w, y + h), title, B_TITLED_WINDOW, B_NOT_RESIZABLE|B_QUIT_ON_WINDOW_CLOSE, B_CURRENT_WORKSPACE){

    }

    ~Athena_Window(){
        connection_disabled = false;
        Hide();
        Sync();
    }

	int DrawImage(int x, int y, unsigned w, unsigned h, unsigned format, const void *RGB);
	int DrawRect(int x, int y, unsigned w, unsigned h, const struct Athena_Color *color);

    virtual void DirectConnected(direct_buffer_info* info) ATHENA_OVERRIDE;

};

void Athena_Window::ConvertColorSpaces(const uint32_t *in, void *out, size_t num_pixels, color_space c){
    switch(c){
        case B_RGB32:
        case B_RGBA32:
        case B_RGB32_BIG:
        case B_RGBA32_BIG:
            std::copy(in, in + num_pixels, static_cast<uint32_t *>(out));
            break;
        case B_RGB24:
        case B_RGB24_BIG:
            for(unsigned i = 0; i<num_pixels; i++){
                uint8_t *pixel = static_cast<uint8_t *>(out) + (3 * i);
                pixel[0] = in[i] & 0xFF;
                pixel[1] = (in[i] >> 8) & 0xFF;
                pixel[2] = (in[i] >> 16) & 0xFF;
            }
            break;
        default:
            // ...
            break;
    }
}

void Athena_Window::DirectConnected(direct_buffer_info *info){
    switch(info->buffer_state & B_DIRECT_MODE_MASK){
        case B_DIRECT_START:
            connected = true;
        case B_DIRECT_MODIFY:
            clip_list.clear();
            clip_list.insert(clip_list.end(), info->clip_list, info->clip_list + info->clip_list_count);

            screen = reinterpret_cast<uint8_t *>(info->bits);
            pitch = info->bytes_per_row;
            depth = info->bits_per_pixel >> 3;
            format = info->pixel_format;
            bounds = info->window_bounds;

        case B_DIRECT_STOP:
            connected = false;
    }
}

void *Athena_Private_CreateHandle();
int Athena_Private_DestroyHandle(void *);
int Athena_Private_CreateWindow(void *handle, int x, int y, unsigned w, unsigned h, const char *title);
int Athena_Private_ShowWindow(void *);
int Athena_Private_HideWindow(void *);

/* Neither the BeBook nor the Haiku docs mention the composition of a clipping_rect :( */
int Athena_Private_DrawImage(void *handle, int x, int y, unsigned w, unsigned h, unsigned format, const void *RGB){
	return static_cast<Athena_Window *>(handle)->DrawImage(x, y, w, h, format, RGB);
}

int Athena_Window::DrawImage(int x, int y, unsigned w, unsigned h, unsigned format_x, const void *RGB){
    /* In terms of RGB */
    const int starting_x = std::max(0, -x),
        starting_y = std::max(0, -y),
        ending_x = std::min<int>(w, static_cast<int>(bounds.right) - x),
        ending_y = std::min<int>(h, static_cast<int>(bounds.bottom) - y);

    uint8_t *row_start = screen + (starting_y * pitch);

    for(int i = starting_y; i < ending_y; i++){
        ConvertColorSpaces(static_cast<const uint32_t *>(RGB) + starting_x + (i * w), row_start + (x * depth), ending_x - starting_x, this->format);
        row_start += pitch;
    }
    return 0;
}

int Athena_Private_DrawImage(void *handle, int x, int y, unsigned w, unsigned h, unsigned format, const struct Athena_Color *color){
	return 0;
}

int Athena_Private_DrawLine(void *handle, int x1, int y1, int x2, int y2, const struct Athena_Color *color){
	return 0;
}

int Athena_Private_FlipWindow(void *handle){
	return 0;
}

unsigned Athena_Private_GetEvent(void *handle, struct Athena_Event *to){
	return 0;
}

int Athena_Private_IsKeyPressed(void *handle, unsigned key);

<commit_msg>Added window exposure handling to Haiku backend<commit_after>#include "private_window.h"

#include <game/DirectWindow.h>

// We really shouldn't be compiled with an older compiler to begin with...we need epic TCO to work properly...
#if __cplusplus > 199711L
    #define ATHENA_OVERRIDE override
#else
    #define ATHENA_OVERRIDE
#endif

#include <vector>
#include <algorithm>

class Athena_Window : public BDirectWindow{

    /*
    typedef struct {
       direct_buffer_state   buffer_state;
       direct_driver_state   driver_state;
       void*                 bits;
       void*                 pci_bits;
       int32                 bytes_per_row;
       uint32                bits_per_pixel;
       color_space           pixel_format;
       buffer_layout         layout;
       buffer_orientation    orientation;
       uint32                _reserved[9];
       uint32                _dd_type_;
       uint32                _dd_token_;
       uint32                clip_list_count;
       clipping_rect         window_bounds;
       clipping_rect         clip_bounds;
       clipping_rect         clip_list[1];
    } direct_buffer_info;
    */

    uint8_t *screen;
    uint32_t pitch, depth; // both in bytes.
    color_space format;
    clipping_rect bounds;

    std::vector<clipping_rect> clip_list;

    bool connected, connection_disabled;

    static void ConvertColorSpaces(const uint32_t *in, void *out, size_t num_pixels, color_space c);

public:
    Athena_Window(int x, int y, unsigned w, unsigned h, const char *title)
      : BDirectWindow(BRect(x, y, x + w, y + h), title, B_TITLED_WINDOW, B_NOT_RESIZABLE|B_QUIT_ON_WINDOW_CLOSE, B_CURRENT_WORKSPACE){

    }

    ~Athena_Window(){
        connection_disabled = false;
        Hide();
        Sync();
    }

	int DrawImage(int x, int y, unsigned w, unsigned h, unsigned format, const void *RGB);
	int DrawRect(int x, int y, unsigned w, unsigned h, const struct Athena_Color *color);

    virtual void DirectConnected(direct_buffer_info* info) ATHENA_OVERRIDE;

};

void Athena_Window::ConvertColorSpaces(const uint32_t *in, void *out, size_t num_pixels, color_space c){
    switch(c){
        case B_RGB32:
        case B_RGBA32:
        case B_RGB32_BIG:
        case B_RGBA32_BIG:
            std::copy(in, in + num_pixels, static_cast<uint32_t *>(out));
            break;
        case B_RGB24:
        case B_RGB24_BIG:
            for(unsigned i = 0; i<num_pixels; i++){
                uint8_t *pixel = static_cast<uint8_t *>(out) + (3 * i);
                pixel[0] = in[i] & 0xFF;
                pixel[1] = (in[i] >> 8) & 0xFF;
                pixel[2] = (in[i] >> 16) & 0xFF;
            }
            break;
        default:
            // ...
            break;
    }
}

void Athena_Window::DirectConnected(direct_buffer_info *info){
    switch(info->buffer_state & B_DIRECT_MODE_MASK){
        case B_DIRECT_START:
            connected = true;
        case B_DIRECT_MODIFY:
            clip_list.clear();
            clip_list.insert(clip_list.end(), info->clip_list, info->clip_list + info->clip_list_count);

            screen = reinterpret_cast<uint8_t *>(info->bits);
            pitch = info->bytes_per_row;
            depth = info->bits_per_pixel >> 3;
            format = info->pixel_format;
            bounds = info->window_bounds;

        case B_DIRECT_STOP:
            connected = false;
    }
}

struct Athena_WindowHandle{
    Athena_Window *window;
};

void *Athena_Private_CreateHandle(){
    Athena_WindowHandle *handle = new Athena_WindowHandle();
    return handle;
}

int Athena_Private_DestroyHandle(void *that){
    Athena_WindowHandle *const handle = static_cast<Athena_WindowHandle *>(that);
    delete handle->window;
    delete handle;
    return 0;
}

int Athena_Private_CreateWindow(void *that, int x, int y, unsigned w, unsigned h, const char *title){
    Athena_WindowHandle *const handle = static_cast<Athena_WindowHandle *>(that);
    handle->window = new Athena_Window(x, y, w, h, title);
    return 0;
}

int Athena_Private_ShowWindow(void *that){
    Athena_WindowHandle *const handle = static_cast<Athena_WindowHandle *>(that);
    handle->window->Show();
    return 0;
}

int Athena_Private_HideWindow(void *that){
    Athena_WindowHandle *const handle = static_cast<Athena_WindowHandle *>(that);
    handle->window->Hide();
    return 0;

}

/* Neither the BeBook nor the Haiku docs mention the composition of a clipping_rect :( */
int Athena_Private_DrawImage(void *handle, int x, int y, unsigned w, unsigned h, unsigned format, const void *RGB){
	return static_cast<Athena_Window *>(handle)->DrawImage(x, y, w, h, format, RGB);
}

int Athena_Window::DrawImage(int x, int y, unsigned w, unsigned h, unsigned format_x, const void *RGB){
    /* In terms of RGB */
    const int starting_x = std::max(0, -x),
        starting_y = std::max(0, -y),
        ending_x = std::min<int>(w, static_cast<int>(bounds.right) - x),
        ending_y = std::min<int>(h, static_cast<int>(bounds.bottom) - y);

    uint8_t *row_start = screen + (starting_y * pitch);

    for(int i = starting_y; i < ending_y; i++){
        ConvertColorSpaces(static_cast<const uint32_t *>(RGB) + starting_x + (i * w), row_start + (x * depth), ending_x - starting_x, this->format);
        row_start += pitch;
    }
    return 0;
}

int Athena_Private_DrawImage(void *handle, int x, int y, unsigned w, unsigned h, unsigned format, const struct Athena_Color *color){
	return 0;
}

int Athena_Private_DrawLine(void *handle, int x1, int y1, int x2, int y2, const struct Athena_Color *color){
	return 0;
}

int Athena_Private_FlipWindow(void *handle){
	return 0;
}

unsigned Athena_Private_GetEvent(void *handle, struct Athena_Event *to){
	return 0;
}

int Athena_Private_IsKeyPressed(void *handle, unsigned key);

<|endoftext|>


<commit_before>/* -------------------------------------------------------------------------- */
/* Copyright 2002-2010, OpenNebula Project Leads (OpenNebula.org)             */
/*                                                                            */
/* Licensed under the Apache License, Version 2.0 (the "License"); you may    */
/* not use this file except in compliance with the License. You may obtain    */
/* a copy of the License at                                                   */
/*                                                                            */
/* http://www.apache.org/licenses/LICENSE-2.0                                 */
/*                                                                            */
/* Unless required by applicable law or agreed to in writing, software        */
/* distributed under the License is distributed on an "AS IS" BASIS,          */
/* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   */
/* See the License for the specific language governing permissions and        */
/* limitations under the License.                                             */
/* -------------------------------------------------------------------------- */

#include "RequestManager.h"

#include "NebulaLog.h"
#include "Nebula.h"

#include "AuthManager.h"

/* -------------------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */

void RequestManager::ImagePublish::execute(
    xmlrpc_c::paramList const& paramList,
    xmlrpc_c::value *   const  retval)
{
    string              session;

    int                 iid;
    bool                publish_flag;
    int                 uid;

    int                 image_owner;
    bool                is_public;

    Image             * image;

    bool                response;

    ostringstream       oss;

    const string        method_name = "ImagePublish";

    vector<xmlrpc_c::value> arrayData;
    xmlrpc_c::value_array * arrayresult;

    NebulaLog::log("ReM",Log::DEBUG,"ImagePublish invoked");

    session      = xmlrpc_c::value_string (paramList.getString(0));
    iid          = xmlrpc_c::value_int    (paramList.getInt(1));
    publish_flag = xmlrpc_c::value_boolean(paramList.getBoolean(2));

    // First, we need to authenticate the user
    uid = ImagePublish::upool->authenticate(session);

    if ( uid == -1 )
    {
        goto error_authenticate;
    }

    // Get image from the ImagePool
    image = ImagePublish::ipool->get(iid,true);

    if ( image == 0 )
    {
        goto error_image_get;
    }

    image_owner = image->get_uid();
    is_public   = image->isPublic();

    image->unlock();

    //Authorize the operation
    if ( uid != 0 ) // uid == 0 means oneadmin
    {
        AuthRequest ar(uid);

        ar.add_auth(AuthRequest::IMAGE,
                    iid,
                    AuthRequest::MANAGE,
                    image_owner,
                    is_public);

        if (UserPool::authorize(ar) == -1)
        {
            goto error_authorize;
        }
    }

    // Get the image locked again
    image = ImagePublish::ipool->get(iid,true);

    if ( image == 0 )
    {
        goto error_image_get;
    }

    response = image->publish(publish_flag);

    if (!response)
    {
        goto error_publish;
    }

    ImagePublish::ipool->update(image);

    image->unlock();

    arrayData.push_back(xmlrpc_c::value_boolean(true));
    arrayData.push_back(xmlrpc_c::value_int(iid));

    // Copy arrayresult into retval mem space
    arrayresult = new xmlrpc_c::value_array(arrayData);
    *retval = *arrayresult;

    delete arrayresult; // and get rid of the original

    return;

error_authenticate:
    oss.str(authenticate_error(method_name));
    goto error_common;

error_image_get:
    oss.str(get_error(method_name, "IMAGE", iid));
    goto error_common;

error_authorize:
    oss.str(authorization_error(method_name, "MANAGE", "IMAGE", uid, iid));
    goto error_common;

error_publish:
    oss<< action_error(method_name, "MANAGE", "IMAGE", iid, 0)
       <<". Is the image persistent? An Image cannot be public and persistent.";
    goto error_common;

error_common:
    arrayData.push_back(xmlrpc_c::value_boolean(false));  // FAILURE
    arrayData.push_back(xmlrpc_c::value_string(oss.str()));

    NebulaLog::log("ReM",Log::ERROR,oss);

    xmlrpc_c::value_array arrayresult_error(arrayData);

    *retval = arrayresult_error;

    return;
}

/* -------------------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */
<commit_msg>Bug #340: Fixes image lock when publish fails<commit_after>/* -------------------------------------------------------------------------- */
/* Copyright 2002-2010, OpenNebula Project Leads (OpenNebula.org)             */
/*                                                                            */
/* Licensed under the Apache License, Version 2.0 (the "License"); you may    */
/* not use this file except in compliance with the License. You may obtain    */
/* a copy of the License at                                                   */
/*                                                                            */
/* http://www.apache.org/licenses/LICENSE-2.0                                 */
/*                                                                            */
/* Unless required by applicable law or agreed to in writing, software        */
/* distributed under the License is distributed on an "AS IS" BASIS,          */
/* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   */
/* See the License for the specific language governing permissions and        */
/* limitations under the License.                                             */
/* -------------------------------------------------------------------------- */

#include "RequestManager.h"

#include "NebulaLog.h"
#include "Nebula.h"

#include "AuthManager.h"

/* -------------------------------------------------------------------------- */
/* -------------------------------------------------------------------------- */

void RequestManager::ImagePublish::execute(
    xmlrpc_c::paramList const& paramList,
    xmlrpc_c::value *   const  retval)
{
    string              session;

    int                 iid;
    bool                publish_flag;
    int                 uid;

    int                 image_owner;
    bool                is_public;

    Image             * image;

    bool                response;

    ostringstream       oss;

    const string        method_name = "ImagePublish";

    vector<xmlrpc_c::value> arrayData;
    xmlrpc_c::value_array * arrayresult;

    NebulaLog::log("ReM",Log::DEBUG,"ImagePublish invoked");

    session      = xmlrpc_c::value_string (paramList.getString(0));
    iid          = xmlrpc_c::value_int    (paramList.getInt(1));
    publish_flag = xmlrpc_c::value_boolean(paramList.getBoolean(2));

    // First, we need to authenticate the user
    uid = ImagePublish::upool->authenticate(session);

    if ( uid == -1 )
    {
        goto error_authenticate;
    }

    // Get image from the ImagePool
    image = ImagePublish::ipool->get(iid,true);

    if ( image == 0 )
    {
        goto error_image_get;
    }

    image_owner = image->get_uid();
    is_public   = image->isPublic();

    image->unlock();

    //Authorize the operation
    if ( uid != 0 ) // uid == 0 means oneadmin
    {
        AuthRequest ar(uid);

        ar.add_auth(AuthRequest::IMAGE,
                    iid,
                    AuthRequest::MANAGE,
                    image_owner,
                    is_public);

        if (UserPool::authorize(ar) == -1)
        {
            goto error_authorize;
        }
    }

    // Get the image locked again
    image = ImagePublish::ipool->get(iid,true);

    if ( image == 0 )
    {
        goto error_image_get;
    }

    response = image->publish(publish_flag);

    if (!response)
    {
        image->unlock();

        goto error_publish;
    }

    ImagePublish::ipool->update(image);

    image->unlock();

    arrayData.push_back(xmlrpc_c::value_boolean(true));
    arrayData.push_back(xmlrpc_c::value_int(iid));

    // Copy arrayresult into retval mem space
    arrayresult = new xmlrpc_c::value_array(arrayData);
    *retval = *arrayresult;

    delete arrayresult; // and get rid of the original

    return;

error_authenticate:
    oss.str(authenticate_error(method_name));
    goto error_common;

error_image_get:
    oss.str(get_error(method_name, "IMAGE", iid));
    goto error_common;

error_authorize:
    oss.str(authorization_error(method_name, "MANAGE", "IMAGE", uid, iid));
    goto error_common;

error_publish:
    oss<< action_error(method_name, "MANAGE", "IMAGE", iid, 0)
       <<". Is the image persistent? An Image cannot be public and persistent.";
    goto error_common;

error_common:
    arrayData.push_back(xmlrpc_c::value_boolean(false));  // FAILURE
    arrayData.push_back(xmlrpc_c::value_string(oss.str()));

    NebulaLog::log("ReM",Log::ERROR,oss);

    xmlrpc_c::value_array arrayresult_error(arrayData);

    *retval = arrayresult_error;

    return;
}

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


<commit_before>///
/// @file  BitSieve.cpp
/// @brief Bit array for prime sieving.
///
/// Copyright (C) 2014 Kim Walisch, <kim.walisch@gmail.com>
///
/// This file is distributed under the BSD License. See the COPYING
/// file in the top level directory.
///

#if !defined(__STDC_CONSTANT_MACROS)
  #define __STDC_CONSTANT_MACROS
#endif

#include <stdint.h>
#include <algorithm>
#include <cassert>
#include <vector>

#include <BitSieve.hpp>
#include <popcount64.hpp>

namespace primecount {

const unsigned int BitSieve::unset_bit_[32] =
{
  ~(1u <<  0), ~(1u <<  1), ~(1u <<  2),
  ~(1u <<  3), ~(1u <<  4), ~(1u <<  5),
  ~(1u <<  6), ~(1u <<  7), ~(1u <<  8),
  ~(1u <<  9), ~(1u << 10), ~(1u << 11),
  ~(1u << 12), ~(1u << 13), ~(1u << 14),
  ~(1u << 15), ~(1u << 16), ~(1u << 17),
  ~(1u << 18), ~(1u << 19), ~(1u << 20),
  ~(1u << 21), ~(1u << 22), ~(1u << 23),
  ~(1u << 24), ~(1u << 25), ~(1u << 26),
  ~(1u << 27), ~(1u << 28), ~(1u << 29),
  ~(1u << 30), ~(1u << 31)
};

/// Set all bits to 1, except bits corresponding
/// to 0, 1 and even numbers > 2.
///
void BitSieve::memset(uint64_t low)
{
  unsigned mask = (low & 1) ? 0x55555555u : 0xAAAAAAAAu;
  std::fill(bits_.begin(), bits_.end(), mask);

  // correct 0, 1 and 2
  if (low <= 2)
  {
    uint32_t bitmask = ~((1u << (2 - low)) - 1);
    bits_[0] &= bitmask;
    bits_[0] |= 1 << (2 - low);
  }
}

uint64_t BitSieve::popcount(const uint64_t* bits, uint64_t start, uint64_t stop)
{
  if (start > stop)
    return 0;

  uint64_t bit_count = popcount_edges(bits, start, stop);
  uint64_t start_idx = (start >> 6) + 1;
  uint64_t limit = stop  >> 6;

  for (uint64_t i = start_idx; i < limit; i++)
    bit_count += popcount64(bits[i]);

  return bit_count;
}

uint64_t BitSieve::popcount_edges(const uint64_t* bits, uint64_t start, uint64_t stop)
{
  uint64_t index1 = start >> 6;
  uint64_t index2 = stop  >> 6;
  uint64_t mask1 = UINT64_C(0xffffffffffffffff) << (start & 63);
  uint64_t mask2 = UINT64_C(0xffffffffffffffff) >> (63 - (stop & 63));
  uint64_t count = 0;

  if (index1 == index2)
    count += popcount64(bits[index1] & (mask1 & mask2));
  else
  {
    count += popcount64(bits[index1] & mask1);
    count += popcount64(bits[index2] & mask2);
  }

  return count;
}

/// Count the number of 1 bits inside [start, stop]
uint64_t BitSieve::count(uint64_t start, uint64_t stop) const
{
  assert(stop < size_);
  const uint64_t* bits = reinterpret_cast<const uint64_t*>(bits_.data());
  return popcount(bits, start, stop);
}

} // namespace
<commit_msg>Update BitSieve.cpp<commit_after>///
/// @file  BitSieve.cpp
/// @brief Bit array for prime sieving.
///
/// Copyright (C) 2014 Kim Walisch, <kim.walisch@gmail.com>
///
/// This file is distributed under the BSD License. See the COPYING
/// file in the top level directory.
///

#if !defined(__STDC_CONSTANT_MACROS)
  #define __STDC_CONSTANT_MACROS
#endif

#include <stdint.h>
#include <algorithm>
#include <cassert>
#include <vector>

#include <BitSieve.hpp>
#include <popcount64.hpp>

namespace primecount {

const unsigned int BitSieve::unset_bit_[32] =
{
  ~(1u <<  0), ~(1u <<  1), ~(1u <<  2),
  ~(1u <<  3), ~(1u <<  4), ~(1u <<  5),
  ~(1u <<  6), ~(1u <<  7), ~(1u <<  8),
  ~(1u <<  9), ~(1u << 10), ~(1u << 11),
  ~(1u << 12), ~(1u << 13), ~(1u << 14),
  ~(1u << 15), ~(1u << 16), ~(1u << 17),
  ~(1u << 18), ~(1u << 19), ~(1u << 20),
  ~(1u << 21), ~(1u << 22), ~(1u << 23),
  ~(1u << 24), ~(1u << 25), ~(1u << 26),
  ~(1u << 27), ~(1u << 28), ~(1u << 29),
  ~(1u << 30), ~(1u << 31)
};

BitSieve::BitSieve(std::size_t size) :
  size_(size)
{
  std::size_t size32 = (size + 31) / 32;
  // align to 64-bit boundary
  if (size32 % 2 != 0)
    size32 += 2 - size32 % 2;
  bits_.resize(size32);
}

/// Set all bits to 1, except bits corresponding
/// to 0, 1 and even numbers > 2.
///
void BitSieve::memset(uint64_t low)
{
  unsigned mask = (low & 1) ? 0x55555555u : 0xAAAAAAAAu;
  std::fill(bits_.begin(), bits_.end(), mask);

  // correct 0, 1 and 2
  if (low <= 2)
  {
    uint32_t bitmask = ~((1u << (2 - low)) - 1);
    bits_[0] &= bitmask;
    bits_[0] |= 1 << (2 - low);
  }
}

uint64_t BitSieve::popcount(const uint64_t* bits, uint64_t start, uint64_t stop)
{
  if (start > stop)
    return 0;

  uint64_t bit_count = popcount_edges(bits, start, stop);
  uint64_t start_idx = (start >> 6) + 1;
  uint64_t limit = stop  >> 6;

  for (uint64_t i = start_idx; i < limit; i++)
    bit_count += popcount64(bits[i]);

  return bit_count;
}

uint64_t BitSieve::popcount_edges(const uint64_t* bits, uint64_t start, uint64_t stop)
{
  uint64_t index1 = start >> 6;
  uint64_t index2 = stop  >> 6;
  uint64_t mask1 = UINT64_C(0xffffffffffffffff) << (start & 63);
  uint64_t mask2 = UINT64_C(0xffffffffffffffff) >> (63 - (stop & 63));
  uint64_t count = 0;

  if (index1 == index2)
    count += popcount64(bits[index1] & (mask1 & mask2));
  else
  {
    count += popcount64(bits[index1] & mask1);
    count += popcount64(bits[index2] & mask2);
  }

  return count;
}

/// Count the number of 1 bits inside [start, stop]
uint64_t BitSieve::count(uint64_t start, uint64_t stop) const
{
  assert(stop < size_);
  const uint64_t* bits = reinterpret_cast<const uint64_t*>(bits_.data());
  return popcount(bits, start, stop);
}

} // namespace
<|endoftext|>