text:
/**
 * Parrot Drones Awesome Video Viewer Library
 * C wrapper functions
 *
 * Copyright (c) 2018 Parrot Drones SAS
 * Copyright (c) 2016 Aurelien Barre
 *
 * 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 copyright holders 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 HOLDERS 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 "pdraw_session.hpp"
#include "pdraw_utils.hpp"

#include <errno.h>
#include <pthread.h>

#include <string>

#include <pdraw/pdraw.h>
#define ULOG_TAG pdraw_wrapper
#include <ulog.h>
ULOG_DECLARE_TAG(pdraw_wrapper);

/* codecheck_ignore[COMPLEX_MACRO] */
#define ENUM_CASE(_prefix, _name)                                              \
	case _prefix##_name:                                                   \
		return #_name

/* NOTE: Pdraw::VideoMedia::Format and enum pdraw_video_media_format values
 * must be synchronized */
PDRAW_STATIC_ASSERT((int)Pdraw::VideoMedia::Format::FORMAT_UNKNOWN ==
		    PDRAW_VIDEO_MEDIA_FORMAT_UNKNOWN);
PDRAW_STATIC_ASSERT((int)Pdraw::VideoMedia::Format::YUV ==
		    PDRAW_VIDEO_MEDIA_FORMAT_YUV);
PDRAW_STATIC_ASSERT((int)Pdraw::VideoMedia::Format::H264 ==
		    PDRAW_VIDEO_MEDIA_FORMAT_H264);
PDRAW_STATIC_ASSERT((int)Pdraw::VideoMedia::Format::OPAQUE ==
		    PDRAW_VIDEO_MEDIA_FORMAT_OPAQUE);
#include <SViewData.h>
#include <boost/shared_ptr.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/thread/thread.hpp>
boost::shared_ptr<SViewData> _frameViewData;
boost::mutex sviewMutex;
bool _viewDataInitialized = false;
//vmeta_frame* dgdframeMeta;
class PdrawListener : public Pdraw::IPdraw::Listener {
public:
	PdrawListener(struct pdraw *pdraw,
		      const struct pdraw_cbs *cbs,
		      void *userdata) :
			mPdraw(pdraw),
			mCbs(*cbs), mUserdata(userdata)
	{
	}

	~PdrawListener() {}

	void openResponse(Pdraw::IPdraw *pdraw, int status)
	{
		if (mCbs.open_resp) {
			(*mCbs.open_resp)(mPdraw, status, mUserdata);
		}
	}

	void closeResponse(Pdraw::IPdraw *pdraw, int status)
	{
		if (mCbs.close_resp) {
			(*mCbs.close_resp)(mPdraw, status, mUserdata);
		}
	}

	void onUnrecoverableError(Pdraw::IPdraw *pdraw)
	{
		if (mCbs.unrecoverable_error) {
			(*mCbs.unrecoverable_error)(mPdraw, mUserdata);
		}
	}

	int selectDemuxerMedia(Pdraw::IPdraw *pdraw,
			       const struct pdraw_demuxer_media *medias,
			       size_t count)
	{
		if (mCbs.select_demuxer_media) {
			return (*mCbs.select_demuxer_media)(
				mPdraw, medias, count, mUserdata);
		}
		return -ENOSYS;
	}

	void onMediaAdded(Pdraw::IPdraw *pdraw,
			  const struct pdraw_media_info *info)
	{
		if (mCbs.media_added) {
			(*mCbs.media_added)(mPdraw, info, mUserdata);
		}
	}

	void onMediaRemoved(Pdraw::IPdraw *pdraw,
			    const struct pdraw_media_info *info)
	{
		if (mCbs.media_removed) {
			(*mCbs.media_removed)(mPdraw, info, mUserdata);
		}
	}

	void readyToPlay(Pdraw::IPdraw *pdraw, bool ready)
	{
		if (mCbs.ready_to_play) {
			(*mCbs.ready_to_play)(mPdraw, ready ? 1 : 0, mUserdata);
		}
	}

	void onEndOfRange(Pdraw::IPdraw *pdraw, uint64_t timestamp)
	{
		if (mCbs.end_of_range) {
			(*mCbs.end_of_range)(mPdraw, timestamp, mUserdata);
		}
	}

	void playResponse(Pdraw::IPdraw *pdraw,
			  int status,
			  uint64_t timestamp,
			  float speed)
	{
		if (mCbs.play_resp) {
			(*mCbs.play_resp)(
				mPdraw, status, timestamp, speed, mUserdata);
		}
	}

	void pauseResponse(Pdraw::IPdraw *pdraw, int status, uint64_t timestamp)
	{
		if (mCbs.pause_resp) {
			(*mCbs.pause_resp)(
				mPdraw, status, timestamp, mUserdata);
		}
	}

	void seekResponse(Pdraw::IPdraw *pdraw,
			  int status,
			  uint64_t timestamp,
			  float speed)
	{
		if (mCbs.seek_resp) {
			(*mCbs.seek_resp)(
				mPdraw, status, timestamp, speed, mUserdata);
		}
	}

	void onSocketCreated(Pdraw::IPdraw *pdraw, int fd)
	{
		if (mCbs.socket_created)
			(*mCbs.socket_created)(mPdraw, fd, mUserdata);
	}

private:
	struct pdraw *mPdraw;
	struct pdraw_cbs mCbs;
	void *mUserdata;
};

class PdrawVideoRendererListener : public Pdraw::IPdraw::VideoRendererListener {
public:
	PdrawVideoRendererListener(struct pdraw *pdraw,
				   const struct pdraw_video_renderer_cbs *cbs,
				   void *userdata) :
			mPdraw(pdraw),
			mCbs(*cbs), mUserdata(userdata), mRenderer(NULL)
	{
	}

	~PdrawVideoRendererListener() {}

	void onVideoRenderReady(Pdraw::IPdraw *pdraw,
				struct pdraw_video_renderer *renderer)
	{
		if (mCbs.render_ready)
			(*mCbs.render_ready)(mPdraw, renderer, mUserdata);
	}

	int loadVideoTexture(Pdraw::IPdraw *pdraw,
			     struct pdraw_video_renderer *renderer,
			     unsigned int textureWidth,
			     unsigned int textureHeight,
			     const struct pdraw_session_info *sessionInfo,
			     const struct vmeta_session *sessionMeta,
			     const struct pdraw_video_frame *frame,
			     const void *frameUserdata,
			     size_t frameUserdataLen)
	{
		if (mCbs.load_texture == NULL)
			return -ENOSYS;
		return (*mCbs.load_texture)(mPdraw,
					    renderer,
					    textureWidth,
					    textureHeight,
					    sessionInfo,
					    sessionMeta,
					    frame,
					    frameUserdata,
					    frameUserdataLen,
					    mUserdata);
	}

    static double rad2deg(float x) {
        return (57.2958 * x);
    }
	int renderVideoOverlay(Pdraw::IPdraw *pdraw,
			       struct pdraw_video_renderer *renderer,
			       const struct pdraw_rect *renderPos,
			       const struct pdraw_rect *contentPos,
			       const float *viewMat,
			       const float *projMat,
			       const struct pdraw_session_info *sessionInfo,
			       const struct vmeta_session *sessionMeta,
			       const struct vmeta_frame *frameMeta,
			       const struct pdraw_video_frame_extra *frameExtra)
	{
        struct vmeta_euler vme,cme;
        //dgdframeMeta = (vmeta_frame*)frameMeta;
        float cameraPan, cameraTilt,dronePitch, droneHeading, droneRoll;
        struct vmeta_location location;
        vmeta_frame_get_location(frameMeta, &location);
        vmeta_frame_get_camera_pan(frameMeta,&cameraPan);
        vmeta_frame_get_camera_tilt(frameMeta,&cameraTilt);
        cameraPan = rad2deg(cameraPan);
        cameraTilt = rad2deg(cameraTilt);
        vmeta_frame_get_drone_euler(frameMeta,&vme);
        vmeta_frame_get_frame_euler(frameMeta,&cme);
        struct vmeta_fov fov;
        float hfov,vfov;
        vmeta_frame_get_picture_h_fov(frameMeta,&hfov);
        vmeta_frame_get_picture_v_fov(frameMeta,&vfov);
        double sec,lt,ln,alt,p,h,r,hf,vf,agl,az,el;
        //hr = times / 3600.0;
        //min = (times - hr * 3600.0) / 60.0;
        //sec = (times - hr * 3600.0 - min * 60.0);
        lt = location.latitude;
        ln = location.longitude;
        alt = location.altitude;
        p = rad2deg(cme.pitch);
        h = rad2deg(cme.yaw);
        r = rad2deg(cme.roll);

        {
            boost::mutex::scoped_lock scoped_lock(sviewMutex);
            if(frameMeta != NULL)
                _viewDataInitialized = true;
            _frameViewData.reset(new SViewData);
            _frameViewData->dfov = vfov;
            _frameViewData->dVehicleRoll = 0.0;

            _frameViewData->dfov = vfov;
            _frameViewData->dFovVerticalAngle = vfov;
            _frameViewData->dFovHorizontalAngle = hfov;
            _frameViewData->dVehicleAltitude = alt;
            //46.764594, -92.151235 Lincoln Park
            //esko 46.685940, -92.363685
            _frameViewData->dVehicleLat =lt;
            _frameViewData->dVehicleLon =ln;
            _frameViewData->dCameraPitch = p;
            _frameViewData->dCameraHeading = h;
            _frameViewData->dCameraRoll = r;
            _frameViewData->dVehiclePitch = 0.0;
            _frameViewData->dVehicleHeading = 0.0;
            _frameViewData->dVehicleRoll = 0.0;
            _frameViewData->dVehicldAltitudeAGL = alt;
        }

		if (mCbs.render_overlay == NULL)
			return -ENOSYS;
		if ((renderer == NULL) || (renderPos == NULL) ||
		    (contentPos == NULL) || (viewMat == NULL) ||
		    (projMat == NULL) || (sessionInfo == NULL) ||
		    (sessionMeta == NULL) || (frameMeta == NULL))
			return -EINVAL;
		(*mCbs.render_overlay)(mPdraw,
				       renderer,
				       renderPos,
				       contentPos,
				       viewMat,
				       projMat,
				       sessionInfo,
				       sessionMeta,
				       frameMeta,
				       frameExtra,
				       mUserdata);
		return 0;
	}

	struct pdraw_video_renderer *getVideoRenderer()
	{
		return mRenderer;
	}

	void setVideoRenderer(struct pdraw_video_renderer *renderer)
	{
		mRenderer = renderer;
	}

private:
	struct pdraw *mPdraw;
	struct pdraw_video_renderer_cbs mCbs;
	void *mUserdata;
	struct pdraw_video_renderer *mRenderer;
};

class PdrawVideoSinkListener : public Pdraw::IPdraw::VideoSinkListener {
public:
	PdrawVideoSinkListener(struct pdraw *pdraw,
			       const struct pdraw_video_sink_cbs *cbs,
			       void *userdata) :
			mPdraw(pdraw),
			mCbs(*cbs), mUserdata(userdata), mSink(NULL)
	{
	}

	~PdrawVideoSinkListener() {}

	void onVideoSinkFlush(Pdraw::IPdraw *pdraw,
			      struct pdraw_video_sink *sink)
	{
		if (mCbs.flush)
			(*mCbs.flush)(mPdraw, mSink, mUserdata);
	}

	struct pdraw_video_sink *getVideoSink()
	{
		return mSink;
	}

	void setVideoSink(struct pdraw_video_sink *sink)
	{
		mSink = sink;
	}

private:
	struct pdraw *mPdraw;
	struct pdraw_video_sink_cbs mCbs;
	void *mUserdata;
	struct pdraw_video_sink *mSink;
};

struct pdraw {
	Pdraw::IPdraw *pdraw;
	PdrawListener *listener;
	pthread_mutex_t mutex;
	std::vector<PdrawVideoRendererListener *> *videoRendererListeners;
	std::vector<PdrawVideoSinkListener *> *videoSinkListeners;
};

int pdraw_new(struct pomp_loop *loop,
	      const struct pdraw_cbs *cbs,
	      void *userdata,
	      struct pdraw **ret_obj)
{
	int ret = 0;
	struct pdraw *pdraw;

	if (loop == NULL)
		return -EINVAL;
	if (cbs == NULL)
		return -EINVAL;
	if (ret_obj == NULL)
		return -EINVAL;

	pdraw = (struct pdraw *)calloc(1, sizeof(*pdraw));
	if (pdraw == NULL)
		return -ENOMEM;

	ret = pthread_mutex_init(&pdraw->mutex, NULL);
	if (ret != 0) {
		free(pdraw);
		return -ret;
	}

	pdraw->videoRendererListeners =
		new std::vector<PdrawVideoRendererListener *>();
	if (pdraw->videoRendererListeners == NULL) {
		ret = -ENOMEM;
		goto error;
	}

	pdraw->videoSinkListeners = new std::vector<PdrawVideoSinkListener *>();
	if (pdraw->videoSinkListeners == NULL) {
		ret = -ENOMEM;
		goto error;
	}

	pdraw->listener = new PdrawListener(pdraw, cbs, userdata);
	if (pdraw->listener == NULL) {
		ret = -ENOMEM;
		goto error;
	}

	pdraw->pdraw = new Pdraw::Session(loop, pdraw->listener);
	if (pdraw->pdraw == NULL) {
		ret = -ENOMEM;
		goto error;
	}

	*ret_obj = pdraw;
	return 0;

error:
	pdraw_destroy(pdraw);
	return ret;
}

int pdraw_destroy(struct pdraw *pdraw)
{
	if (pdraw == NULL)
		return -EINVAL;
	if (pdraw->pdraw != NULL)
		delete pdraw->pdraw;
	if (pdraw->listener != NULL)
		delete pdraw->listener;

	if (pdraw->videoSinkListeners != NULL) {
		std::vector<PdrawVideoSinkListener *>::iterator l =
			pdraw->videoSinkListeners->begin();
		while (l != pdraw->videoSinkListeners->end()) {
			if (*l != NULL)
				delete (*l);
			l++;
		}
		pdraw->videoSinkListeners->clear();
		delete pdraw->videoSinkListeners;
	}

	pthread_mutex_lock(&pdraw->mutex);
	if (pdraw->videoRendererListeners != NULL) {
		std::vector<PdrawVideoRendererListener *>::iterator r =
			pdraw->videoRendererListeners->begin();
		while (r != pdraw->videoRendererListeners->end()) {
			if (*r != NULL)
				delete (*r);
			r++;
		}
		pdraw->videoRendererListeners->clear();
		delete pdraw->videoRendererListeners;
	}
	pthread_mutex_unlock(&pdraw->mutex);
	pthread_mutex_destroy(&pdraw->mutex);

	free(pdraw);
	return 0;
}

int pdraw_open_url(struct pdraw *pdraw, const char *url)
{
	if ((pdraw == NULL) || (url == NULL))
		return -EINVAL;

	std::string u(url);
	return pdraw->pdraw->open(u);
}

int pdraw_open_single_stream(struct pdraw *pdraw,
			     const char *local_addr,
			     uint16_t local_stream_port,
			     uint16_t local_control_port,
			     const char *remote_addr,
			     uint16_t remote_stream_port,
			     uint16_t remote_control_port)
{
	if ((pdraw == NULL) || (local_addr == NULL) || (remote_addr == NULL))
		return -EINVAL;

	std::string local(local_addr);
	std::string remote(remote_addr);
	return pdraw->pdraw->open(local,
				  local_stream_port,
				  local_control_port,
				  remote,
				  remote_stream_port,
				  remote_control_port);
}

int pdraw_open_url_mux(struct pdraw *pdraw,
		       const char *url,
		       struct mux_ctx *mux)
{
	if ((pdraw == NULL) || (url == NULL) || (mux == NULL))
		return -EINVAL;

	std::string u(url);
	return pdraw->pdraw->open(u, mux);
}

int pdraw_close(struct pdraw *pdraw)
{
	if (pdraw == NULL)
		return -EINVAL;

	return pdraw->pdraw->close();
}

uint16_t pdraw_get_single_stream_local_stream_port(struct pdraw *pdraw)
{
	if (pdraw == NULL) {
		ULOG_ERRNO("pdraw", EINVAL);
		return 0;
	}

	return pdraw->pdraw->getSingleStreamLocalStreamPort();
}

uint16_t pdraw_get_single_stream_local_control_port(struct pdraw *pdraw)
{
	if (pdraw == NULL) {
		ULOG_ERRNO("pdraw", EINVAL);
		return 0;
	}

	return pdraw->pdraw->getSingleStreamLocalControlPort();
}

int pdraw_is_ready_to_play(struct pdraw *pdraw)
{
	if (pdraw == NULL)
		return -EINVAL;

	return (pdraw->pdraw->isReadyToPlay()) ? 1 : 0;
}

int pdraw_is_paused(struct pdraw *pdraw)
{
	if (pdraw == NULL)
		return -EINVAL;

	return (pdraw->pdraw->isPaused()) ? 1 : 0;
}

int pdraw_play(struct pdraw *pdraw)
{
	if (pdraw == NULL)
		return -EINVAL;

	return pdraw->pdraw->play();
}

int pdraw_play_with_speed(struct pdraw *pdraw, float speed)
{
	if (pdraw == NULL)
		return -EINVAL;

	return pdraw->pdraw->play(speed);
}

int pdraw_pause(struct pdraw *pdraw)
{
	if (pdraw == NULL)
		return -EINVAL;

	return pdraw->pdraw->pause();
}

int pdraw_previous_frame(struct pdraw *pdraw)
{
	if (pdraw == NULL)
		return -EINVAL;

	return pdraw->pdraw->previousFrame();
}

int pdraw_next_frame(struct pdraw *pdraw)
{
	if (pdraw == NULL)
		return -EINVAL;

	return pdraw->pdraw->nextFrame();
}

int pdraw_seek(struct pdraw *pdraw, int64_t delta, int exact)
{
	if (pdraw == NULL)
		return -EINVAL;

	return pdraw->pdraw->seek(delta, exact ? true : false);
}

int pdraw_seek_forward(struct pdraw *pdraw, uint64_t delta, int exact)
{
	if (pdraw == NULL)
		return -EINVAL;

	return pdraw->pdraw->seekForward(delta, exact ? true : false);
}

int pdraw_seek_back(struct pdraw *pdraw, uint64_t delta, int exact)
{
	if (pdraw == NULL)
		return -EINVAL;

	return pdraw->pdraw->seekBack(delta, exact ? true : false);
}

int pdraw_seek_to(struct pdraw *pdraw, uint64_t timestamp, int exact)
{
	if (pdraw == NULL)
		return -EINVAL;

	return pdraw->pdraw->seekTo(timestamp, exact ? true : false);
}

uint64_t pdraw_get_duration(struct pdraw *pdraw)
{
	if (pdraw == NULL)
		return 0;

	return pdraw->pdraw->getDuration();
}

uint64_t pdraw_get_current_time(struct pdraw *pdraw)
{
	if (pdraw == NULL)
		return 0;

	return pdraw->pdraw->getCurrentTime();
}

int pdraw_start_video_renderer(struct pdraw *pdraw,
			       const struct pdraw_rect *render_pos,
			       const struct pdraw_video_renderer_params *params,
			       const struct pdraw_video_renderer_cbs *cbs,
			       void *userdata,
			       struct pdraw_video_renderer **ret_obj)
{
	return pdraw_start_video_renderer_egl(
		pdraw, render_pos, params, cbs, userdata, NULL, ret_obj);
}

#include <iostream>
int pdraw_start_video_renderer_egl(
	struct pdraw *pdraw,
	const struct pdraw_rect *render_pos,
	const struct pdraw_video_renderer_params *params,
	const struct pdraw_video_renderer_cbs *cbs,
	void *userdata,
	struct egl_display *egl_display,
	struct pdraw_video_renderer **ret_obj)
{
	int ret = 0;
	struct pdraw_video_renderer *renderer = NULL;
	if (pdraw == NULL)
		return -EINVAL;
	if (ret_obj == NULL)
		return -EINVAL;

	pthread_mutex_lock(&pdraw->mutex);
	PdrawVideoRendererListener *l =
		new PdrawVideoRendererListener(pdraw, cbs, userdata);
	if (l == NULL) {
		ULOGE("failed to create video renderer listener");
		ret = -ENOMEM;
		goto error;
	}

	ret = pdraw->pdraw->startVideoRenderer(
		render_pos, params, l, &renderer, egl_display);
	if (ret < 0) {
        //std::cout<<"dgd fail"<<std::endl;
		delete l;
		goto error;
	}

	pdraw->videoRendererListeners->push_back(l);
	l->setVideoRenderer(renderer);
	pthread_mutex_unlock(&pdraw->mutex);
	*ret_obj = renderer;

	return 0;
error:
	pthread_mutex_unlock(&pdraw->mutex);
	return ret;
}

int pdraw_stop_video_renderer(struct pdraw *pdraw,
			      struct pdraw_video_renderer *renderer)
{
	int ret = 0;
	std::vector<PdrawVideoRendererListener *>::iterator l;

	if (pdraw == NULL)
		return -EINVAL;

	pthread_mutex_lock(&pdraw->mutex);
	ret = pdraw->pdraw->stopVideoRenderer(renderer);
	if (ret < 0)
		goto out;

	l = pdraw->videoRendererListeners->begin();
	while (l != pdraw->videoRendererListeners->end()) {
		if ((*l)->getVideoRenderer() != renderer) {
			l++;
			continue;
		}

		delete *l;
		pdraw->videoRendererListeners->erase(l);
		break;
	}

out:
	pthread_mutex_unlock(&pdraw->mutex);
	return ret;
}

int pdraw_resize_video_renderer(struct pdraw *pdraw,
				struct pdraw_video_renderer *renderer,
				const struct pdraw_rect *render_pos)
{
	if (pdraw == NULL)
		return -EINVAL;

	return pdraw->pdraw->resizeVideoRenderer(renderer, render_pos);
}

int pdraw_set_video_renderer_params(
	struct pdraw *pdraw,
	struct pdraw_video_renderer *renderer,
	const struct pdraw_video_renderer_params *params)
{
	if (pdraw == NULL)
		return -EINVAL;

	return pdraw->pdraw->setVideoRendererParams(renderer, params);
}

int pdraw_get_video_renderer_params(struct pdraw *pdraw,
				    struct pdraw_video_renderer *renderer,
				    struct pdraw_video_renderer_params *params)
{
	if (pdraw == NULL)
		return -EINVAL;

	return pdraw->pdraw->getVideoRendererParams(renderer, params);
}

int pdraw_render_video(struct pdraw *pdraw,
		       struct pdraw_video_renderer *renderer,
		       struct pdraw_rect *content_pos)
{
	return pdraw_render_video_mat(pdraw, renderer, content_pos, NULL, NULL);
}

int pdraw_render_video_mat(struct pdraw *pdraw,
			   struct pdraw_video_renderer *renderer,
			   struct pdraw_rect *content_pos,
			   const float *view_mat,
			   const float *proj_mat)
{
	if (pdraw == NULL)
		return -EINVAL;

	return pdraw->pdraw->renderVideo(
		renderer, content_pos, view_mat, proj_mat);
}

int pdraw_start_video_sink(struct pdraw *pdraw,
			   unsigned int media_id,
			   const struct pdraw_video_sink_params *params,
			   const struct pdraw_video_sink_cbs *cbs,
			   void *userdata,
			   struct pdraw_video_sink **ret_obj)
{
	int res;
	struct pdraw_video_sink *sink = NULL;

	if (pdraw == NULL)
		return -EINVAL;
	if (params == NULL)
		return -EINVAL;
	if ((cbs == NULL) || (cbs->flush == NULL))
		return -EINVAL;
	if (ret_obj == NULL)
		return -EINVAL;

	PdrawVideoSinkListener *videoSinkListener =
		new PdrawVideoSinkListener(pdraw, cbs, userdata);
	if (videoSinkListener == NULL) {
		ULOGE("failed to create video sink listener");
		return -ENOMEM;
	}

	res = pdraw->pdraw->startVideoSink(
		media_id, params, videoSinkListener, &sink);
	if (res < 0) {
		delete videoSinkListener;
		return res;
	}

	videoSinkListener->setVideoSink(sink);
	pdraw->videoSinkListeners->push_back(videoSinkListener);
	*ret_obj = sink;
	return 0;
}

int pdraw_stop_video_sink(struct pdraw *pdraw, struct pdraw_video_sink *sink)
{
	int res;

	if (pdraw == NULL)
		return -EINVAL;
	if (sink == NULL)
		return -EINVAL;

	res = pdraw->pdraw->stopVideoSink(sink);
	if (res < 0) {
		ULOG_ERRNO("stopVideoSink", -res);
		return res;
	}

	std::vector<PdrawVideoSinkListener *>::iterator l =
		pdraw->videoSinkListeners->begin();
	while (l != pdraw->videoSinkListeners->end()) {
		if ((*l)->getVideoSink() != sink) {
			l++;
			continue;
		}
		delete *l;
		pdraw->videoSinkListeners->erase(l);
		break;
	}

	return 0;
}

int pdraw_resync_video_sink(struct pdraw *pdraw, struct pdraw_video_sink *sink)
{
	int res;

	if (pdraw == NULL)
		return -EINVAL;
	if (sink == NULL)
		return -EINVAL;

	res = pdraw->pdraw->resyncVideoSink(sink);
	if (res < 0) {
		ULOG_ERRNO("resyncVideoSink", -res);
		return res;
	}

	return 0;
}

struct vbuf_queue *pdraw_get_video_sink_queue(struct pdraw *pdraw,
					      struct pdraw_video_sink *sink)
{
	if (pdraw == NULL) {
		ULOG_ERRNO("pdraw", EINVAL);
		return NULL;
	}
	if (sink == NULL) {
		ULOG_ERRNO("sink", EINVAL);
		return NULL;
	}

	return pdraw->pdraw->getVideoSinkQueue(sink);
}

int pdraw_video_sink_queue_flushed(struct pdraw *pdraw,
				   struct pdraw_video_sink *sink)
{
	int res;

	if (pdraw == NULL)
		return -EINVAL;
	if (sink == NULL)
		return -EINVAL;

	res = pdraw->pdraw->videoSinkQueueFlushed(sink);
	if (res < 0) {
		ULOG_ERRNO("videoSinkQueueFlushed", -res);
		return res;
	}

	return 0;
}

enum pdraw_session_type pdraw_get_session_type(struct pdraw *pdraw)
{
	if (pdraw == NULL)
		return PDRAW_SESSION_TYPE_UNKNOWN;

	return pdraw->pdraw->getSessionType();
}

int pdraw_get_self_friendly_name(struct pdraw *pdraw, char *str, size_t len)
{
	if (pdraw == NULL)
		return -EINVAL;

	std::string fn;
	pdraw->pdraw->getSelfFriendlyName(&fn);
	if ((str) && (fn.length() >= len))
		return -ENOBUFS;

	if (str)
		strcpy(str, fn.c_str());
	return 0;
}

int pdraw_set_self_friendly_name(struct pdraw *pdraw, const char *friendly_name)
{
	if (pdraw == NULL)
		return -EINVAL;

	std::string fn(friendly_name);
	pdraw->pdraw->setSelfFriendlyName(fn);
	return 0;
}

int pdraw_get_self_serial_number(struct pdraw *pdraw, char *str, size_t len)
{
	if (pdraw == NULL)
		return -EINVAL;

	std::string sn;
	pdraw->pdraw->getSelfSerialNumber(&sn);
	if ((str) && (sn.length() >= len))
		return -ENOBUFS;

	if (str)
		strcpy(str, sn.c_str());
	return 0;
}

int pdraw_set_self_serial_number(struct pdraw *pdraw, const char *serial_number)
{
	if (pdraw == NULL)
		return -EINVAL;

	std::string sn(serial_number);
	pdraw->pdraw->setSelfSerialNumber(sn);
	return 0;
}

int pdraw_get_self_software_version(struct pdraw *pdraw, char *str, size_t len)
{
	if (pdraw == NULL)
		return -EINVAL;

	std::string sv;
	pdraw->pdraw->getSelfSoftwareVersion(&sv);
	if ((str) && (sv.length() >= len))
		return -ENOBUFS;

	if (str)
		strcpy(str, sv.c_str());
	return 0;
}

int pdraw_set_self_software_version(struct pdraw *pdraw,
				    const char *software_version)
{
	if (pdraw == NULL)
		return -EINVAL;

	std::string sv(software_version);
	pdraw->pdraw->setSelfSoftwareVersion(sv);
	return 0;
}

int pdraw_is_self_pilot(struct pdraw *pdraw)
{
	if (pdraw == NULL)
		return -EINVAL;

	return (pdraw->pdraw->isSelfPilot()) ? 1 : 0;
}

int pdraw_set_self_pilot(struct pdraw *pdraw, int is_pilot)
{
	if (pdraw == NULL)
		return -EINVAL;

	pdraw->pdraw->setSelfPilot((is_pilot) ? true : false);
	return 0;
}

int pdraw_get_peer_session_metadata(struct pdraw *pdraw,
				    struct vmeta_session *session)
{
	if (pdraw == NULL)
		return -EINVAL;

	pdraw->pdraw->getPeerSessionMetadata(session);
	return 0;
}

enum pdraw_drone_model pdraw_get_peer_drone_model(struct pdraw *pdraw)
{
	if (pdraw == NULL)
		return PDRAW_DRONE_MODEL_UNKNOWN;

	return pdraw->pdraw->getPeerDroneModel();
}

enum pdraw_pipeline_mode pdraw_get_pipeline_mode_setting(struct pdraw *pdraw)
{
	if (pdraw == NULL)
		return (enum pdraw_pipeline_mode)(-EINVAL);

	return pdraw->pdraw->getPipelineModeSetting();
}

int pdraw_set_pipeline_mode_setting(struct pdraw *pdraw,
				    enum pdraw_pipeline_mode mode)
{
	if (pdraw == NULL)
		return -EINVAL;

	pdraw->pdraw->setPipelineModeSetting(mode);
	return 0;
}

int pdraw_get_display_screen_settings(struct pdraw *pdraw,
				      float *xdpi,
				      float *ydpi,
				      float *device_margin_top,
				      float *device_margin_bottom,
				      float *device_margin_left,
				      float *device_margin_right)
{
	if (pdraw == NULL)
		return -EINVAL;

	pdraw->pdraw->getDisplayScreenSettings(xdpi,
					       ydpi,
					       device_margin_top,
					       device_margin_bottom,
					       device_margin_left,
					       device_margin_right);
	return 0;
}

int pdraw_set_display_screen_settings(struct pdraw *pdraw,
				      float xdpi,
				      float ydpi,
				      float device_margin_top,
				      float device_margin_bottom,
				      float device_margin_left,
				      float device_margin_right)
{
	if (pdraw == NULL)
		return -EINVAL;

	pdraw->pdraw->setDisplayScreenSettings(xdpi,
					       ydpi,
					       device_margin_top,
					       device_margin_bottom,
					       device_margin_left,
					       device_margin_right);
	return 0;
}

enum pdraw_hmd_model pdraw_get_hmd_model_setting(struct pdraw *pdraw)
{
	if (pdraw == NULL)
		return (enum pdraw_hmd_model)(-EINVAL);

	return pdraw->pdraw->getHmdModelSetting();
}

int pdraw_set_hmd_model_setting(struct pdraw *pdraw,
				enum pdraw_hmd_model hmd_model)
{
	if (pdraw == NULL)
		return -EINVAL;

	pdraw->pdraw->setHmdModelSetting(hmd_model);
	return 0;
}

int pdraw_set_android_jvm(struct pdraw *pdraw, void *jvm)
{
	if (pdraw == NULL)
		return -EINVAL;

	pdraw->pdraw->setAndroidJvm(jvm);
	return 0;
}

const char *pdraw_drone_model_str(enum pdraw_drone_model val)
{
	return pdraw_droneModelStr(val);
}

const char *pdraw_hmd_model_str(enum pdraw_hmd_model val)
{
	return pdraw_hmdModelStr(val);
}

const char *pdraw_pipeline_mode_str(enum pdraw_pipeline_mode val)
{
	return pdraw_pipelineModeStr(val);
}

const char *pdraw_session_type_str(enum pdraw_session_type val)
{
	return pdraw_sessionTypeStr(val);
}

const char *pdraw_media_type_str(enum pdraw_media_type val)
{
	return pdraw_mediaTypeStr(val);
}

const char *pdraw_video_media_format_str(enum pdraw_video_media_format val)
{
	return pdraw_videoMediaFormatStr(val);
}

const char *pdraw_yuv_format_str(enum pdraw_yuv_format val)
{
	return pdraw_yuvFormatStr(val);
}

const char *pdraw_h264_format_str(enum pdraw_h264_format val)
{
	return pdraw_h264FormatStr(val);
}

const char *pdraw_video_type_str(enum pdraw_video_type val)
{
	return pdraw_videoTypeStr(val);
}

const char *pdraw_histogram_channel_str(enum pdraw_histogram_channel val)
{
	return pdraw_histogramChannelStr(val);
}

const char *
pdraw_video_renderer_fill_mode_str(enum pdraw_video_renderer_fill_mode val)
{
	return pdraw_videoRendererFillModeStr(val);
}

const char *pdraw_video_renderer_transition_flag_str(
	enum pdraw_video_renderer_transition_flag val)
{
	return pdraw_videoRendererTransitionFlagStr(val);
}

int pdraw_video_frame_to_json_str(const struct pdraw_video_frame *frame,
				  char *str,
				  unsigned int len)
{
	return pdraw_frameMetadataToJsonStr(frame, str, len);
}

int pdraw_video_frame_to_json(const struct pdraw_video_frame *frame,
			      struct json_object *jobj)
{
	return pdraw_frameMetadataToJson(frame, jobj);
}

int pdraw_pack_yuv_frame(const struct pdraw_video_frame *in_frame,
			 struct pdraw_video_frame *out_frame,
			 struct vbuf_buffer *out_buf)
{
	return pdraw_packYUVFrame(in_frame, out_frame, out_buf);
}

max_stars_repo_name: Rapid-Imaging-Tech/pdraw

max_issues_repo_name: Rapid-Imaging-Tech/pdraw

max_forks_repo_name: Rapid-Imaging-Tech/pdraw


text:
#pragma once

/*
 * Convert a CFG into a BGL (Boost Graph Library) graph
 */
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/properties.hpp>
#include <boost/iterator/transform_iterator.hpp>

#include <crab/cfg/cfg.hpp>

namespace crab {
namespace cfg {
namespace graph {
template <typename G>
struct mk_in_edge : public std::unary_function<
                        typename boost::graph_traits<G>::vertex_descriptor,
                        typename boost::graph_traits<G>::edge_descriptor> {
  using Node = typename boost::graph_traits<G>::vertex_descriptor;
  using Edge = typename boost::graph_traits<G>::edge_descriptor;

  Node _dst;
  mk_in_edge() {}
  mk_in_edge(const Node &dst) : _dst(dst) {}
  Edge operator()(const Node &src) const { return Edge(src, _dst); }
};

template <typename G>
struct mk_out_edge : public std::unary_function<
                         typename boost::graph_traits<G>::vertex_descriptor,
                         typename boost::graph_traits<G>::edge_descriptor> {
  using Node = typename boost::graph_traits<G>::vertex_descriptor;
  using Edge = typename boost::graph_traits<G>::edge_descriptor;

  Node _src;
  mk_out_edge() {}
  mk_out_edge(const Node &src) : _src(src) {}
  Edge operator()(const Node &dst) const { return Edge(_src, dst); }
};
} // end namespace graph
} // end namespace cfg
} // end namespace crab

namespace boost {

// cfg
template <typename BasicBlockLabel, typename VariableName, typename Number>
struct graph_traits<crab::cfg::cfg<BasicBlockLabel, VariableName, Number>> {
  using graph_t = crab::cfg::cfg<BasicBlockLabel, VariableName, Number>;
  using vertex_descriptor = BasicBlockLabel;
  using edge_descriptor = std::pair<vertex_descriptor, vertex_descriptor>;
  using const_edge_descriptor =
      std::pair<const vertex_descriptor, const vertex_descriptor>;

  using edge_parallel_category = disallow_parallel_edge_tag;
  using directed_category = bidirectional_tag;
  struct this_graph_tag : virtual bidirectional_graph_tag,
                          virtual vertex_list_graph_tag {};
  using traversal_category = this_graph_tag;

  using vertices_size_type = size_t;
  using edges_size_type = size_t;
  using degree_size_type = size_t;

  static vertex_descriptor null_vertex() {
    if (std::is_pointer<vertex_descriptor>::value)
      return nullptr;
    else {
      // XXX: if vertex_descriptor is a basic type then
      // null_vertex will return an undefined value, otherwise it
      // will return the result of calling the default
      // constructor.
      vertex_descriptor n;
      return n;
    }
  }

  // iterator of basic_block_label_t's
  using vertex_iterator = typename graph_t::label_iterator;
  // iterator of pairs of basic_block_label_t's
  using in_edge_iterator =
      transform_iterator<crab::cfg::graph::mk_in_edge<graph_t>,
                         typename graph_t::pred_iterator>;
  // iterator of pairs of basic_block_label_t's
  using out_edge_iterator =
      transform_iterator<crab::cfg::graph::mk_out_edge<graph_t>,
                         typename graph_t::succ_iterator>;
}; // end class graph_traits

// cfg_ref
template <class CFG> struct graph_traits<crab::cfg::cfg_ref<CFG>> {
  using graph_t = crab::cfg::cfg_ref<CFG>;
  using vertex_descriptor = typename graph_t::basic_block_label_t;
  using edge_descriptor = std::pair<vertex_descriptor, vertex_descriptor>;
  using const_edge_descriptor =
      std::pair<const vertex_descriptor, const vertex_descriptor>;

  using edge_parallel_category = disallow_parallel_edge_tag;
  using directed_category = bidirectional_tag;
  struct this_graph_tag : virtual bidirectional_graph_tag,
                          virtual vertex_list_graph_tag {};
  using traversal_category = this_graph_tag;

  using vertices_size_type = size_t;
  using edges_size_type = size_t;
  using degree_size_type = size_t;

  static vertex_descriptor null_vertex() {
    if (std::is_pointer<vertex_descriptor>::value)
      return nullptr;
    else {
      // XXX: if vertex_descriptor is a basic type then
      // null_vertex will return an undefined value, otherwise it
      // will return the result of calling the default
      // constructor.
      vertex_descriptor n;
      return n;
    }
  }

  using vertex_iterator = typename graph_t::label_iterator;
  using in_edge_iterator =
      boost::transform_iterator<crab::cfg::graph::mk_in_edge<graph_t>,
                                typename graph_t::pred_iterator>;
  using out_edge_iterator =
      boost::transform_iterator<crab::cfg::graph::mk_out_edge<graph_t>,
                                typename graph_t::succ_iterator>;
}; // end class graph_traits;

// cfg_rev
template <class CFG> struct graph_traits<crab::cfg::cfg_rev<CFG>> {
  using graph_t = crab::cfg::cfg_rev<CFG>;
  using vertex_descriptor = typename graph_t::basic_block_label_t;
  using edge_descriptor = std::pair<vertex_descriptor, vertex_descriptor>;
  using const_edge_descriptor =
      std::pair<const vertex_descriptor, const vertex_descriptor>;

  using edge_parallel_category = disallow_parallel_edge_tag;
  using directed_category = bidirectional_tag;
  struct this_graph_tag : virtual bidirectional_graph_tag,
                          virtual vertex_list_graph_tag {};
  using traversal_category = this_graph_tag;

  using vertices_size_type = size_t;
  using edges_size_type = size_t;
  using degree_size_type = size_t;

  static vertex_descriptor null_vertex() {
    if (std::is_pointer<vertex_descriptor>::value)
      return nullptr;
    else {
      // XXX: if vertex_descriptor is a basic type then
      // null_vertex will return an undefined value, otherwise it
      // will return the result of calling the default
      // constructor.
      vertex_descriptor n;
      return n;
    }
  }

  using vertex_iterator = typename graph_t::label_iterator;
  using in_edge_iterator =
      boost::transform_iterator<crab::cfg::graph::mk_in_edge<graph_t>,
                                typename graph_t::pred_iterator>;
  using out_edge_iterator =
      boost::transform_iterator<crab::cfg::graph::mk_out_edge<graph_t>,
                                typename graph_t::succ_iterator>;
}; // end class graph_traits
} // namespace boost

// XXX: do not put it in the boost namespace because it won't compile
namespace crab {
namespace cfg {

// cfg

//// boost/graph/graph_traits.hpp has already default source/target
//// functions in case the edge_descriptor is std::pair.

// this is not part of BGL but needed by Crab's algorithms
template <typename BasicBlockLabel, typename VariableName, typename Number>
typename boost::graph_traits<
    cfg<BasicBlockLabel, VariableName, Number>>::vertex_descriptor
entry(const cfg<BasicBlockLabel, VariableName, Number> &g) {
  return g.entry();
}

template <typename BasicBlockLabel, typename VariableName, typename Number>
inline std::pair<typename boost::graph_traits<cfg<BasicBlockLabel, VariableName,
                                                  Number>>::vertex_iterator,
                 typename boost::graph_traits<cfg<BasicBlockLabel, VariableName,
                                                  Number>>::vertex_iterator>
vertices(cfg<BasicBlockLabel, VariableName, Number> g) {
  return std::make_pair(g.label_begin(), g.label_end());
}

template <typename BasicBlockLabel, typename VariableName, typename Number>
inline std::pair<typename boost::graph_traits<cfg<BasicBlockLabel, VariableName,
                                                  Number>>::out_edge_iterator,
                 typename boost::graph_traits<cfg<BasicBlockLabel, VariableName,
                                                  Number>>::out_edge_iterator>
out_edges(typename boost::graph_traits<
              cfg<BasicBlockLabel, VariableName, Number>>::vertex_descriptor v,
          cfg<BasicBlockLabel, VariableName, Number> g) {
  using G = cfg<BasicBlockLabel, VariableName, Number>;

  auto &node = g.get_node(v);
  auto p = node.next_blocks();
  return std::make_pair(
      boost::make_transform_iterator(p.first, graph::mk_out_edge<G>(v)),
      boost::make_transform_iterator(p.second, graph::mk_out_edge<G>(v)));
}

template <typename BasicBlockLabel, typename VariableName, typename Number>
inline std::pair<typename boost::graph_traits<cfg<BasicBlockLabel, VariableName,
                                                  Number>>::in_edge_iterator,
                 typename boost::graph_traits<cfg<BasicBlockLabel, VariableName,
                                                  Number>>::in_edge_iterator>
in_edges(typename boost::graph_traits<
             cfg<BasicBlockLabel, VariableName, Number>>::vertex_descriptor v,
         cfg<BasicBlockLabel, VariableName, Number> g) {
  using G = cfg<BasicBlockLabel, VariableName, Number>;

  auto &node = g.get_node(v);
  auto p = node.prev_blocks();
  return std::make_pair(
      boost::make_transform_iterator(p.first, graph::mk_in_edge<G>(v)),
      boost::make_transform_iterator(p.second, graph::mk_in_edge<G>(v)));
}

template <typename BasicBlockLabel, typename VariableName, typename Number>
typename boost::graph_traits<
    cfg<BasicBlockLabel, VariableName, Number>>::vertices_size_type
num_vertices(cfg<BasicBlockLabel, VariableName, Number> g) {
  return std::distance(g.label_begin(), g.label_end());
}

template <typename BasicBlockLabel, typename VariableName, typename Number>
typename boost::graph_traits<
    cfg<BasicBlockLabel, VariableName, Number>>::degree_size_type
in_degree(typename boost::graph_traits<
              cfg<BasicBlockLabel, VariableName, Number>>::vertex_descriptor v,
          cfg<BasicBlockLabel, VariableName, Number> g) {
  auto preds = g.prev_nodes(v);
  return std::distance(preds.begin(), preds.end());
}

template <typename BasicBlockLabel, typename VariableName, typename Number>
typename boost::graph_traits<
    cfg<BasicBlockLabel, VariableName, Number>>::degree_size_type
out_degree(typename boost::graph_traits<
               cfg<BasicBlockLabel, VariableName, Number>>::vertex_descriptor v,
           cfg<BasicBlockLabel, VariableName, Number> g) {
  auto succs = g.next_nodes(v);
  return std::distance(succs.begin(), succs.end());
}

template <typename BasicBlockLabel, typename VariableName, typename Number>
typename boost::graph_traits<
    cfg<BasicBlockLabel, VariableName, Number>>::degree_size_type
degree(typename boost::graph_traits<
           cfg<BasicBlockLabel, VariableName, Number>>::vertex_descriptor v,
       cfg<BasicBlockLabel, VariableName, Number> g) {
  return out_degree(v, g) + in_degree(v, g);
}

// cfg_ref

//// boost/graph/graph_traits.hpp has already default source/target
//// functions in case the edge_descriptor is std::pair.

// this is not part of BGL but needed by Crab's algorithms
template <class CFG>
typename boost::graph_traits<cfg_ref<CFG>>::vertex_descriptor
entry(const cfg_ref<CFG> &g) {
  return g.entry();
}

template <class CFG>
inline std::pair<typename boost::graph_traits<cfg_ref<CFG>>::vertex_iterator,
                 typename boost::graph_traits<cfg_ref<CFG>>::vertex_iterator>
vertices(cfg_ref<CFG> g) {
  return std::make_pair(g.label_begin(), g.label_end());
}

template <class CFG>
inline std::pair<typename boost::graph_traits<cfg_ref<CFG>>::out_edge_iterator,
                 typename boost::graph_traits<cfg_ref<CFG>>::out_edge_iterator>
out_edges(typename boost::graph_traits<cfg_ref<CFG>>::vertex_descriptor v,
          cfg_ref<CFG> g) {
  using G = cfg_ref<CFG>;
  auto &node = g.get_node(v);
  auto p = node.next_blocks();
  return std::make_pair(
      boost::make_transform_iterator(p.first, graph::mk_out_edge<G>(v)),
      boost::make_transform_iterator(p.second, graph::mk_out_edge<G>(v)));
}

template <class CFG>
inline std::pair<typename boost::graph_traits<cfg_ref<CFG>>::in_edge_iterator,
                 typename boost::graph_traits<cfg_ref<CFG>>::in_edge_iterator>
in_edges(typename boost::graph_traits<cfg_ref<CFG>>::vertex_descriptor v,
         cfg_ref<CFG> g) {
  using G = cfg_ref<CFG>;
  auto &node = g.get_node(v);
  auto p = node.prev_blocks();
  return std::make_pair(
      boost::make_transform_iterator(p.first, graph::mk_in_edge<G>(v)),
      boost::make_transform_iterator(p.second, graph::mk_in_edge<G>(v)));
}

template <class CFG>
typename boost::graph_traits<cfg_ref<CFG>>::vertices_size_type
num_vertices(cfg_ref<CFG> g) {
  return std::distance(g.label_begin(), g.label_end());
}

template <class CFG>
typename boost::graph_traits<cfg_ref<CFG>>::degree_size_type
in_degree(typename boost::graph_traits<cfg_ref<CFG>>::vertex_descriptor v,
          cfg_ref<CFG> g) {
  auto preds = g.prev_nodes(v);
  return std::distance(preds.begin(), preds.end());
}

template <class CFG>
typename boost::graph_traits<cfg_ref<CFG>>::degree_size_type
out_degree(typename boost::graph_traits<cfg_ref<CFG>>::vertex_descriptor v,
           cfg_ref<CFG> g) {
  auto succs = g.next_nodes(v);
  return std::distance(succs.begin(), succs.end());
}

template <class CFG>
typename boost::graph_traits<cfg_ref<CFG>>::degree_size_type
degree(typename boost::graph_traits<cfg_ref<CFG>>::vertex_descriptor v,
       cfg_ref<CFG> g) {
  return out_degree(v, g) + in_degree(v, g);
}

// cfg_rev

//// boost/graph/graph_traits.hpp has already default source/target
//// functions in case the edge_descriptor is std::pair.

// this is not part of BGL but needed by Crab's algorithms
template <class CFG>
typename boost::graph_traits<cfg_rev<CFG>>::vertex_descriptor
entry(const cfg_rev<CFG> &g) {
  return g.entry();
}

template <class CFG>
inline std::pair<typename boost::graph_traits<cfg_rev<CFG>>::vertex_iterator,
                 typename boost::graph_traits<cfg_rev<CFG>>::vertex_iterator>
vertices(cfg_rev<CFG> g) {
  return std::make_pair(g.label_begin(), g.label_end());
}

template <class CFG>
inline std::pair<typename boost::graph_traits<cfg_rev<CFG>>::out_edge_iterator,
                 typename boost::graph_traits<cfg_rev<CFG>>::out_edge_iterator>
out_edges(typename boost::graph_traits<cfg_rev<CFG>>::vertex_descriptor v,
          cfg_rev<CFG> g) {
  using G = cfg_rev<CFG>;
  auto &node = g.get_node(v);
  auto p = node.next_blocks();
  return std::make_pair(
      boost::make_transform_iterator(p.first, graph::mk_out_edge<G>(v)),
      boost::make_transform_iterator(p.second, graph::mk_out_edge<G>(v)));
}

template <class CFG>
inline std::pair<typename boost::graph_traits<cfg_rev<CFG>>::in_edge_iterator,
                 typename boost::graph_traits<cfg_rev<CFG>>::in_edge_iterator>
in_edges(typename boost::graph_traits<cfg_rev<CFG>>::vertex_descriptor v,
         cfg_rev<CFG> g) {
  using G = cfg_rev<CFG>;
  auto &node = g.get_node(v);
  auto p = node.prev_blocks();
  return std::make_pair(
      boost::make_transform_iterator(p.first, graph::mk_in_edge<G>(v)),
      boost::make_transform_iterator(p.second, graph::mk_in_edge<G>(v)));
}

template <class CFG>
typename boost::graph_traits<cfg_rev<CFG>>::vertices_size_type
num_vertices(cfg_rev<CFG> g) {
  return std::distance(g.label_begin(), g.label_end());
}

template <class CFG>
typename boost::graph_traits<cfg_rev<CFG>>::degree_size_type
in_degree(typename boost::graph_traits<cfg_rev<CFG>>::vertex_descriptor v,
          cfg_rev<CFG> g) {
  auto preds = g.prev_nodes(v);
  return std::distance(preds.begin(), preds.end());
}

template <class CFG>
typename boost::graph_traits<cfg_rev<CFG>>::degree_size_type
out_degree(typename boost::graph_traits<cfg_rev<CFG>>::vertex_descriptor v,
           cfg_rev<CFG> g) {
  auto succs = g.next_nodes(v);
  return std::distance(succs.begin(), succs.end());
}

template <class CFG>
typename boost::graph_traits<cfg_rev<CFG>>::degree_size_type
degree(typename boost::graph_traits<cfg_rev<CFG>>::vertex_descriptor v,
       cfg_rev<CFG> g) {
  return out_degree(v, g) + in_degree(v, g);
}

} // namespace cfg
} // namespace crab

ext: hpp

max_stars_repo_name: LinerSu/crab

max_issues_repo_name: LinerSu/crab

max_forks_repo_name: LinerSu/crab


text: #include <boost/mpl/aux_/preprocessed/dmc/plus.hpp>

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text:
#include <iostream>
#include <vector>
#include <string>
#include <exception>
#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <zmq.h>

template <typename T>
void print(T&& t)
{
  std::cout << t << "\n";
}

template <typename First, typename... Args>
void print(First&& f, Args&&... args)
{
  std::cout << f << " ";
  print(std::forward<Args>(args)...);
}

namespace asio_zmq {

class zmq_exception: public std::exception {
public:
	zmq_exception (std::string __reason):
	reason_(__reason) {

	}
    const char* what() const noexcept {
    	return reason_.c_str();
    }
    std::string reason_;
};

class zmq_context {
public:
	typedef void* zmq_context_handle;
	zmq_context() {
		void *tmp = zmq_ctx_new ();
		if (tmp == NULL)
			throw zmq_exception("could not obtain a zmq context");
		handle_ = tmp;
	}
	zmq_context_handle handle() {
		return handle_;
	}
private:
	zmq_context_handle handle_;
};

void *get_zmq_context() {
	static zmq_context zmq_ctx_;
	return zmq_ctx_.handle();
}

template <typename ConstBufferSequence, typename Handler>
class zsock_send_op {
public:

	zsock_send_op(const void *__zmq_sock, const ConstBufferSequence & __buffers, Handler __handler):
		buffers_(__buffers),
		handler_(__handler),
		zmq_sock_(__zmq_sock)
	{
	}

	void operator()(const boost::system::error_code &ec, std::size_t bytes_transferred) {

		boost::system::error_code zec;
		int send_rc = 0;

		if (!ec) {
			int flags = 0;
			size_t fsize = sizeof (flags);
			zmq_getsockopt(const_cast<void *>(zmq_sock_), ZMQ_EVENTS, &flags, &fsize);

			if (flags & ZMQ_POLLOUT) {
				const void* buf = boost::asio::buffer_cast<const void*>(buffers_);
				int buf_size = boost::asio::buffer_size(buffers_);
			    send_rc = zmq_send (const_cast<void *>(zmq_sock_),
			    	const_cast<void *>(buf), buf_size, 0);

			    if (send_rc == -1)
			    {
			    	zec.assign(errno, boost::system::system_category());
			    }
			}
			handler_ (zec, send_rc);
		} // ec
		else {
			handler_ (ec, send_rc);
		}
	}

private:
	ConstBufferSequence buffers_;
	Handler handler_;
	const void *zmq_sock_;
};

template <typename MutableBufferSequence, typename Handler>
class zsock_recv_op {
public:

	zsock_recv_op(void *__zmq_sock, const MutableBufferSequence & __buffers, Handler __handler):
		buffers_(__buffers),
		handler_(__handler),
		zmq_sock_(__zmq_sock)
	{
	}

	void operator()(const boost::system::error_code &ec,  std::size_t bytes_transferred) {

		if (!ec) {
			boost::system::error_code zec; //defaut ctor no error
			int recvd = 0;
			int flags = 0;
			size_t fsize = sizeof (flags);
			zmq_getsockopt(const_cast<void *>(zmq_sock_), ZMQ_EVENTS, &flags, &fsize);
			if (flags & ZMQ_POLLIN) {
				//we can read from the zmq socket
				void *buf = boost::asio::buffer_cast<void *>(buffers_);
				int buf_size = boost::asio::buffer_size(buffers_);
				recvd = zmq_recv(zmq_sock_, buf, buf_size, 0);
				//check if recvd == -1
				if (recvd == -1) {
					zec.assign(errno, boost::system::system_category());
				}
			}
			//if we were not able to read it is not an error the application should try
			//again later
			handler_(zec, recvd);
		}
		else {
			//something wrong happened with
			//the underlying socket report the error
			handler_(ec, bytes_transferred);
		}
	}
private:
	MutableBufferSequence buffers_;
	Handler handler_;
	void *zmq_sock_;
};

class asio_zmq_req_socket {
public:

	asio_zmq_req_socket(boost::asio::io_service& __io_service, std::string __srv_addr):
		sock_(__io_service)
	{

		void *zmq_ctx = get_zmq_context ();
    	zmq_sock_ = zmq_socket (zmq_ctx, ZMQ_REQ);

    	int opt = 0;
    	std::size_t optlen = sizeof (opt);

    	zmq_setsockopt(zmq_sock_, ZMQ_LINGER, &opt, optlen);

    	if (zmq_sock_ == NULL) {
    		throw zmq_exception("could not create a zmq REQ socket");
    	}

    	int zfd;
    	optlen = sizeof (zfd);
    	zmq_getsockopt (zmq_sock_, ZMQ_FD, &zfd, &optlen);
    	sock_.assign (boost::asio::ip::tcp::v4(), zfd);

    	int rc = zmq_connect (zmq_sock_, __srv_addr.c_str ());
    	if (rc) {
    		throw zmq_exception("zmq socket: could not connect to " + __srv_addr);
    	}
	}

	template <typename ConstBufferSequence, typename Handler>
	void async_send (const ConstBufferSequence & buffer, Handler handler) {
		zsock_send_op<ConstBufferSequence, Handler> send_op (zmq_sock_, buffer, handler);
		sock_.async_write_some (boost::asio::null_buffers(), send_op);
	}

	template <typename MutableBufferSequence, typename Handler>
	void async_recv (const MutableBufferSequence & buffer, Handler handler) {
		zsock_recv_op<MutableBufferSequence, Handler> recv_op (zmq_sock_, buffer, handler);
		sock_.async_read_some (boost::asio::null_buffers(), recv_op);
	}

	~asio_zmq_req_socket() {
		sock_.shutdown(boost::asio::ip::tcp::socket::shutdown_both);
		sock_.close();
		zmq_close (zmq_sock_);
	}

private:

	void *zmq_sock_ = nullptr;
	boost::asio::ip::tcp::socket sock_;
};

} //namespace asio_zmq

class my_zmq_req_client : public std::enable_shared_from_this<my_zmq_req_client>{
public:
	my_zmq_req_client(boost::asio::io_service& __io_service, const std::string& __addr):
	zsock_(__io_service, __addr)
	{

	}

	void send(const std::string& __message) {
		print("my_zmq_req_client: sending message:", __message);
		zsock_.async_send (
				boost::asio::buffer(__message),
				boost::bind(&my_zmq_req_client::handle_send, shared_from_this(),
					boost::asio::placeholders::error,
					boost::asio::placeholders::bytes_transferred
				)
		);
	}

private:

	void handle_send(const boost::system::error_code &ec, std::size_t bytes_transferred) {

		if (ec) {
			std::cout << "failed to send buffer\n";
		}
		else {
			print("my_zmq_req_client: message sent");
			zsock_.async_recv (
				boost::asio::buffer(recv_buffer_),
				boost::bind (&my_zmq_req_client::handle_recv, shared_from_this(), boost::asio::placeholders::error,
					boost::asio::placeholders::bytes_transferred)
			);
		}
	}

	void handle_recv(const boost::system::error_code &ec, std::size_t bytes_transferred) {

		if (!ec) {
			// if no error happened but no bytes were received because the zeromq socket
			// is not POLL_IN ready (check the zero_mq zmq_getsockopt: it says that
			// a socket can be reported as writable by the OS but not yet for zeromq)
			// schedule another async recv operation. This is less likely to happen for send operations
			// and in practice never had to do it.
			if (bytes_transferred == 0)
				zsock_.async_recv (boost::asio::buffer(recv_buffer_),
					boost::bind (&my_zmq_req_client::handle_recv, shared_from_this(), boost::asio::placeholders::error,
						boost::asio::placeholders::bytes_transferred));
			else {

				printf("my_zmq_req_client: zmq REP: %s\n", recv_buffer_.data());
			}
		}
	}

	asio_zmq::asio_zmq_req_socket zsock_;
	std::array<char, 256> recv_buffer_;
};

int main(int, char**) {

	asio_zmq::get_zmq_context();

	boost::asio::io_service io_service;

	std::string srv_addr("tcp://127.0.0.1:11155");
	std::string msg("zmq REQ: hello from client");

	{
		auto zclient = std::make_shared<my_zmq_req_client>(io_service, srv_addr);
		zclient->send(msg);
	}

	io_service.run();

	zmq_ctx_term(asio_zmq::get_zmq_context());

}

ext: cxx


text:
/**
 * \ref kinect2_tracker.hpp
 *
 *  \date 20160322
 *  \author Stephen Reddish
 *  \version 1.0
 *  \bug
 *   It will be quicker to work out the vec3s before passing them to the transform publisher
 *   Solve bodgey if torso elses
 *  \copyright GNU Public License.
 */

#ifndef KINECT2_TRACKER_HPP_
#define KINECT2_TRACKER_HPP_

// ROS Dependencies
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <iostream>

// Self-defined messages
#include <kinect2_tracker/user_IDs.h>
#include <kinect2_tracker/user_points.h>
#include <kinect2_tracker/bounding_box.h>

#include <cv_bridge/cv_bridge.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/distortion_models.h>
#include <image_transport/image_transport.h>
#include "NiTE.h"
#include "visualization.hpp"
#include <sensor_msgs/image_encodings.h>
#include <Eigen/Geometry>
#include <tf_conversions/tf_eigen.h>
#include <visualization_msgs/Marker.h>
#include <NiteCTypes.h>

#ifndef ALPHA
#define ALPHA 1/256
#endif

#define MAX_USERS 10

#define USER_MESSAGE(msg) \
        {printf("[%08llu] User #%d:\t%s\n",ts, user.getId(),msg);}

using namespace std;

/// Joint map
typedef std::map<std::string, nite::SkeletonJoint> JointMap;

/**
 * Class \ref kinect2_tracker. This class can track the skeleton of people and returns joints as a TF stream,
 */
class k2_tracker
{
public:
  /**
   * Constructor
   */
  k2_tracker() :
      it_(nh_)
  {

    // Get some parameters from the server
    ros::NodeHandle pnh("~");
    if (!pnh.getParam("tf_prefix", tf_prefix_))
    {
      ROS_FATAL("tf_prefix not found on Param Server! Maybe you should add it to your launch file!");
      ros::shutdown();
      return;
    }
    if (!pnh.getParam("relative_frame", relative_frame_))
    {
      ROS_FATAL("relative_frame not found on Param Server! Maybe you should add it to your launch file!");
      ros::shutdown();
      return;
    }

    // Initialize OpenNI
    if (openni::OpenNI::initialize() != openni::STATUS_OK)
    {
      ROS_FATAL("OpenNI initial error");
      ros::shutdown();
      return;
    }

    // Open the device
    if (devDevice_.open(openni::ANY_DEVICE) != openni::STATUS_OK)
    {
      ROS_FATAL("Can't Open Device");
      ros::shutdown();
      return;
    }
    ROS_INFO("Device opened");

    // Initialize the tracker
    nite::NiTE::initialize();

    // user tracker registration
    niteRc_ = userTracker_.create();
    if (niteRc_ != nite::STATUS_OK)
    {
      ROS_FATAL("Couldn't create user tracker");
      ros::shutdown();
      return;
    }

    ///////////////////////////////////////////////////
    // Create color stream

    if( vsColorStream.create( devDevice_, openni::SENSOR_COLOR ) == openni::STATUS_OK )
    {
        // set video mode
        openni::VideoMode mMode;
        //mMode.setResolution( 640, 480 );
        mMode.setResolution( 640, 480 );
        mMode.setFps( 30 );
        mMode.setPixelFormat( openni::PIXEL_FORMAT_RGB888 );

        if( vsColorStream.setVideoMode( mMode) != openni::STATUS_OK )
        {
            ROS_INFO("Can't apply videomode\n");
            //cout << "Can't apply VideoMode: " << OpenNI::getExtendedError() << endl;
        }

        // image registration
        // if( devDevice_.isImageRegistrationModeSupported( openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR ) )
        // {
        //     devDevice_.setImageRegistrationMode( openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR );
        // }
        vsColorStream.setMirroringEnabled(false);
    }
    else
    {
        ROS_ERROR("Can't create color stream on device: ");// << OpenNI::getExtendedError() << endl;
        //cerr <<  "Can't create color stream on device: " << OpenNI::getExtendedError() << endl;
        return;
    }
    vsColorStream.start();
    /////////////////////////////////////////////////////////

    // Initialize the users IDs publisher
    userPub_ = nh_.advertise<kinect2_tracker::user_IDs>("/people_skeleton", 1);
    pointPub_ = nh_.advertise<kinect2_tracker::user_points>("/people_points", 1);
    pointVizPub_ = nh_.advertise<visualization_msgs::Marker>("/people_points_viz", 1);
    imagePub_ = it_.advertise("/kinect_rgb", 1);

    // userPub_ = nh_.advertise<user_IDs>("/people", 1);
    rate_ = new ros::Rate(100);

  }
  /**
   * Destructor
   */
  ~k2_tracker()
  {
    nite::NiTE::shutdown();
  }

  /**
   * Spinner!!!
   */
  void spinner()
  {
    // Broadcast the joint frames (if they exist)
    this->getSkeleton();
    this->getRGB();
    rate_->sleep();
  }

  /**
   * Update the Users State
   * @param user: the user
   * @param ts: timestamp
   */
  void updateUserState(const nite::UserData& user, unsigned long long ts)
  {
    if (user.isNew())
      USER_MESSAGE("New")
    else if (user.isVisible() && !g_visibleUsers_[user.getId()])
      USER_MESSAGE("Visible")
    else if (!user.isVisible() && g_visibleUsers_[user.getId()])
      USER_MESSAGE("Out of Scene")
    else if (user.isLost())
      USER_MESSAGE("Lost")

    g_visibleUsers_[user.getId()] = user.isVisible();

    //检测动作是否有变化，有变化就进行跟踪Tracking
    if (g_skeletonStates_[user.getId()] != user.getSkeleton().getState())
    {
      switch (g_skeletonStates_[user.getId()] = user.getSkeleton().getState())
      {
        case nite::SKELETON_NONE:
          USER_MESSAGE("Stopped tracking.")
          break;
        case nite::SKELETON_CALIBRATING:
          USER_MESSAGE("Calibrating...")
          break;
        case nite::SKELETON_TRACKED:
          USER_MESSAGE("Tracking!")
          break;
        case nite::SKELETON_CALIBRATION_ERROR_NOT_IN_POSE:
        case nite::SKELETON_CALIBRATION_ERROR_HANDS:
        case nite::SKELETON_CALIBRATION_ERROR_LEGS:
        case nite::SKELETON_CALIBRATION_ERROR_HEAD:
        case nite::SKELETON_CALIBRATION_ERROR_TORSO:
          USER_MESSAGE("Calibration Failed... :-|")
          break;
      }
    }
  }

  /**
   * Publish the joints over the TF stream
   * @param j_name: joint name
   * @param j: the joint
   * @param r: relative joint (joint j connects to)
   * @param uid: user's ID
   */
  void publishJointTF(std::string j_name, nite::SkeletonJoint j, std::string r_name, nite::SkeletonJoint r, int uid)
  {
    if (j.getPositionConfidence() > 0.0)
    {
      tf::Vector3 currentVec3 = tf::Vector3(j.getPosition().x / 1000.0, j.getPosition().y / 1000.0, j.getPosition().z / 1000.0);
      tf::Transform transform;
        if (j_name != "torso")
        {
            tf::Vector3 rVec3 = tf::Vector3(r.getPosition().x / 1000.0, r.getPosition().y / 1000.0, r.getPosition().z / 1000.0);
            transform.setOrigin(currentVec3 - rVec3);
            transform.setRotation(tf::Quaternion(0,0,0,1));
        }
        else
        {
            transform.setOrigin(currentVec3);
            transform.setRotation(tf::Quaternion(0,0,0,1));
        }

        std::stringstream j_frame_id_stream; //stringstream of frame id values
        std::string j_frame_id; // string of the stringstream
        j_frame_id_stream << "/" << tf_prefix_ << "/user_" << uid << "/" << j_name;
        j_frame_id = j_frame_id_stream.str();

        std::stringstream r_frame_id_stream; //stringstream of frame id values
        std::string r_frame_id; // string of the stringstream
        r_frame_id_stream << "/" << tf_prefix_ << "/user_" << uid << "/" << r_name;
        r_frame_id = r_frame_id_stream.str();

        if (j_name == "torso")
        {

            tfBroadcast_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), relative_frame_, j_frame_id));
        }
        else
        {

            tfBroadcast_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), r_frame_id, j_frame_id));
        }
    }
    return;
  }

  //Publish the calibration tf_frame as the cross product of the shoulder vectors
 // This function publishes the calibration_space opposite the shoulders of the user
   void publishCalibrationOriginTF(nite::SkeletonJoint skelTorso, nite::SkeletonJoint skelRshoulder, nite::SkeletonJoint skelLshoulder, int uid)
   {
     if (skelTorso.getPositionConfidence() > 0.0)
     {
       tf::Transform calibrationOriginTransform;
       tf::Transform torsoTransform;

             tf::Vector3 torsoVec3 = tf::Vector3(skelTorso.getPosition().x / 1000.0, skelTorso.getPosition().y / 1000.0, skelTorso.getPosition().z / 1000.0);
             torsoTransform.setOrigin(torsoVec3);
             torsoTransform.setRotation(tf::Quaternion(0,0,0,1));

             tf::Vector3 RshoulderVec3 = tf::Vector3(skelRshoulder.getPosition().x / 1000.0, skelRshoulder.getPosition().y / 1000.0, skelRshoulder.getPosition().z / 1000.0);                 //create a vector for the right shoulder
             RshoulderVec3 = (RshoulderVec3 - torsoVec3); //vector is the difference of the two

             tf::Vector3 LshoulderVec3 = tf::Vector3(skelLshoulder.getPosition().x / 1000.0, skelLshoulder.getPosition().y / 1000.0, skelLshoulder.getPosition().z / 1000.0);                 //create a vector for the left shoulder
            LshoulderVec3 = (LshoulderVec3 - torsoVec3);
            tf::Vector3 calibrationOriginVec3 = RshoulderVec3.cross(LshoulderVec3);

		//give the calibration origin some length
//	   calibrationOriginVec3 = calibrationOriginVec3 * 20;
           calibrationOriginTransform.setOrigin(calibrationOriginVec3);// set the x,y,z coordinates of the calibration transform frame

              Eigen::Vector3d eigencalibrationOriginVec3;
              tf::vectorTFToEigen(calibrationOriginVec3, eigencalibrationOriginVec3); //conversion of tf:Vec3 to eigen
              Eigen::Vector3d eigenTorsoVec3;
              tf::vectorTFToEigen(torsoVec3, eigenTorsoVec3);               //conversion of torse tf:vec3 to eigen
              Eigen::Quaterniond eigen_calibrationOrigenQuaternion;
              eigen_calibrationOrigenQuaternion.setFromTwoVectors(eigencalibrationOriginVec3, eigenTorsoVec3);
              tf::Quaternion tf_calibrationOriginQuaternion;
              tf::quaternionEigenToTF(eigen_calibrationOrigenQuaternion, tf_calibrationOriginQuaternion);

            calibrationOriginTransform.setRotation(tf_calibrationOriginQuaternion);

        std::stringstream calibration_frame_id_stream; //stringstream of frame id values
        std::string calibration_frame_id; // string of the stringstream
        calibration_frame_id_stream << "/" << tf_prefix_ << "/user_" << uid << "/calibrationOrigin";
        calibration_frame_id = calibration_frame_id_stream.str();

	std::stringstream r_frame_id_stream; //stringstream of frame id values
        std::string r_frame_id; // string of the stringstream
        r_frame_id_stream << "/" << tf_prefix_ << "/user_" << uid << "/torso";
        r_frame_id = r_frame_id_stream.str();

        tfBroadcast_.sendTransform(tf::StampedTransform(calibrationOriginTransform, ros::Time::now(), r_frame_id, calibration_frame_id));
    }
    return;
  }

  /**
    * Get the RGB feed and publish it to ROS
  */
  void getRGB()
  {
    openni::VideoFrameRef vfColorFrame;
    cv::Mat mImageBGR;
    if( vsColorStream.readFrame( &vfColorFrame ) == openni::STATUS_OK )
    {
        // convert data to OpenCV format
        const cv::Mat mImageRGB( vfColorFrame.getHeight(), vfColorFrame.getWidth(), CV_8UC3, const_cast<void*>( vfColorFrame.getData() ) );
        // convert form RGB to BGR
        cv::cvtColor( mImageRGB, mImageBGR, CV_RGB2BGR );
        vfColorFrame.release();

        sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", mImageBGR).toImageMsg();
        imagePub_.publish(msg);
    }

  }
  /**
   * Get the skeleton's joints and the users IDs and make them all relative to the Torso躯干 joint
  */
  void getSkeleton()
  {
    // skeleton_tracker::user_IDs ids;
    kinect2_tracker::user_IDs ids;
    kinect2_tracker::user_points points;

    niteRc_ = userTracker_.readFrame(&userTrackerFrame_);
    if (niteRc_ != nite::STATUS_OK)
    {
      printf("Get next frame failed\n");
      return;
    }

    // Get all the users
    const nite::Array<nite::UserData>& users = userTrackerFrame_.getUsers();

    // Get the skeleton for every user
    for (int i = 0; i < users.getSize(); ++i)
    {
      const nite::UserData& user = users[i];
      updateUserState(user, userTrackerFrame_.getTimestamp());
      if (user.isNew())
      {
        userTracker_.startSkeletonTracking(user.getId());
      }
      else if (user.getSkeleton().getState() == nite::SKELETON_TRACKED)
      {
        JointMap named_joints;

        named_joints["torso"] = (user.getSkeleton().getJoint(nite::JOINT_TORSO));// this value is Joint_name, position & orientation & confidence, userid
        named_joints["left_hip"] = (user.getSkeleton().getJoint(nite::JOINT_LEFT_HIP));
        named_joints["right_hip"] = (user.getSkeleton().getJoint(nite::JOINT_RIGHT_HIP));
        named_joints["left_knee"] = (user.getSkeleton().getJoint(nite::JOINT_LEFT_KNEE));
        named_joints["right_knee"] = (user.getSkeleton().getJoint(nite::JOINT_RIGHT_KNEE));
        named_joints["left_foot"] = (user.getSkeleton().getJoint(nite::JOINT_LEFT_FOOT));
        named_joints["right_foot"] = (user.getSkeleton().getJoint(nite::JOINT_RIGHT_FOOT));
        named_joints["neck"] = (user.getSkeleton().getJoint(nite::JOINT_NECK));
        named_joints["head"] = (user.getSkeleton().getJoint(nite::JOINT_HEAD));
        named_joints["left_shoulder"] = (user.getSkeleton().getJoint(nite::JOINT_LEFT_SHOULDER));
        named_joints["right_shoulder"] = (user.getSkeleton().getJoint(nite::JOINT_RIGHT_SHOULDER));
        named_joints["left_elbow"] = (user.getSkeleton().getJoint(nite::JOINT_LEFT_ELBOW));
        named_joints["right_elbow"] = (user.getSkeleton().getJoint(nite::JOINT_RIGHT_ELBOW));
        named_joints["left_hand"] = (user.getSkeleton().getJoint(nite::JOINT_LEFT_HAND));
        named_joints["right_hand"] = (user.getSkeleton().getJoint(nite::JOINT_RIGHT_HAND));
//Publish the joint (name, niteConstruct, ConnectedJoint name, niteConstruct, User
        publishJointTF("torso", named_joints["torso"], "torso", named_joints["torso"], user.getId());
        publishJointTF("left_hip", named_joints["left_hip"], "torso", named_joints["torso"], user.getId());
        publishJointTF("right_hip", named_joints["right_hip"], "torso", named_joints["torso"], user.getId());
        publishJointTF("left_knee", named_joints["left_knee"], "left_hip", named_joints["left_hip"], user.getId());
        publishJointTF("right_knee", named_joints["right_knee"], "right_hip", named_joints["right_hip"], user.getId());
        publishJointTF("left_foot", named_joints["left_foot"], "left_knee", named_joints["left_knee"], user.getId());
        publishJointTF("right_foot", named_joints["right_foot"], "right_knee", named_joints["right_knee"], user.getId());
        publishJointTF("neck", named_joints["neck"], "torso", named_joints["torso"], user.getId());
        publishJointTF("head", named_joints["head"], "neck", named_joints["neck"], user.getId());
        publishJointTF("left_shoulder", named_joints["left_shoulder"], "torso", named_joints["torso"], user.getId());
        publishJointTF("right_shoulder", named_joints["right_shoulder"], "torso", named_joints["torso"], user.getId());
        publishJointTF("left_elbow", named_joints["left_elbow"], "left_shoulder", named_joints["left_shoulder"], user.getId());
        publishJointTF("right_elbow", named_joints["right_elbow"], "right_shoulder", named_joints["right_shoulder"], user.getId());
        publishJointTF("left_hand", named_joints["left_hand"], "left_elbow", named_joints["left_elbow"], user.getId());
        publishJointTF("right_hand", named_joints["right_hand"], "right_elbow", named_joints["right_elbow"], user.getId());

          //添加变量让其输出
          geometry_msgs::Point p_LS,p_LE,p_LH,p_LP;
          //LEFT_HAND
          p_LH.x=user.getSkeleton().getJoint(nite::JOINT_LEFT_HAND).getPosition().x;
          p_LH.y=user.getSkeleton().getJoint(nite::JOINT_LEFT_HAND).getPosition().y;
          p_LH.z=user.getSkeleton().getJoint(nite::JOINT_LEFT_HAND).getPosition().z;
          //LEFT_ELBOW
          p_LE.x=user.getSkeleton().getJoint(nite::JOINT_LEFT_ELBOW).getPosition().x;
          p_LE.y=user.getSkeleton().getJoint(nite::JOINT_LEFT_ELBOW).getPosition().y;
          p_LE.z=user.getSkeleton().getJoint(nite::JOINT_LEFT_ELBOW).getPosition().z;
          //LEFT_SHOULDER
          p_LS.x=user.getSkeleton().getJoint(nite::JOINT_LEFT_SHOULDER).getPosition().x;
          p_LS.y=user.getSkeleton().getJoint(nite::JOINT_LEFT_SHOULDER).getPosition().y;
          p_LS.z=user.getSkeleton().getJoint(nite::JOINT_LEFT_SHOULDER).getPosition().z;
          //LEFT_HIP
          p_LP.x=user.getSkeleton().getJoint(nite::JOINT_LEFT_HIP).getPosition().x;
          p_LP.y=user.getSkeleton().getJoint(nite::JOINT_LEFT_HIP).getPosition().y;
          p_LP.z=user.getSkeleton().getJoint(nite::JOINT_LEFT_HIP).getPosition().z;

          pub1.publish(p_LH);
          pub2.publish(p_LE);
          pub3.publish(p_LS);
          pub4.publish(p_LP);
//publishes the funny normal vector from the users chest
        publishCalibrationOriginTF(named_joints["torso"], named_joints["left_shoulder"], named_joints["right_shoulder"], user.getId());

        // Add the user's ID
        ids.users.push_back(int(user.getId()));
      }
      if(user.isVisible()){
        // Adding center of mass of users
        points.users.push_back(int(user.getId()));
        nite::Point3f user_point = user.getCenterOfMass();
        nite::BoundingBox boundingBox = user.getBoundingBox();
        geometry_msgs::PointStamped p;
        kinect2_tracker::bounding_box bbox;
        bbox.min.point.x = boundingBox.min.x / 1000;
        bbox.min.point.y = boundingBox.min.y / 1000;
        bbox.min.point.z = boundingBox.min.z / 1000;
        bbox.max.point.x = boundingBox.max.x / 1000;
        bbox.max.point.y = boundingBox.max.y / 1000;
        bbox.max.point.z = boundingBox.max.z / 1000;
        p.header.stamp = ros::Time::now();
        p.header.frame_id = relative_frame_;
        bbox.min.header = p.header;
        bbox.max.header = p.header;
        p.point.x = user_point.x / 1000;
        p.point.y = user_point.y / 1000;
        p.point.z = user_point.z / 1000;

        points.people_points.push_back(p);
        points.boxes.push_back(bbox);
      }
    }
    // Publish the users' IDs
    userPub_.publish(ids);
    pointPub_.publish(points);
    pointVizPub_.publish(getMarkers(points.people_points, relative_frame_));
  }

  /// ROS NodeHandle
  ros::NodeHandle nh_;

  //publish
  ros::Publisher pub1 = nh_.advertise<geometry_msgs::Point>("pose1",1000);
  ros::Publisher pub2 = nh_.advertise<geometry_msgs::Point>("pose2",1000);
  ros::Publisher pub3 = nh_.advertise<geometry_msgs::Point>("pose3",1000);
  ros::Publisher pub4 = nh_.advertise<geometry_msgs::Point>("pose4",1000);

  bool g_visibleUsers_[MAX_USERS] = {false};
  nite::SkeletonState g_skeletonStates_[MAX_USERS] = {nite::SKELETON_NONE};

  /// Image transport
  image_transport::ImageTransport it_;
  std::string tf_prefix_, relative_frame_;

  /// Frame broadcaster
  tf::TransformBroadcaster tfBroadcast_;

  /// The openni device
  openni::Device devDevice_;
  openni::VideoStream vsColorStream;

  /// Some NITE stuff
  nite::UserTracker userTracker_;
  nite::Status niteRc_;
  nite::UserTrackerFrameRef userTrackerFrame_;

  /// Users IDs publisher
  ros::Publisher userPub_;
  ros::Publisher pointPub_;
  ros::Publisher pointVizPub_;
  ros::Publisher boxPub_;

  //Image publisher
  // image_transport::ImageTransport it_;
  image_transport::Publisher imagePub_;

  /// Image message
  sensor_msgs::ImagePtr msg_;

  /// Node rate
  ros::Rate* rate_;

}
;

#endif /* KINECT2_TRACKER_HPP_ */


text:
#include "ros/ros.h"
#include "geometry_msgs/Vector3.h"
#include "sensor_msgs/Joy.h"
#include "create_driver/vicon_driver.h"
#include "geometry_msgs/Twist.h"

#include "create_controller/ControlMsgs.h"

#include <string>
#include <vector>
#include <map>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <algorithm>
#include <cmath>

#define PI 3.141592
using namespace Eigen;
using namespace std;
Vector3d position(-5,-5,-5);

void position_callback(const geometry_msgs::Vector3::ConstPtr& msg)
{
	position(0) = msg->x;
	position(1) = msg->y;
	position(2) = msg->z;
}

int main(int argc, char **argv)
{
	// ROS Initalization
	ros::init(argc, argv, "falcon_ellipse_feedback");

	ros::NodeHandle n;
	ros::NodeHandle private_n("~");

	// List of robot names
	vector<string> robot_names;
	XmlRpc::XmlRpcValue robot_list;
	private_n.getParam("robot_list", robot_list);
	ROS_ASSERT(robot_list.getType() == XmlRpc::XmlRpcValue::TypeArray);

	for (int i = 0; i < robot_list.size(); i++)
	{
		ROS_ASSERT(robot_list[i].getType() == XmlRpc::XmlRpcValue::TypeString);
		robot_names.push_back(static_cast<string>(robot_list[i]));
	}

	// Number of robots
	const int num_robots = robot_names.size();

	// ROS Subscribers
	map<string, create_driver::ViconStream> vicon;
	vector<string>::iterator name_it;
	vector<ros::Subscriber> sub;
	for (name_it = robot_names.begin(); name_it != robot_names.end(); name_it++)
	{
		vicon.insert(pair<string, create_driver::ViconStream>(*name_it, create_driver::ViconStream()));
		sub.push_back(n.subscribe("/" + *name_it + "/tf", 10, &create_driver::ViconStream::callback, &vicon[*name_it]));
	}

	sub.push_back(n.subscribe("position", 10, position_callback));

	ros::Publisher joy_pub = n.advertise<sensor_msgs::Joy>("joy", 10);
	ros::Publisher force_pub = n.advertise<geometry_msgs::Vector3>("force", 10);

	// ROS loop
	ros::Rate loop_rate(1000); // 1 kHz
	bool uninitialized = true;
	while (ros::ok())
	{
		ros::spinOnce();

		Vector3d joy(position(0)/0.06,  position(1)/0.06, (2.0*(position(2)-0.073))/0.1035 - 1.0);

		Vector3d force;
		double resistance;

		double p[num_robots];
		double q[num_robots];

		for (int i = 0; i < num_robots; i++) {
			p[i] = cos(vicon[robot_names[i]].theta());
			q[i] = sin(vicon[robot_names[i]].theta());
		}

		double p_bar = 0.0;
		double q_bar = 0.0;

		for (int i = 0; i < num_robots; i++) {
			p_bar += p[i]/num_robots;
			q_bar += q[i]/num_robots;
		}

		double r = sqrt(pow(p_bar, 2) + pow(q_bar, 2))/2.0;

		double phi = atan2(q_bar, p_bar);

		double b = 0.2;

		double a = sqrt(pow(r, 2) + pow(b, 2));

		double x = joy(0) - r*cos(phi);
		double y = joy(1) - r*sin(phi);

		double x_barE = x * cos(phi) + y*sin(phi);
		double y_barE = -x * sin(phi) + y*cos(phi);

		double c = pow(x_barE,2) / pow(a,2) + pow(y_barE,2) / pow(b,2);

		double lowerbound = 1.0;
		double upperbound = 25.0;
		double gain = 0.0;
		if(lowerbound < c && c < upperbound){
			gain = (-500)*(pow(upperbound,2) - pow(lowerbound,2) )*(pow(c,2) - pow(lowerbound,2) ) / pow((pow(c,2) - pow(upperbound,2) ),3);

		}

		double v_bar1 = -2 * x_barE/pow(a,2);
		double v_bar2 = -2 * y_barE/pow(b,2);

		double v_bar_norm = sqrt(pow(v_bar1, 2) + pow(v_bar2, 2));

		Vector3d v_bar(v_bar1*cos(phi)-v_bar2*sin(phi), v_bar1*sin(phi)+v_bar2*cos(phi), 0);
		force = gain*v_bar_norm*v_bar;
		// if (joy.norm()<0.25) {
		// 	resistance =1;
		// } else {
		// 	double radius = joy.norm() - 0.25;

		// 	double sum_etheta = fabs(etheta[0])+ fabs(etheta[1])+fabs(etheta[2])+fabs(etheta[3]);
		// 	resistance = 1; // + 6 * (radius * sum_etheta);
		// 	printf("%f\n", sum_etheta);
		// }

		// for (int i = 0; i < 3; i++)
		// {
		// 	force(i) = -5*joy(i)*resistance;
		// }

		geometry_msgs::Vector3 fmsg;
		fmsg.x = force(0);
		fmsg.y = force(1);
		fmsg.z = -joy(2)*5;
		force_pub.publish(fmsg);

		// Deadband
		if (joy.norm() < 0.25) {
			for (int i = 0; i < 2; ++i)
			{
				joy(i) =0;
			}
		}

		sensor_msgs::Joy jmsg;
		jmsg.axes.resize(3);
		jmsg.axes[0] = -joy(0);//position(0)/0.06; // -1.0 to 1.0
		jmsg.axes[1] = joy(1);//position(1)/0.06;
		jmsg.axes[2] = joy(2);//(2.0*(position(2)-0.073))/0.1035 - 1.0;//32767
		joy_pub.publish(jmsg);

		loop_rate.sleep();
	}

	return 0;
}

ext: cpp

max_stars_repo_name: rsthomp/UTDchess-RospyXbee

max_issues_repo_name: RachaelT/UTDchess-RospyXbee

max_forks_repo_name: RachaelT/UTDchess-RospyXbee


text:
#include <boost/container/string.hpp>

#include "easylogging++.h"

INITIALIZE_EASYLOGGINGPP

int main(void) {
   boost::container::string s = "This is boost::container::string";
   LOG(INFO) << s;
}

max_stars_repo_name: VolodymyrKuksa/easyloggingpp

max_issues_repo_name: VolodymyrKuksa/easyloggingpp

max_forks_repo_name: VolodymyrKuksa/easyloggingpp


text:
#include <capnp/message.h>
#include <capnp/serialize.h>
#include <google/protobuf/util/message_differencer.h>
#include <gtest/gtest.h>
#include <sys/mman.h>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/serialization/vector.hpp>
#include <chrono>
#include <iostream>
#include "../hps.h"
#include "capnproto_benchmark.capnp.h"
#include "protobuf_benchmark.pb.h"

constexpr static unsigned long long N_ELEMS = 1 << 26;

TEST(DoubleArrayBenchmarkLargeTest, Warmup) {
  std::vector<double> origin;
  origin.resize(N_ELEMS);
  for (unsigned long long i = 0; i < N_ELEMS; i++) {
    origin[i] = i + 0.1;
  }
}

TEST(DoubleArrayBenchmarkLargeTest, Protobuf) {
  using namespace std::chrono;

  auto start_time = high_resolution_clock::now();

  // Construct.
  protobuf_benchmark::DoubleArray origin;
  origin.mutable_elems()->Reserve(N_ELEMS);
  for (unsigned long long i = 0; i < N_ELEMS; i++) {
    origin.add_elems(i + 0.1);
  }

  auto constructed_time = high_resolution_clock::now();

  // Serialize and parse.
  std::string serialized;
  origin.SerializeToString(&serialized);
  protobuf_benchmark::DoubleArray parsed;
  parsed.ParseFromString(serialized);

  auto finish_time = high_resolution_clock::now();

  EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(origin, parsed));

  auto construction_time = duration_cast<milliseconds>(constructed_time - start_time).count();
  auto serialization_time = duration_cast<milliseconds>(finish_time - constructed_time).count();
  std::cout << "Construction Time (ms): " << construction_time << std::endl;
  std::cout << "Serialization and Parse Time (ms): " << serialization_time << std::endl;
  std::cout << "Size (B): " << serialized.size() << std::endl;
}

TEST(DoubleArrayBenchmarkLargeTest, CapnProto) {
  using namespace std::chrono;

  auto start_time = high_resolution_clock::now();

  // Construct.
  capnp::MallocMessageBuilder origin_builder;
  capnproto_benchmark::DoubleArray::Builder origin =
      origin_builder.getRoot<capnproto_benchmark::DoubleArray>();
  auto elems = origin.initElems(N_ELEMS);
  for (unsigned long long i = 0; i < N_ELEMS; i++) {
    elems.set(i, i + 0.1);
  }

  auto constructed_time = high_resolution_clock::now();

  // Serialize and parse.
  auto serialized = messageToFlatArray(origin_builder);
  capnp::ReaderOptions options;
  options.traversalLimitInWords = 1ull << 60;
  capnp::MallocMessageBuilder parsed_builder;
  capnp::initMessageBuilderFromFlatArrayCopy(serialized, parsed_builder, options);

  auto finish_time = high_resolution_clock::now();

  capnproto_benchmark::DoubleArray::Reader parsed =
      parsed_builder.getRoot<capnproto_benchmark::DoubleArray>();
  EXPECT_EQ(parsed.getElems().size(), N_ELEMS);

  auto construction_time = duration_cast<milliseconds>(constructed_time - start_time).count();
  auto serialization_time = duration_cast<milliseconds>(finish_time - constructed_time).count();
  std::cout << "Construction Time (ms): " << construction_time << std::endl;
  std::cout << "Serialization and Parse Time (ms): " << serialization_time << std::endl;
  std::cout << "Size (B): " << serialized.asChars().size() << std::endl;
}

TEST(DoubleArrayBenchmarkLargeTest, BoostSerialization) {
  using namespace std::chrono;

  auto start_time = high_resolution_clock::now();

  // Construct.
  std::vector<double> origin;
  origin.resize(N_ELEMS);
  for (unsigned long long i = 0; i < N_ELEMS; i++) {
    origin[i] = i + 0.1;
  }

  auto constructed_time = high_resolution_clock::now();

  // Serialize and parse.
  std::ostringstream ostream;
  boost::archive::binary_oarchive oarchive(ostream);
  oarchive << origin;
  const std::string serialized = ostream.str();
  std::istringstream istream(serialized);
  boost::archive::binary_iarchive iarchive(istream);
  std::vector<double> parsed;
  iarchive >> parsed;

  auto finish_time = high_resolution_clock::now();

  EXPECT_EQ(origin, parsed);

  auto construction_time = duration_cast<milliseconds>(constructed_time - start_time).count();
  auto serialization_time = duration_cast<milliseconds>(finish_time - constructed_time).count();
  std::cout << "Construction Time (ms): " << construction_time << std::endl;
  std::cout << "Serialization and Parse Time (ms): " << serialization_time << std::endl;
  std::cout << "Size (B): " << serialized.size() << std::endl;
}

TEST(DoubleArrayBenchmarkLargeTest, HPS) {
  using namespace std::chrono;

  auto start_time = high_resolution_clock::now();

  // Construct.
  std::vector<double> origin;
  origin.resize(N_ELEMS);
  for (unsigned long long i = 0; i < N_ELEMS; i++) {
    origin[i] = i + 0.1;
  }

  auto constructed_time = high_resolution_clock::now();

  // Serialize and parse.
  const std::string serialized = hps::to_string(origin);
  auto parsed = hps::from_string<std::vector<double>>(serialized);

  auto finish_time = high_resolution_clock::now();

  EXPECT_EQ(origin, parsed);

  auto construction_time = duration_cast<milliseconds>(constructed_time - start_time).count();
  auto serialization_time = duration_cast<milliseconds>(finish_time - constructed_time).count();
  std::cout << "Construction Time (ms): " << construction_time << std::endl;
  std::cout << "Serialization and Parse Time (ms): " << serialization_time << std::endl;
  std::cout << "Size (B): " << serialized.size() << std::endl;
}

ext: cc

max_stars_repo_name: thiagofigcosta/V-REx

max_issues_repo_name: thiagofigcosta/V-REx

max_forks_repo_name: thiagofigcosta/V-REx


text: #include <boost/geometry/formulas/vincenty_direct.hpp>


text:
//
// GraphTools library
// Copyright 2017-2019 Illumina, Inc.
// All rights reserved.
//
// Author: Sai Chen <schen6@illumina.com>,
//         Egor Dolzhenko <edolzhenko@illumina.com>
//
// 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.
//

#pragma once

#include <boost/math/distributions/normal.hpp>
#include <cstdint>

using boost::math::normal_distribution;

class DepthTest
{
public:
    /**
     * Set up depth test
     * @param expectedNumReads The mean number of reads when the coverage is as expected
     * @param stdDeviation The standard deviation of the number of reads
     * @param lowerSignificanceThreshold P value cutoff at the lower tail of the distribution
     * @param upperSignificanceThreshold P value cutoff at the upper tail of the distribution
     */
    DepthTest(
        int32_t expectedNumReads, double stdDeviation, double lowerSignificanceThreshold,
        double upperSignificanceThreshold);

    /**
     * Given observed number of reads, return true if pass depth test
     */
    bool testReadCount(int32_t observedNumReads);

private:
    int32_t expectedNumReads_;
    double stdDeviation_;
    double lowerSignificanceThreshold_;
    double upperSignificanceThreshold_;
    const normal_distribution<> coverageDistribution_;
};

ext: hh

max_stars_repo_name: AlesMaver/ExpansionHunter

max_issues_repo_name: AlesMaver/ExpansionHunter

max_forks_repo_name: AlesMaver/ExpansionHunter


text:
#include <catch2/catch.hpp>

#include <Eigen/Core>

#include <ipc/broad_phase/hash_grid.hpp>

using namespace ipc;

// TEST_CASE("AABB initilization", "[hash_grid][AABB]")
// {
//     int dim = GENERATE(2, 3);
//     CAPTURE(dim);
//     AABB aabb;
//     ArrayMax3d actual_center(dim);
//     SECTION("Empty AABB")
//     {
//         aabb = AABB(ArrayMax3d::Zero(dim), ArrayMax3d::Zero(dim));
//         actual_center.setZero();
//     }
//     SECTION("Box centered at zero")
//     {
//         ArrayMax3d min =
//             ArrayMax3d::Random(dim).array() - 1; // in range [-2, 0]
//         ArrayMax3d max = -min;
//         aabb = AABB(min, max);
//         actual_center.setZero();
//     }
//     SECTION("Box not centered at zero")
//     {
//         ArrayMax3d min(dim), max(dim);
//         if (dim == 2) {
//             min << 5.1, 3.14;
//             max << 10.4, 7.89;
//             actual_center << 7.75, 5.515;
//         } else {
//             min << 5.1, 3.14, 7.94;
//             max << 10.4, 7.89, 10.89;
//             actual_center << 7.75, 5.515, 9.415;
//         }
//         aabb = AABB(min, max);
//     }
//     ArrayMax3d center_diff = aabb.getCenter() - actual_center;
//     CHECK(center_diff.matrix().norm() == Approx(0.0).margin(1e-12));
// }

TEST_CASE("AABB overlapping", "[has_grid][AABB]")
{
    AABB a, b;
    bool are_overlapping = false;
    SECTION("a to the right of b")
    {
        a = AABB(Eigen::Array2d(-1, 0), Eigen::Array2d(0, 1));
        SECTION("overlapping")
        {
            b = AABB(Eigen::Array2d(-0.5, 0), Eigen::Array2d(0.5, 1));
            are_overlapping = true;
        }
        SECTION("not overlapping")
        {
            b = AABB(Eigen::Array2d(0.5, 0), Eigen::Array2d(1.5, 1));
            are_overlapping = false;
        }
    }
    SECTION("b to the right of a")
    {
        b = AABB(Eigen::Array2d(-1, 0), Eigen::Array2d(0, 1));
        SECTION("overlapping")
        {
            a = AABB(Eigen::Array2d(-0.5, 0), Eigen::Array2d(0.5, 1));
            are_overlapping = true;
        }
        SECTION("not overlapping")
        {
            a = AABB(Eigen::Array2d(0.5, 0), Eigen::Array2d(1.5, 1));
            are_overlapping = false;
        }
    }
    SECTION("a above b")
    {
        a = AABB(Eigen::Array2d(0, -1), Eigen::Array2d(1, 0));
        SECTION("overlapping")
        {
            b = AABB(Eigen::Array2d(0, -0.5), Eigen::Array2d(1, 0.5));
            are_overlapping = true;
        }
        SECTION("not overlapping")
        {
            b = AABB(Eigen::Array2d(0, 0.5), Eigen::Array2d(1, 1.5));
            are_overlapping = false;
        }
    }
    SECTION("a above b")
    {
        b = AABB(Eigen::Array2d(0, -1), Eigen::Array2d(1, 0));
        SECTION("overlapping")
        {
            a = AABB(Eigen::Array2d(0, -0.5), Eigen::Array2d(1, 0.5));
            are_overlapping = true;
        }
        SECTION("not overlapping")
        {
            a = AABB(Eigen::Array2d(0, 0.5), Eigen::Array2d(1, 1.5));
            are_overlapping = false;
        }
    }
    CHECK(AABB::are_overlapping(a, b) == are_overlapping);
}

max_stars_repo_name: ipc-sim/ipc-toolk

max_issues_repo_name: dbelgrod/ipc-toolkit

max_forks_repo_name: dbelgrod/ipc-toolkit


text:
/**
 * @file
 * @brief We build a tensor product mesh on a torus and apply uniform refinement
 * @author Anian Ruoss
 * @date   2018-10-20 16:27:17
 * @copyright MIT License
 */

#include <boost/program_options.hpp>
#include <iostream>

#include <lf/refinement/mesh_hierarchy.h>
#include "lf/io/io.h"

using size_type = lf::base::size_type;

int main(int argc, char **argv) {
  // define allowed command line arguments:
  namespace po = boost::program_options;
  po::options_description desc("Allowed options");
  desc.add_options()("help", "Produce this help message")(
      "num_steps", po::value<size_t>()->default_value(4),
      "Number of uniform refinement steps")(
      "num_x_cells", po::value<size_type>()->default_value(4),
      "Number of cells in x direction")(
      "num_y_cells", po::value<size_type>()->default_value(4),
      "Number of cells in y direction")(
      "top_right_corner",
      po::value<std::vector<double>>()->multitoken()->default_value(
          std::vector<double>{1., 4.}, "1., 4."),
      "Coordinates of top right corner of rectangle with bottom left at (0,0)");

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

  if (vm.count("help") != 0u) {
    std::cout << desc << std::endl;
    return 1;
  }

  const size_t num_steps = vm["num_steps"].as<size_t>();
  const size_type num_x_cells = vm["num_x_cells"].as<size_type>();
  const size_type num_y_cells = vm["num_y_cells"].as<size_type>();
  std::vector<double> top_right_corner_coords =
      vm["top_right_corner"].as<std::vector<double>>();

  std::shared_ptr<lf::mesh::hybrid2d::MeshFactory> mesh_factory_ptr =
      std::make_shared<lf::mesh::hybrid2d::MeshFactory>(3);

  // build tensor product mesh by specifying rectangle from which torus will
  // be generated by identifying opposite edges
  lf::mesh::hybrid2d::TorusMeshBuilder builder(mesh_factory_ptr);
  builder.setBottomLeftCorner(Eigen::Vector2d{0., 0.});
  builder.setTopRightCorner(Eigen::Vector2d{top_right_corner_coords.data()});
  builder.setNoXCells(num_x_cells);
  builder.setNoYCells(num_y_cells);
  std::shared_ptr<lf::mesh::Mesh> mesh_ptr = builder.Build();

  // output mesh information
  const lf::mesh::Mesh &mesh = *mesh_ptr;
  lf::mesh::utils::PrintInfo(mesh, std::cout);
  std::cout << std::endl;

  // build mesh hierarchy
  lf::refinement::MeshHierarchy multi_mesh(mesh_ptr, mesh_factory_ptr);

  for (int step = 0; step < num_steps; ++step) {
    // obtain pointer to mesh on finest level
    const size_type n_levels = multi_mesh.NumLevels();
    std::shared_ptr<const lf::mesh::Mesh> mesh_fine =
        multi_mesh.getMesh(n_levels - 1);

    // print number of entities of various co-dimensions
    std::cout << "Mesh on level " << n_levels - 1 << ": "
              << mesh_fine->NumEntities(2) << " nodes, "
              << mesh_fine->NumEntities(1) << " edges, "
              << mesh_fine->NumEntities(0) << " cells," << std::endl;

    lf::io::writeMatplotlib(*mesh_fine, std::string("torus_refinement") +
                                            std::to_string(step) + ".csv");

    lf::io::VtkWriter vtk_writer(mesh_fine, std::string("torus_refinement") +
                                                std::to_string(step) + ".vtk");

    multi_mesh.RefineRegular();
  }

  return 0;
}

max_stars_repo_name: Pascal-So/lehrfempp

max_issues_repo_name: Pascal-So/lehrfempp

max_forks_repo_name: Pascal-So/lehrfempp


text:
/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2018, Locus Robotics
 *  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 copyright holder 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 <fuse_core/callback_wrapper.h>
#include <ros/callback_queue.h>
#include <ros/ros.h>

#include <boost/shared_ptr.hpp>
#include <gtest/gtest.h>

#include <functional>
#include <numeric>
#include <vector>

class MyClass
{
public:
  double processData(const std::vector<double>& data)
  {
    return std::accumulate(data.begin(), data.end(), 0.0);
  }

  void processDataInPlace(const std::vector<double>& data, double& output)
  {
    output = std::accumulate(data.begin(), data.end(), 0.0);
  }
};

TEST(CallbackWrapper, Double)
{
  MyClass my_object;
  std::vector<double> data = {1.0, 2.0, 3.0, 4.0, 5.0};
  auto callback = boost::make_shared<fuse_core::CallbackWrapper<double>>(
    std::bind(&MyClass::processData, &my_object, std::ref(data)));
  auto result = callback->getFuture();
  ros::getGlobalCallbackQueue()->addCallback(callback);
  result.wait();

  EXPECT_EQ(15.0, result.get());
}

TEST(CallbackWrapper, Void)
{
  MyClass my_object;
  std::vector<double> data = {1.0, 2.0, 3.0, 4.0, 5.0};
  double output;
  auto callback = boost::make_shared<fuse_core::CallbackWrapper<void>>(
    std::bind(&MyClass::processDataInPlace, &my_object, std::ref(data), std::ref(output)));
  auto result = callback->getFuture();
  ros::getGlobalCallbackQueue()->addCallback(callback);
  result.wait();

  EXPECT_EQ(15.0, output);
}

int main(int argc, char** argv)
{
  testing::InitGoogleTest(&argc, argv);
  ros::init(argc, argv, "callback_wrapper_test");

  ros::AsyncSpinner spinner(1);
  spinner.start();
  int ret = RUN_ALL_TESTS();
  spinner.stop();
  ros::shutdown();
  return ret;
}

max_stars_repo_name: congleetea/fuse

max_issues_repo_name: congleetea/fuse

max_forks_repo_name: congleetea/fuse