<commit_before>/***********************************************************************
 * Software License Agreement (BSD License)
 *
 * Copyright 2012  Carlos D. Correa (info@ngraph.org) All rights reserved.
 *
 * THE BSD LICENSE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. 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.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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.
 *************************************************************************/
#ifndef EMPTYREGIONH
#define EMPTYREGIONH

#include <assert.h>
#include <vector>
#include <utility>
#include <algorithm>
#include <limits>

#include "nglGeometry.hpp"

namespace ngl
{
	template<typename T>
	class EdgeInfo
	{
	public:
		NGLPoint<T> p;
		NGLPoint<T> q;
		NGLPoint<T> midpoint;
		T len2, radius2;
		void initialize()
		{
			Geometry<T>::allocate(midpoint);
			Geometry<T>::allocate(p);
			Geometry<T>::allocate(q);
		}
		void compute(NGLPoint<T> &pin, NGLPoint<T> &qin)
		{
			Geometry<T>::set(p, pin);
			Geometry<T>::set(q, qin);
			Geometry<T>::interpolate(p, q, 0.5, midpoint);
			len2 = Geometry<T>::distanceL2sqr(p,q);
			radius2 = len2/4.0;
		}
		void destroy()
		{
			Geometry<T>::deallocate(midpoint);
			Geometry<T>::deallocate(p);
			Geometry<T>::deallocate(q);
		}
	};

	template<typename T>
	class EmptyRegionTest
	{
	public:
		virtual void initialize() = 0;
		virtual void destroy() = 0;
		virtual T contains(EdgeInfo<T> &edge, NGLPoint<T> &r) = 0;
		virtual ~EmptyRegionTest() { }
	};
	template<typename T>
	class UmbraTest
	{
	public:
		virtual void initialize() = 0;
		virtual void destroy() = 0;
		virtual T shadows(EdgeInfo<T> &edge, NGLPoint<T> &r) = 0;
		virtual ~UmbraTest() { }
	};

	template<typename T>
	class EmptyRegionMethod: public NGMethod<T>
	{
	protected:
		EmptyRegionTest<T> *test;
	public:
		EmptyRegionMethod(EmptyRegionTest<T> *test): NGMethod<T>()
		{
			this->test = test;
		}

		virtual ~EmptyRegionMethod() { }

		virtual void initialize()
    {
			NGMethod<T>::initialize();
		}
		virtual void destroy()
    {
		}
		virtual void getNeighbors(NGLPoint<T> &p, NGLPointSet<T> &points,
                              IndexType **ptrIndices, int &numNeighbors)
    {
			assert(test);
			std::vector<IndexType> neighbors;

			int candidateSize;
			IndexType *candidateNeighbors;

			//
			// Assumes that point set in 'points' has pre-computed a set of neighbors
			// (say, KNN with k = kMax) or if it doesn't, the subset equals the entire
			//  set
			points.getNeighbors(p, &candidateNeighbors, candidateSize);

			EdgeInfo<T> edgeInfo;
			edgeInfo.initialize();
			for(int k = 0; k < candidateSize; k++ )
			{
				IndexType idx = candidateNeighbors[k];
				if(!NGMethod<T>::isValid(idx))
          continue;

				// Pre-compute edge information
				edgeInfo.compute(p, points[idx]);

				if(edgeInfo.len2==0)
          continue;
				bool isRegionEmpty = true;
				for(int j = 0; j < candidateSize; j++ )
				{
					if(j==k)
            continue;
					IndexType idx2 = candidateNeighbors[j];
					if(!NGMethod<T>::isValid(idx2))
            continue;
					T testresult = test->contains(edgeInfo, points[idx2]);
					//DM: Use epsilon to ensure that the edge cases still
					//    fail accordingly (this should fix a peculiarity
					//    in the computation that was only manifested on
					//    Windows)
					if(testresult<=std::numeric_limits<T>::epsilon())
					{
						isRegionEmpty = false;
						break;
					}
				}
				if(isRegionEmpty)
				{
					neighbors.push_back(idx);
				}
			}

			edgeInfo.destroy();
			delete[] candidateNeighbors;

			numNeighbors = (int) neighbors.size();
			if(neighbors.size()>0)
      {
				*ptrIndices = new IndexType[(int) neighbors.size()];
				IndexType *indices = *ptrIndices;
				for(unsigned int k=0;k<neighbors.size();k++)
        {
					indices[k] = neighbors[k];
				}
			}
		}
		virtual void getNeighbors(IndexType queryIndex, NGLPointSet<T> &points,
                              IndexType **ptrIndices, int &numNeighbors)
    {
			assert(test);
			std::vector<IndexType> neighbors;

			int candidateSize;
			IndexType *candidateNeighbors;

			//
			// Assumes that point set in 'points' has pre-computed a set of neighbors
			// (say, KNN with k = kMax) or if it doesn't, the subset equals the entire
			//  set
			points.getNeighbors(queryIndex, &candidateNeighbors, candidateSize);
      NGLPoint<T> p = points[queryIndex];

			EdgeInfo<T> edgeInfo;
			edgeInfo.initialize();
			for(int k = 0; k < candidateSize; k++ )
			{
				IndexType idx = candidateNeighbors[k];
				if(!NGMethod<T>::isValid(idx)) continue;

				// Pre-compute edge information
				edgeInfo.compute(p, points[idx]);

				if(edgeInfo.len2==0) continue;
				bool isRegionEmpty = true;
				for(int j = 0; j < candidateSize; j++ )
				{
					if(j==k) continue;
					IndexType idx2 = candidateNeighbors[j];
					if(!NGMethod<T>::isValid(idx2)) continue;
					T testresult = test->contains(edgeInfo, points[idx2]);
					//DM: Use epsilon to ensure that the edge cases still
					//    fail accordingly (this should fix a peculiarity
					//    in the computation that was only manifested on
					//    Windows)
					if(testresult<=std::numeric_limits<T>::epsilon())
					{
						isRegionEmpty = false;
						break;
					}
				}
				if(isRegionEmpty)
				{
					neighbors.push_back(idx);
				}
			}

			edgeInfo.destroy();
			delete[] candidateNeighbors;

			numNeighbors = (int) neighbors.size();
			if(neighbors.size()>0)
      {
				*ptrIndices = new IndexType[(int) neighbors.size()];
				IndexType *indices = *ptrIndices;
				for(unsigned int k=0;k<neighbors.size();k++)
        {
					indices[k] = neighbors[k];
				}
			}
		}
	};

	template<typename T>
	class RelaxedEmptyRegionMethod: public EmptyRegionMethod<T>
	{
	public:
		RelaxedEmptyRegionMethod(EmptyRegionTest<T> *test)
      : EmptyRegionMethod<T>(test)
		{
		}

		virtual ~RelaxedEmptyRegionMethod() { }

		virtual void initialize()
    {
			EmptyRegionMethod<T>::initialize();
		}
		virtual void destroy()
    {
			EmptyRegionMethod<T>::destroy();
		}
		virtual void getNeighbors(NGLPoint<T> &p, NGLPointSet<T> &points,
                              IndexType **ptrIndices, int &numNeighbors)
    {
			assert(this->test);
			std::vector<IndexType> neighbors;

			int candidateSize;
			IndexType *candidateNeighbors;

			//
			// Assumes that point set in 'points' has pre-computed a set of neighbors
			// (say, KNN with k = kMax) Or if it doesn't, the subset equals the entire
			//  set
			// Assume these points are ordered by distance?
			// DM: Assume nothing, sort them now!
			points.getNeighbors(p, &candidateNeighbors, candidateSize);

			std::vector< std::pair<T,int> > sortedIndices;
			for (int k = 0; k < candidateSize; k++)
			{
				IndexType idx = candidateNeighbors[k];
				T distance = Geometry<T>::distanceL2sqr(p,points[idx]);
				sortedIndices.push_back(std::pair<T,int>(distance,idx));
			}
			std::sort(sortedIndices.begin(),sortedIndices.end());
			//DM: Now just put them back and be done with the vector
			for (int k = 0; k < candidateSize; k++)
				candidateNeighbors[k] = sortedIndices[k].second;

			EdgeInfo<T> edgeInfo;
			edgeInfo.initialize();
			std::vector<int> added;
			for(unsigned int k = 0; k < (unsigned int) candidateSize; k++ )
			{
				IndexType idx = candidateNeighbors[k];
				if(!NGMethod<T>::isValid(idx))
          continue;

				// Pre-compute edge information
				edgeInfo.compute(p, points[idx]);

				if(edgeInfo.len2==0)
          continue;

				bool isRegionEmpty = true;
				for(unsigned int i = 0; i < added.size(); i++ )
				{
					unsigned int j = added[i];
					if(j==k)
            continue;
					IndexType idx2 = candidateNeighbors[j];
					if(!NGMethod<T>::isValid(idx2))
            continue;
					T testresult = this->test->contains(edgeInfo, points[idx2]);
					//DM: Use epsilon to ensure that the edge cases still
					//    fail accordingly (this should fix a peculiarity
					//    in the computation that was only manifested on
					//    Windows)
					if(testresult<=std::numeric_limits<T>::epsilon())
					{
						isRegionEmpty = false;
						break;
					}
				}
				if(isRegionEmpty)
				{
					added.push_back(k);
					neighbors.push_back(idx);
				}
			}

			edgeInfo.destroy();
			delete[] candidateNeighbors;
			numNeighbors = (int) neighbors.size();
			if(neighbors.size()>0)
      {
				*ptrIndices = new IndexType[(int) neighbors.size()];
				IndexType *indices = *ptrIndices;
				for(unsigned int k=0;k<neighbors.size();k++)
        {
					indices[k] = neighbors[k];
				}
			}
		}

		virtual void getNeighbors(IndexType queryIndex, NGLPointSet<T> &points,
                              IndexType **ptrIndices, int &numNeighbors)
    {
			assert(this->test);
			std::vector<IndexType> neighbors;

			int candidateSize;
			IndexType *candidateNeighbors;

			//
			// Assumes that point set in 'points' has pre-computed a set of neighbors
			// (say, KNN with k = kMax) Or if it doesn't, the subset equals the entire
			//  set
			// Assume these points are ordered by distance?
			points.getNeighbors(queryIndex, &candidateNeighbors, candidateSize);
      NGLPoint<T> p = points[queryIndex];

			// DM: Assume nothing, sort them now!
			std::vector< std::pair<T,int> > sortedIndices;
			for (int k = 0; k < candidateSize; k++)
			{
				IndexType idx = candidateNeighbors[k];
				T distance = Geometry<T>::distanceL2sqr(p,points[idx]);
				sortedIndices.push_back(std::pair<T,int>(distance,idx));
			}
			std::sort(sortedIndices.begin(),sortedIndices.end());
			//DM: Now just put them back and be done with the vector
			for (int k = 0; k < candidateSize; k++)
				candidateNeighbors[k] = sortedIndices[k].second;

			EdgeInfo<T> edgeInfo;
			edgeInfo.initialize();
			std::vector<int> added;
			for(unsigned int k = 0; k < (unsigned int) candidateSize; k++ )
			{
				IndexType idx = candidateNeighbors[k];
				if(!NGMethod<T>::isValid(idx))
          continue;

				// Pre-compute edge information
				edgeInfo.compute(p, points[idx]);

				if(edgeInfo.len2==0)
          continue;

				bool isRegionEmpty = true;
				for(unsigned int i = 0; i < added.size(); i++ )
				{
					unsigned int j = added[i];
					if(j==k)
            continue;
					IndexType idx2 = candidateNeighbors[j];
					if(!NGMethod<T>::isValid(idx2))
            continue;
					T testresult = this->test->contains(edgeInfo, points[idx2]);
					//DM: Use epsilon to ensure that the edge cases still
					//    fail accordingly (this should fix a peculiarity
					//    in the computation that was only manifested on
					//    Windows)
					if(testresult<=std::numeric_limits<T>::epsilon())
					{
						isRegionEmpty = false;
						break;
					}
				}
				if(isRegionEmpty)
				{
					added.push_back(k);
					neighbors.push_back(idx);
				}
			}

			edgeInfo.destroy();
			delete[] candidateNeighbors;
			numNeighbors = (int) neighbors.size();
			if(neighbors.size()>0)
      {
				*ptrIndices = new IndexType[(int) neighbors.size()];
				IndexType *indices = *ptrIndices;
				for(unsigned int k=0;k<neighbors.size();k++)
        {
					indices[k] = neighbors[k];
				}
			}
		}
	};
}

template class ngl::EdgeInfo<float>;
template class ngl::EmptyRegionTest<float>;
template class ngl::UmbraTest<float>;
template class ngl::RelaxedEmptyRegionMethod<float>;
template class ngl::EmptyRegionMethod<float>;

template class ngl::EdgeInfo<double>;
template class ngl::EmptyRegionTest<double>;
template class ngl::UmbraTest<double>;
template class ngl::RelaxedEmptyRegionMethod<double>;
template class ngl::EmptyRegionMethod<double>;
#endif
<commit_msg>Commit for creating a new merge request, since the last one snuck under the radar.<commit_after>/***********************************************************************
 * Software License Agreement (BSD License)
 *
 * Copyright 2012  Carlos D. Correa (info@ngraph.org) All rights reserved.
 *
 * THE BSD LICENSE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. 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.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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.
 *************************************************************************/
#ifndef EMPTYREGIONH
#define EMPTYREGIONH

#include <assert.h>
#include <vector>
#include <utility>
#include <algorithm>
#include <limits>

#include "nglGeometry.hpp"

namespace ngl
{
	template<typename T>
	class EdgeInfo
	{
	public:
		NGLPoint<T> p;
		NGLPoint<T> q;
		NGLPoint<T> midpoint;
		T len2, radius2;
		void initialize()
		{
			Geometry<T>::allocate(midpoint);
			Geometry<T>::allocate(p);
			Geometry<T>::allocate(q);
		}
		void compute(NGLPoint<T> &pin, NGLPoint<T> &qin)
		{
			Geometry<T>::set(p, pin);
			Geometry<T>::set(q, qin);
			Geometry<T>::interpolate(p, q, 0.5, midpoint);
			len2 = Geometry<T>::distanceL2sqr(p,q);
			radius2 = len2/4.0;
		}
		void destroy()
		{
			Geometry<T>::deallocate(midpoint);
			Geometry<T>::deallocate(p);
			Geometry<T>::deallocate(q);
		}
	};

	template<typename T>
	class EmptyRegionTest
	{
	public:
		virtual void initialize() = 0;
		virtual void destroy() = 0;
		virtual T contains(EdgeInfo<T> &edge, NGLPoint<T> &r) = 0;
		virtual ~EmptyRegionTest() { }
	};
	template<typename T>
	class UmbraTest
	{
	public:
		virtual void initialize() = 0;
		virtual void destroy() = 0;
		virtual T shadows(EdgeInfo<T> &edge, NGLPoint<T> &r) = 0;
		virtual ~UmbraTest() { }
	};

	template<typename T>
	class EmptyRegionMethod: public NGMethod<T>
	{
	protected:
		EmptyRegionTest<T> *test;
	public:
		EmptyRegionMethod(EmptyRegionTest<T> *test): NGMethod<T>()
		{
			this->test = test;
		}

		virtual ~EmptyRegionMethod() { }

		virtual void initialize()
    {
			NGMethod<T>::initialize();
		}
		virtual void destroy()
    {
		}
		virtual void getNeighbors(NGLPoint<T> &p, NGLPointSet<T> &points,
                              IndexType **ptrIndices, int &numNeighbors)
    {
			assert(test);
			std::vector<IndexType> neighbors;

			int candidateSize;
			IndexType *candidateNeighbors;

			//
			// Assumes that point set in 'points' has pre-computed a set of neighbors
			// (say, KNN with k = kMax) or if it doesn't, the subset equals the entire
			//  set
			points.getNeighbors(p, &candidateNeighbors, candidateSize);

			EdgeInfo<T> edgeInfo;
			edgeInfo.initialize();
			for(int k = 0; k < candidateSize; k++ )
			{
				IndexType idx = candidateNeighbors[k];
				if(!NGMethod<T>::isValid(idx))
          continue;

				// Pre-compute edge information
				edgeInfo.compute(p, points[idx]);

				if(edgeInfo.len2==0)
          continue;
				bool isRegionEmpty = true;
				for(int j = 0; j < candidateSize; j++ )
				{
					if(j==k)
            continue;
					IndexType idx2 = candidateNeighbors[j];
					if(!NGMethod<T>::isValid(idx2))
            continue;
					T testresult = test->contains(edgeInfo, points[idx2]);
					//DM: Use epsilon to ensure that the edge cases still
					//    fail accordingly (this should fix a peculiarity
					//    in the computation that was only manifested on
					//    Windows)
					if(testresult<=std::numeric_limits<T>::epsilon())
					{
						isRegionEmpty = false;
						break;
					}
				}
				if(isRegionEmpty)
				{
					neighbors.push_back(idx);
				}
			}

			edgeInfo.destroy();
			delete[] candidateNeighbors;

			numNeighbors = (int) neighbors.size();
			if(neighbors.size()>0)
      {
				*ptrIndices = new IndexType[(int) neighbors.size()];
				IndexType *indices = *ptrIndices;
				for(unsigned int k=0;k<neighbors.size();k++)
        {
					indices[k] = neighbors[k];
				}
			}
		}
		virtual void getNeighbors(IndexType queryIndex, NGLPointSet<T> &points,
                              IndexType **ptrIndices, int &numNeighbors)
    {
			assert(test);
			std::vector<IndexType> neighbors;

			int candidateSize;
			IndexType *candidateNeighbors;

			//
			// Assumes that point set in 'points' has pre-computed a set of neighbors
			// (say, KNN with k = kMax) or if it doesn't, the subset equals the entire
			//  set
			points.getNeighbors(queryIndex, &candidateNeighbors, candidateSize);
      NGLPoint<T> p = points[queryIndex];

			EdgeInfo<T> edgeInfo;
			edgeInfo.initialize();
			for(int k = 0; k < candidateSize; k++ )
			{
				IndexType idx = candidateNeighbors[k];
				if(!NGMethod<T>::isValid(idx)) continue;

				// Pre-compute edge information
				edgeInfo.compute(p, points[idx]);

				if(edgeInfo.len2==0) continue;
				bool isRegionEmpty = true;
				for(int j = 0; j < candidateSize; j++ )
				{
					if(j==k) continue;
					IndexType idx2 = candidateNeighbors[j];
					if(!NGMethod<T>::isValid(idx2)) continue;
					T testresult = test->contains(edgeInfo, points[idx2]);
					//DM: Use epsilon to ensure that the edge cases still
					//    fail accordingly (this should fix a peculiarity
					//    in the computation that was only manifested on
					//    Windows)
					if(testresult<=std::numeric_limits<T>::epsilon())
					{
						isRegionEmpty = false;
						break;
					}
				}
				if(isRegionEmpty)
				{
					neighbors.push_back(idx);
				}
			}

			edgeInfo.destroy();
			delete[] candidateNeighbors;

			numNeighbors = (int) neighbors.size();
			if(neighbors.size()>0)
      {
				*ptrIndices = new IndexType[(int) neighbors.size()];
				IndexType *indices = *ptrIndices;
				for(unsigned int k=0;k<neighbors.size();k++)
        {
					indices[k] = neighbors[k];
				}
			}
		}
	};

	template<typename T>
	class RelaxedEmptyRegionMethod: public EmptyRegionMethod<T>
	{
	public:
		RelaxedEmptyRegionMethod(EmptyRegionTest<T> *test)
      : EmptyRegionMethod<T>(test)
		{
		}

		virtual ~RelaxedEmptyRegionMethod() { }

		virtual void initialize()
    {
			EmptyRegionMethod<T>::initialize();
		}
		virtual void destroy()
    {
			EmptyRegionMethod<T>::destroy();
		}
		virtual void getNeighbors(NGLPoint<T> &p, NGLPointSet<T> &points,
                              IndexType **ptrIndices, int &numNeighbors)
    {
			assert(this->test);
			std::vector<IndexType> neighbors;

			int candidateSize;
			IndexType *candidateNeighbors;

			//
			// Assumes that point set in 'points' has pre-computed a set of neighbors
			// (say, KNN with k = kMax) Or if it doesn't, the subset equals the entire
			//  set
			// Assume these points are ordered by distance?
			// DM: Assume nothing, sort them now!
			points.getNeighbors(p, &candidateNeighbors, candidateSize);

			std::vector< std::pair<T,int> > sortedIndices;
			for (int k = 0; k < candidateSize; k++)
			{
				IndexType idx = candidateNeighbors[k];
				T distance = Geometry<T>::distanceL2sqr(p,points[idx]);
				sortedIndices.push_back(std::pair<T,int>(distance,idx));
			}
			std::sort(sortedIndices.begin(),sortedIndices.end());
			//DM: Now just put them back and be done with the vector
			for (int k = 0; k < candidateSize; k++)
				candidateNeighbors[k] = sortedIndices[k].second;

			EdgeInfo<T> edgeInfo;
			edgeInfo.initialize();
			std::vector<int> added;
			for(unsigned int k = 0; k < (unsigned int) candidateSize; k++ )
			{
				IndexType idx = candidateNeighbors[k];
				if(!NGMethod<T>::isValid(idx))
          continue;

				// Pre-compute edge information
				edgeInfo.compute(p, points[idx]);

				if(edgeInfo.len2==0)
          continue;

				bool isRegionEmpty = true;
				for(unsigned int i = 0; i < added.size(); i++ )
				{
					unsigned int j = added[i];
					if(j==k)
            continue;
					IndexType idx2 = candidateNeighbors[j];
					if(!NGMethod<T>::isValid(idx2))
            continue;
					T testresult = this->test->contains(edgeInfo, points[idx2]);
					//DM: Use epsilon to ensure that the edge cases still
					//    fail accordingly (this should fix a peculiarity
					//    in the computation that was only manifested on
					//    Windows)
					if(testresult<=std::numeric_limits<T>::epsilon())
					{
						isRegionEmpty = false;
						break;
					}
				}
				if(isRegionEmpty)
				{
					added.push_back(k);
					neighbors.push_back(idx);
				}
			}

			edgeInfo.destroy();
			delete[] candidateNeighbors;
			numNeighbors = (int) neighbors.size();
			if(neighbors.size()>0)
      {
				*ptrIndices = new IndexType[(int) neighbors.size()];
				IndexType *indices = *ptrIndices;
				for(unsigned int k=0;k<neighbors.size();k++)
        {
					indices[k] = neighbors[k];
				}
			}
		}

		virtual void getNeighbors(IndexType queryIndex, NGLPointSet<T> &points,
                              IndexType **ptrIndices, int &numNeighbors)
    {
			assert(this->test);
			std::vector<IndexType> neighbors;

			int candidateSize;
			IndexType *candidateNeighbors;

			//
			// Assumes that point set in 'points' has pre-computed a set of neighbors
			// (say, KNN with k = kMax) Or if it doesn't, the subset equals the entire
			//  set
			// Assume these points are ordered by distance?
			points.getNeighbors(queryIndex, &candidateNeighbors, candidateSize);
      NGLPoint<T> p = points[queryIndex];

			// DM: Assume nothing, sort them now!
			std::vector< std::pair<T,int> > sortedIndices;
			for (int k = 0; k < candidateSize; k++)
			{
				IndexType idx = candidateNeighbors[k];
				T distance = Geometry<T>::distanceL2sqr(p,points[idx]);
				sortedIndices.push_back(std::pair<T,int>(distance,idx));
			}
			std::sort(sortedIndices.begin(),sortedIndices.end());
			//DM: Now just put them back and be done with the vector
			for (int k = 0; k < candidateSize; k++)
				candidateNeighbors[k] = sortedIndices[k].second;

			EdgeInfo<T> edgeInfo;
			edgeInfo.initialize();
			std::vector<int> added;
			for(unsigned int k = 0; k < (unsigned int) candidateSize; k++ )
			{
				IndexType idx = candidateNeighbors[k];
				if(!NGMethod<T>::isValid(idx))
          continue;

				// Pre-compute edge information
				edgeInfo.compute(p, points[idx]);

				if(edgeInfo.len2==0)
          continue;

				bool isRegionEmpty = true;
				for(unsigned int i = 0; i < added.size(); i++ )
				{
					unsigned int j = added[i];
					if(j==k)
            continue;
					IndexType idx2 = candidateNeighbors[j];
					if(!NGMethod<T>::isValid(idx2))
            continue;
					T testresult = this->test->contains(edgeInfo, points[idx2]);
					//DM: Use epsilon to ensure that the edge cases still
					//    fail accordingly (this should fix a peculiarity
					//    in the computation that was only manifested on
					//    Windows)
					if(testresult<=std::numeric_limits<T>::epsilon())
					{
						isRegionEmpty = false;
						break;
					}
				}
				if(isRegionEmpty)
				{
					added.push_back(k);
					neighbors.push_back(idx);
				}
			}

			edgeInfo.destroy();
			delete[] candidateNeighbors;
			numNeighbors = (int) neighbors.size();
			if(neighbors.size()>0)
      {
				*ptrIndices = new IndexType[(int) neighbors.size()];
				IndexType *indices = *ptrIndices;
				for(unsigned int k=0;k<neighbors.size();k++)
        {
					indices[k] = neighbors[k];
				}
			}
		}
	};
}

template class ngl::EdgeInfo<float>;
template class ngl::EmptyRegionTest<float>;
template class ngl::UmbraTest<float>;
template class ngl::RelaxedEmptyRegionMethod<float>;
template class ngl::EmptyRegionMethod<float>;

template class ngl::EdgeInfo<double>;
template class ngl::EmptyRegionTest<double>;
template class ngl::UmbraTest<double>;
template class ngl::RelaxedEmptyRegionMethod<double>;
template class ngl::EmptyRegionMethod<double>;
#endif<|endoftext|>


<commit_before>#ifndef _GRAPH_HPP_
#define _GRAPH_HPP_
#include <iostream>
#include <vector>
#include "Arc.hpp"
#include "Vertex.hpp"

using std::vector;
using std::cout;
using std::endl;

template<class T, class C>
class Graph
{
    private:
        Vertex<T,C> *graph;

    public:
        Graph() : graph(NULL) { };
        Graph(const Graph &g);
        ~Graph();

        int order() const;
        vector<T> successors(T) const;
        vector<T> predecessors(T) const;
        Vertex<T,C>* findVertex(T) const;
        void insertVertex(T);
        void insertArc(T, T, C);
        void deleteVertex(T);

        template<class E, class F>
        friend std::ostream& operator<<(std::ostream&, const Graph<E,F>&);
};

template<class T, class C>
Graph<T,C>::Graph(const Graph &g)
{
    if (g.graph != NULL)
    {
        Vertex<T,C> *p;
        Vertex<T,C> *q;
        this->graph = new Vertex<T,C>(g.graph->getKey());
        p = g.graph;
        q = this->graph;

        while (p->getNext() != NULL)
        {
            p = p->getNext();
            q->setNext(new Vertex<T,C>(p->getKey()));
            q = q->getNext();
        }

        for (p = g.graph; p; p = p->getNext())
        {
            Arc<T,C> *a = p->ady;
            while (a)
            {
                this->insertArc(p->getKey(), a->getKey(), a->getCost());
                a = a->getNext();
            }
        }
    }
}

template<class T, class C>
Graph<T,C>::~Graph()
{
    Vertex<T,C> *p;
    while (graph)
    {
        p = graph;
        graph = graph->getNext();
        delete p;
    }
}

template<class T, class C>
vector<T> Graph<T,C>::successors(T e) const
{
    Vertex<T,C> *p = findVertex(e);
    if (p)
        return p->successors;
}

template<class T, class C>
vector<T> Graph<T,C>::predecessors(T e) const
{
    Vertex<T,C> *p, *pivot;
    vector<T> out;
    p = findVertex(e);
    if (p)
    {
        pivot = graph;
        while (pivot)
        {
            if (pivot != p)
            {
                Arc<T,C> *a = pivot->ady;
                while (a)
                {
                    if (a->getVertex() == p)
                        out.push_back(pivot->getKey());

                    a = a->getNext();
                }
            }
            pivot = pivot->getNext();
        }
    }
    return out;
}

template<class T, class C>
int Graph<T,C>::order() const
{
    Vertex<T,C> *pivot = graph;
    int cont = 0;
    while (graph)
    {
        pivot = pivot->getNext();
        cont++;
    }
    return cont;
}

template<class T, class C>
Vertex<T,C>* Graph<T,C>::findVertex(T e) const
{
    Vertex<T,C>* pivot = graph;

    while (pivot)
    {
        if (pivot->getKey() == e)
            return pivot;

        pivot = pivot->getNext();
    }

    return NULL;
}

template<class T, class C>
void Graph<T,C>::insertVertex(T v)
{
    if (graph == NULL)
        graph = new Vertex<T,C>(v);
    else if (graph->getKey() > v)
    {
        Vertex<T,C> *n = new Vertex<T,C>(v);
        n->setNext(graph);
        graph = n;
    }
    else
    {
        Vertex<T,C> *pivot = graph;
        Vertex<T,C> *before;

        while (pivot && pivot->getKey() <= v)
        {
            if (pivot)
            {
                if (pivot->getKey() == v)
                    before = NULL;
                else
                    before = pivot;
            }
            pivot = pivot->getNext();
        }

        if (before)
        {
            before->setNext(new Vertex<T,C>(v));
            before->getNext()->setNext(pivot);
        }
    }
}

template<class T, class C>
void Graph<T,C>::insertArc(T v, T w, C c)
{
    if (v != w)
    {
        Vertex<T,C> *V = findVertex(v);
        Vertex<T,C> *W = findVertex(w);

        if (V != NULL && W != NULL)
            V->insertArc(W, c);
    }
}

template<class T, class C>
void Graph<T,C>::deleteVertex(T v)
{
    Vertex<T,C> *V = findVertex(v);
    if (V)
    {
        Vertex<T,C> *pivot;
        for (pivot = graph; pivot; pivot = pivot->getNext())
            pivot->deleteArc(V);

        if (V != graph)
        {
            pivot = graph;
            while (pivot->getNext() != V)
                pivot = pivot->getNext();

            pivot->setNext(V->getNext());
        }
        else
            graph = graph->getNext();

        delete V;
    }
}

template<class E, class F>
std::ostream& operator<<(std::ostream& out, const Graph<E,F> &g)
{
    Vertex<E,F> *pivot = g.graph;
    while (pivot)
    {
        out << pivot->getKey();
        Arc<E,F> *a = pivot->getAdy();
        while (a)
        {
            out << " " << a->getKey();
            a = a->getNext();
        }
        out << endl;
        pivot = pivot->getNext();
    }
    return out;
}

#endif
<commit_msg>order fixed<commit_after>#ifndef _GRAPH_HPP_
#define _GRAPH_HPP_
#include <iostream>
#include <vector>
#include "Arc.hpp"
#include "Vertex.hpp"

using std::vector;
using std::cout;
using std::endl;

template<class T, class C>
class Graph
{
    private:
        Vertex<T,C> *graph;

    public:
        Graph() : graph(NULL) { };
        Graph(const Graph &g);
        ~Graph();

        int order() const;
        vector<T> successors(T) const;
        vector<T> predecessors(T) const;
        Vertex<T,C>* findVertex(T) const;
        void insertVertex(T);
        void insertArc(T, T, C);
        void deleteVertex(T);

        template<class E, class F>
        friend std::ostream& operator<<(std::ostream&, const Graph<E,F>&);
};

template<class T, class C>
Graph<T,C>::Graph(const Graph &g)
{
    if (g.graph != NULL)
    {
        Vertex<T,C> *p;
        Vertex<T,C> *q;
        this->graph = new Vertex<T,C>(g.graph->getKey());
        p = g.graph;
        q = this->graph;

        while (p->getNext() != NULL)
        {
            p = p->getNext();
            q->setNext(new Vertex<T,C>(p->getKey()));
            q = q->getNext();
        }

        for (p = g.graph; p; p = p->getNext())
        {
            Arc<T,C> *a = p->ady;
            while (a)
            {
                this->insertArc(p->getKey(), a->getKey(), a->getCost());
                a = a->getNext();
            }
        }
    }
}

template<class T, class C>
Graph<T,C>::~Graph()
{
    Vertex<T,C> *p;
    while (graph)
    {
        p = graph;
        graph = graph->getNext();
        delete p;
    }
}

template<class T, class C>
vector<T> Graph<T,C>::successors(T e) const
{
    Vertex<T,C> *p = findVertex(e);
    if (p)
        return p->successors;
}

template<class T, class C>
vector<T> Graph<T,C>::predecessors(T e) const
{
    Vertex<T,C> *p, *pivot;
    vector<T> out;
    p = findVertex(e);
    if (p)
    {
        pivot = graph;
        while (pivot)
        {
            if (pivot != p)
            {
                Arc<T,C> *a = pivot->ady;
                while (a)
                {
                    if (a->getVertex() == p)
                        out.push_back(pivot->getKey());

                    a = a->getNext();
                }
            }
            pivot = pivot->getNext();
        }
    }
    return out;
}

template<class T, class C>
int Graph<T,C>::order() const
{
    Vertex<T,C> *pivot = graph;
    int cont = 0;
    while (pivot)
    {
        cont++;
        pivot = pivot->getNext();

    }
    return cont;
}

template<class T, class C>
Vertex<T,C>* Graph<T,C>::findVertex(T e) const
{
    Vertex<T,C>* pivot = graph;

    while (pivot)
    {
        if (pivot->getKey() == e)
            return pivot;

        pivot = pivot->getNext();
    }

    return NULL;
}

template<class T, class C>
void Graph<T,C>::insertVertex(T v)
{
    if (graph == NULL)
        graph = new Vertex<T,C>(v);
    else if (graph->getKey() > v)
    {
        Vertex<T,C> *n = new Vertex<T,C>(v);
        n->setNext(graph);
        graph = n;
    }
    else
    {
        Vertex<T,C> *pivot = graph;
        Vertex<T,C> *before;

        while (pivot && pivot->getKey() <= v)
        {
            if (pivot)
            {
                if (pivot->getKey() == v)
                    before = NULL;
                else
                    before = pivot;
            }
            pivot = pivot->getNext();
        }

        if (before)
        {
            before->setNext(new Vertex<T,C>(v));
            before->getNext()->setNext(pivot);
        }
    }
}

template<class T, class C>
void Graph<T,C>::insertArc(T v, T w, C c)
{
    if (v != w)
    {
        Vertex<T,C> *V = findVertex(v);
        Vertex<T,C> *W = findVertex(w);

        if (V != NULL && W != NULL)
            V->insertArc(W, c);
    }
}

template<class T, class C>
void Graph<T,C>::deleteVertex(T v)
{
    Vertex<T,C> *V = findVertex(v);
    if (V)
    {
        Vertex<T,C> *pivot;
        for (pivot = graph; pivot; pivot = pivot->getNext())
            pivot->deleteArc(V);

        if (V != graph)
        {
            pivot = graph;
            while (pivot->getNext() != V)
                pivot = pivot->getNext();

            pivot->setNext(V->getNext());
        }
        else
            graph = graph->getNext();

        delete V;
    }
}

template<class E, class F>
std::ostream& operator<<(std::ostream& out, const Graph<E,F> &g)
{
    Vertex<E,F> *pivot = g.graph;
    while (pivot)
    {
        out << pivot->getKey();
        Arc<E,F> *a = pivot->getAdy();
        while (a)
        {
            out << " " << a->getKey();
            a = a->getNext();
        }
        out << endl;
        pivot = pivot->getNext();
    }
    return out;
}

#endif
<|endoftext|>


<commit_before>#ifndef MJOLNIR_PARTICLE
#define MJOLNIR_PARTICLE
#include <mjolnir/util/scalar_type_of.hpp>

namespace mjolnir
{

template<typename coordT>
struct Particle
{
    scalar_type<coordT> mass;
    coordT position;
    coordT velocity;
    coordT force;
};

} // mjolnir
#endif /* MJOLNIR_PARTICLE */
<commit_msg>add helper function make_particle<commit_after>#ifndef MJOLNIR_PARTICLE
#define MJOLNIR_PARTICLE
#include <mjolnir/util/scalar_type_of.hpp>

namespace mjolnir
{

template<typename coordT>
struct Particle
{
    scalar_type<coordT> mass;
    coordT position;
    coordT velocity;
    coordT force;
};

template<typename coordT>
constexpr inline Particle<coordT>
make_particle(const scalar_type<coordT> mass, const coordT& pos,
              const coordT& vel, const coordT& f)
{
    return Particle<coordT>{mass, pos, vel, f};
}

} // mjolnir
#endif /* MJOLNIR_PARTICLE */
<|endoftext|>


<commit_before>/*
 * DistributionContainer.C
 *
 *  Created on: Jul 6, 2012
 *      Author: alfoa
 */
#include "DistributionContainer.h"
//#include "Factory.h"
//#include "distribution_1D.h"
#include "distribution_min.h"
#include <iostream>
#include <math.h>
#include <cmath>
#include <cstdlib>
#include <stdlib.h>
#include <vector>
#include <map>
using namespace std;

#ifndef mooseError
#define mooseError(msg) { std::cerr << "\n\n" << msg << "\n\n"; }
#endif

class DistributionContainer;

DistributionContainer::DistributionContainer()
{
}
DistributionContainer::~DistributionContainer()
{
}

/*void
DistributionContainer::addDistributionInContainer(const std::string & type, const std::string & name, InputParameters params){
   // create the distribution type
   distribution * dist = dynamic_cast<distribution *>(Factory::instance()->create(type, name, params));
   if (_dist_by_name.find(name) == _dist_by_name.end())
    _dist_by_name[name] = dist;
   else
     mooseError("Distribution with name " << name << " already exists");

   _dist_by_type[type].push_back(dist);

   }*/

void
DistributionContainer::addDistributionInContainer(const std::string & type, const std::string & name, distribution * dist){
   // create the distribution type
  //distribution * dist = dynamic_cast<distribution *>(Factory::instance()->create(type, name, params));
   if (_dist_by_name.find(name) == _dist_by_name.end())
    _dist_by_name[name] = dist;
   else
     mooseError("Distribution with name " << name << " already exists");

   _dist_by_type[type].push_back(dist);

}

//void
//DistributionContainer::constructDistributionContainer(std::string DistAlias, distribution_type type, double xmin, double xmax, double param1, double param2, unsigned int seed){
//	_distribution_cont.push_back(distribution(type, xmin, xmax, param1, param2, seed));
//	_vector_pos_map[DistAlias]=_distribution_cont.size()-1;
//}
//void
//DistributionContainer::constructDistributionContainerCustom(std::string DistAlias, distribution_type type, std::vector< double > dist_x, std::vector< double > dist_y, int numPoints, custom_dist_fit_type fit_type, unsigned int seed){
//
//	_distribution_cont.push_back(distribution(dist_x, dist_y, numPoints, fit_type, seed));
//	_vector_pos_map[DistAlias]=_distribution_cont.size()-1;
//}

std::string
DistributionContainer::getType(std::string DistAlias){

    if(_dist_by_name.find(DistAlias) != _dist_by_name.end()){
       distribution * dist = _dist_by_name.find(DistAlias)->second;
       std::string type = getDistributionType(*dist);
       if(type == "DistributionError"){
         mooseError("Type for distribution " << DistAlias << " not found");
       }
       return type;
    }
    else{
       mooseError("Distribution " << DistAlias << " not found in distribution container");
       return "DistributionError";
    }

}

void
DistributionContainer::seedRandom(unsigned int seed){
   srand( seed );
}
double
DistributionContainer::random(){
   return (rand()/RAND_MAX);
//   return -1.0;
}

// to be implemented
bool DistributionContainer::checkCdf(double probability, std::vector<double> values){
   return false;
}
// end to be implemented
// to be implemented
bool DistributionContainer::checkCdf(double probability, double value){
   return false;
}
// end to be implemented

double
DistributionContainer::getVariable(std::string paramName,std::string DistAlias){
    if(_dist_by_name.find(DistAlias) != _dist_by_name.end()){
       distribution * dist = _dist_by_name.find(DistAlias)->second;
       return getDistributionVariable(*dist,paramName);
    }
    mooseError("Distribution " << DistAlias << " not found in distribution container");
    return -1;
}

void
DistributionContainer::updateVariable(std::string paramName,double newValue,std::string DistAlias){
    if(_dist_by_name.find(DistAlias) != _dist_by_name.end()){
       distribution * dist = _dist_by_name.find(DistAlias)->second;
       DistributionUpdateVariable(*dist,paramName,newValue);
    }
    mooseError("Distribution " + DistAlias + " was not found in distribution container.");
}

//double
//DistributionContainer::getMin(std::string DistAlias){
//    int position;
//
//    position = getPosition(DistAlias);
//
//    if(position != -1){
//    	return _distribution_cont[position].getMin();
//    }
//    else{
//    	std::cerr << " ERROR: distribution " << DistAlias << "not present in the mapping";
//    	return DISTRIBUTION_ERROR;
//    }
//
//}
//
//double
//DistributionContainer::getMax(std::string DistAlias){
//    int position;
//
//    position = getPosition(DistAlias);
//
//    if(position != -1){
//    	return _distribution_cont[position].getMax();
//    }
//    else{
//    	std::cerr << " ERROR: distribution " << DistAlias << "not present in the mapping";
//    	return DISTRIBUTION_ERROR;
//    }
//
//}
//
//double
//DistributionContainer::getParamater1(std::string DistAlias){
//    int position;
//
//    position = getPosition(DistAlias);
//
//    if(position != -1){
//    	return _distribution_cont[position].getParamater1();
//    }
//    else{
//    	std::cerr << " ERROR: distribution " << DistAlias << "not present in the mapping";
//    	return DISTRIBUTION_ERROR;
//    }
//
//}
//
//double
//DistributionContainer::getParameter2(std::string DistAlias){
//    int position;
//
//    position = getPosition(DistAlias);
//
//    if(position != -1){
//    	return _distribution_cont[position].getParameter2();
//    }
//    else{
//    	std::cerr << " ERROR: distribution " << DistAlias << "not present in the mapping";
//    	return DISTRIBUTION_ERROR;
//    }
//
//}
//
//void
//DistributionContainer::changeParameter1(std::string DistAlias,double newParameter1){
//    int position;
//
//    position = getPosition(DistAlias);
//
//    if(position != -1){
//    	_distribution_cont[position].changeParameter1(newParameter1);
//    }
//    else{
//    	std::cerr << " ERROR: distribution " << DistAlias << "not present in the mapping";
//    }
//
//}
//
//void
//DistributionContainer::changeParameter2(std::string DistAlias,double newParameter2){
//    int position;
//
//    position = getPosition(DistAlias);
//
//    if(position != -1){
//    	_distribution_cont[position].changeParameter2(newParameter2);
//    }
//    else{
//    	std::cerr << " ERROR: distribution " << DistAlias << "not present in the mapping";
//    }
//
//}

double
DistributionContainer::Pdf(std::string DistAlias, double x){

    if(_dist_by_name.find(DistAlias) != _dist_by_name.end()){
       distribution * dist = _dist_by_name.find(DistAlias)->second;
       return DistributionPdf(*dist,x);
    }
    mooseError("Distribution " + DistAlias + " was not found in distribution container.");
    return -1.0;
}

double
DistributionContainer::Cdf(std::string DistAlias, double x){

   if(_dist_by_name.find(DistAlias) != _dist_by_name.end()){
       distribution * dist = _dist_by_name.find(DistAlias)->second;
       return DistributionCdf(*dist,x);
    }
    mooseError("Distribution " + DistAlias + " was not found in distribution container.");
    return -1.0;

}

double
DistributionContainer::randGen(char * DistAlias, double RNG){
  return randGen(std::string(DistAlias), RNG);
}

double
DistributionContainer::randGen(std::string DistAlias, double RNG){

    if(_dist_by_name.find(DistAlias) != _dist_by_name.end()){
        distribution * dist = _dist_by_name.find(DistAlias)->second;
        return dist->RandomNumberGenerator(RNG);
     }
     mooseError("Distribution " + DistAlias + " was not found in distribution container.");
     return -1.0;

}

//int DistributionContainer::getPosition(std::string DistAlias){
//	std::map <std::string, int>::iterator p;
//
//	p = _vector_pos_map.find(DistAlias);
//	if(p != _vector_pos_map.end()){
//       return (p->second);
//	}
//	else{
//		return -1;
//	}
//}

DistributionContainer & DistributionContainer::Instance() {
  if(_instance == NULL){
    _instance = new DistributionContainer();
  }
  return *_instance;
}

DistributionContainer * DistributionContainer::_instance;

/* the str_to_string_p and free_string_p are for python use */

std::string * str_to_string_p(char *s) {
  return new std::string(s);
}

void free_string_p(std::string * s) {
  delete s;
}

<commit_msg>random from 0 to 1<commit_after>/*
 * DistributionContainer.C
 *
 *  Created on: Jul 6, 2012
 *      Author: alfoa
 */
#include "DistributionContainer.h"
//#include "Factory.h"
//#include "distribution_1D.h"
#include "distribution_min.h"
#include <iostream>
#include <math.h>
#include <cmath>
#include <cstdlib>
#include <stdlib.h>
#include <vector>
#include <map>
using namespace std;

#ifndef mooseError
#define mooseError(msg) { std::cerr << "\n\n" << msg << "\n\n"; }
#endif

class DistributionContainer;

DistributionContainer::DistributionContainer()
{
}
DistributionContainer::~DistributionContainer()
{
}

/*void
DistributionContainer::addDistributionInContainer(const std::string & type, const std::string & name, InputParameters params){
   // create the distribution type
   distribution * dist = dynamic_cast<distribution *>(Factory::instance()->create(type, name, params));
   if (_dist_by_name.find(name) == _dist_by_name.end())
    _dist_by_name[name] = dist;
   else
     mooseError("Distribution with name " << name << " already exists");

   _dist_by_type[type].push_back(dist);

   }*/

void
DistributionContainer::addDistributionInContainer(const std::string & type, const std::string & name, distribution * dist){
   // create the distribution type
  //distribution * dist = dynamic_cast<distribution *>(Factory::instance()->create(type, name, params));
   if (_dist_by_name.find(name) == _dist_by_name.end())
    _dist_by_name[name] = dist;
   else
     mooseError("Distribution with name " << name << " already exists");

   _dist_by_type[type].push_back(dist);

}

//void
//DistributionContainer::constructDistributionContainer(std::string DistAlias, distribution_type type, double xmin, double xmax, double param1, double param2, unsigned int seed){
//	_distribution_cont.push_back(distribution(type, xmin, xmax, param1, param2, seed));
//	_vector_pos_map[DistAlias]=_distribution_cont.size()-1;
//}
//void
//DistributionContainer::constructDistributionContainerCustom(std::string DistAlias, distribution_type type, std::vector< double > dist_x, std::vector< double > dist_y, int numPoints, custom_dist_fit_type fit_type, unsigned int seed){
//
//	_distribution_cont.push_back(distribution(dist_x, dist_y, numPoints, fit_type, seed));
//	_vector_pos_map[DistAlias]=_distribution_cont.size()-1;
//}

std::string
DistributionContainer::getType(std::string DistAlias){

    if(_dist_by_name.find(DistAlias) != _dist_by_name.end()){
       distribution * dist = _dist_by_name.find(DistAlias)->second;
       std::string type = getDistributionType(*dist);
       if(type == "DistributionError"){
         mooseError("Type for distribution " << DistAlias << " not found");
       }
       return type;
    }
    else{
       mooseError("Distribution " << DistAlias << " not found in distribution container");
       return "DistributionError";
    }

}

void
DistributionContainer::seedRandom(unsigned int seed){
   srand( seed );
}
double
DistributionContainer::random(){
   return (static_cast<double>(rand())/
           static_cast<double>(RAND_MAX));
//   return -1.0;
}

// to be implemented
bool DistributionContainer::checkCdf(double probability, std::vector<double> values){
   return false;
}
// end to be implemented
// to be implemented
bool DistributionContainer::checkCdf(double probability, double value){
   return false;
}
// end to be implemented

double
DistributionContainer::getVariable(std::string paramName,std::string DistAlias){
    if(_dist_by_name.find(DistAlias) != _dist_by_name.end()){
       distribution * dist = _dist_by_name.find(DistAlias)->second;
       return getDistributionVariable(*dist,paramName);
    }
    mooseError("Distribution " << DistAlias << " not found in distribution container");
    return -1;
}

void
DistributionContainer::updateVariable(std::string paramName,double newValue,std::string DistAlias){
    if(_dist_by_name.find(DistAlias) != _dist_by_name.end()){
       distribution * dist = _dist_by_name.find(DistAlias)->second;
       DistributionUpdateVariable(*dist,paramName,newValue);
    }
    mooseError("Distribution " + DistAlias + " was not found in distribution container.");
}

//double
//DistributionContainer::getMin(std::string DistAlias){
//    int position;
//
//    position = getPosition(DistAlias);
//
//    if(position != -1){
//    	return _distribution_cont[position].getMin();
//    }
//    else{
//    	std::cerr << " ERROR: distribution " << DistAlias << "not present in the mapping";
//    	return DISTRIBUTION_ERROR;
//    }
//
//}
//
//double
//DistributionContainer::getMax(std::string DistAlias){
//    int position;
//
//    position = getPosition(DistAlias);
//
//    if(position != -1){
//    	return _distribution_cont[position].getMax();
//    }
//    else{
//    	std::cerr << " ERROR: distribution " << DistAlias << "not present in the mapping";
//    	return DISTRIBUTION_ERROR;
//    }
//
//}
//
//double
//DistributionContainer::getParamater1(std::string DistAlias){
//    int position;
//
//    position = getPosition(DistAlias);
//
//    if(position != -1){
//    	return _distribution_cont[position].getParamater1();
//    }
//    else{
//    	std::cerr << " ERROR: distribution " << DistAlias << "not present in the mapping";
//    	return DISTRIBUTION_ERROR;
//    }
//
//}
//
//double
//DistributionContainer::getParameter2(std::string DistAlias){
//    int position;
//
//    position = getPosition(DistAlias);
//
//    if(position != -1){
//    	return _distribution_cont[position].getParameter2();
//    }
//    else{
//    	std::cerr << " ERROR: distribution " << DistAlias << "not present in the mapping";
//    	return DISTRIBUTION_ERROR;
//    }
//
//}
//
//void
//DistributionContainer::changeParameter1(std::string DistAlias,double newParameter1){
//    int position;
//
//    position = getPosition(DistAlias);
//
//    if(position != -1){
//    	_distribution_cont[position].changeParameter1(newParameter1);
//    }
//    else{
//    	std::cerr << " ERROR: distribution " << DistAlias << "not present in the mapping";
//    }
//
//}
//
//void
//DistributionContainer::changeParameter2(std::string DistAlias,double newParameter2){
//    int position;
//
//    position = getPosition(DistAlias);
//
//    if(position != -1){
//    	_distribution_cont[position].changeParameter2(newParameter2);
//    }
//    else{
//    	std::cerr << " ERROR: distribution " << DistAlias << "not present in the mapping";
//    }
//
//}

double
DistributionContainer::Pdf(std::string DistAlias, double x){

    if(_dist_by_name.find(DistAlias) != _dist_by_name.end()){
       distribution * dist = _dist_by_name.find(DistAlias)->second;
       return DistributionPdf(*dist,x);
    }
    mooseError("Distribution " + DistAlias + " was not found in distribution container.");
    return -1.0;
}

double
DistributionContainer::Cdf(std::string DistAlias, double x){

   if(_dist_by_name.find(DistAlias) != _dist_by_name.end()){
       distribution * dist = _dist_by_name.find(DistAlias)->second;
       return DistributionCdf(*dist,x);
    }
    mooseError("Distribution " + DistAlias + " was not found in distribution container.");
    return -1.0;

}

double
DistributionContainer::randGen(char * DistAlias, double RNG){
  return randGen(std::string(DistAlias), RNG);
}

double
DistributionContainer::randGen(std::string DistAlias, double RNG){

    if(_dist_by_name.find(DistAlias) != _dist_by_name.end()){
        distribution * dist = _dist_by_name.find(DistAlias)->second;
        return dist->RandomNumberGenerator(RNG);
     }
     mooseError("Distribution " + DistAlias + " was not found in distribution container.");
     return -1.0;

}

//int DistributionContainer::getPosition(std::string DistAlias){
//	std::map <std::string, int>::iterator p;
//
//	p = _vector_pos_map.find(DistAlias);
//	if(p != _vector_pos_map.end()){
//       return (p->second);
//	}
//	else{
//		return -1;
//	}
//}

DistributionContainer & DistributionContainer::Instance() {
  if(_instance == NULL){
    _instance = new DistributionContainer();
  }
  return *_instance;
}

DistributionContainer * DistributionContainer::_instance;

/* the str_to_string_p and free_string_p are for python use */

std::string * str_to_string_p(char *s) {
  return new std::string(s);
}

void free_string_p(std::string * s) {
  delete s;
}

<|endoftext|>


<commit_before>/*=========================================================================

  Program:   ORFEO Toolbox
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

  Copyright (c) Centre National d'Etudes Spatiales. All rights reserved.
  See OTBCopyright.txt for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notices for more information.

=========================================================================*/
#if defined(_MSC_VER)
#pragma warning ( disable : 4786 )
#endif

//  Software Guide : BeginCommandLineArgs
//    INPUTS: {TO_core_last_zoom.las}
//    OUTPUTS: {lidar-image-4.hdr}, {lidar-image-4.png}
//    1.0 5 4
//  Software Guide : EndCommandLineArgs

//  Software Guide : BeginCommandLineArgs
//    INPUTS: {TO_core_last_zoom.las}
//    OUTPUTS: {lidar-image-8.hdr}, {lidar-image-8.png}
//    1.0 5 8
//  Software Guide : EndCommandLineArgs

// Software Guide : BeginLatex
//
// This example describes how to convert a point set obtained from some lidar data
// to an image file. Lidar produce a point set which is irregular in terms of spatial
// sampling. To be able to generate an image, an interpolation is required.
//
// The interpolation is done using the
// \doxygen{itk}{BSplineScatteredDataPointSetToImageFilter} which uses BSplines. The
// method is fully describes in \cite{Tustison2005} and \cite{Lee1997}.
//
// The first step toward the use of these filters is to include the proper header files.
//
// Software Guide : EndLatex

// Software Guide : BeginCodeSnippet

#include "itkPointSet.h"
#include "otbImage.h"
#include "otbPointSetFileReader.h"
#include "itkBSplineScatteredDataPointSetToImageFilter.h"
#include "otbImageFileWriter.h"

// Software Guide : EndCodeSnippet

#include "itkRescaleIntensityImageFilter.h"

int main( int argc, char* argv[] )
{

  if(argc!=7)
  {
    std::cout << argv[0] <<" <input_lidar_filename> <output_image_filename(double)>"
              << " <output_image_filename(unsigned char)>"
	      << " <output_resolution> <spline_order>"
              << " <number_of_level>"
	      << std::endl;
    return EXIT_FAILURE;
  }

  // Software Guide : BeginLatex
  //
  // Then, we declare the type of the data that we are going to use. As
  // lidar decribes the altitude of the point (often to centimeter accuracy),
  // it is required to use real type to represent it.
  //
  // Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  typedef double RealType;
  typedef itk::Vector<RealType, 1> VectorType;
  typedef itk::PointSet <VectorType, 2> PointSetType;
  typedef otb::Image<RealType, 2> ImageType;
  typedef itk::Image<VectorType, 2> VectorImageType;
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  //
  // Lidar data are read into a point set using the
  // \doxygen{otb}{PointSetFileReader}. Its usage is very similar to
  // the \doxygen{otb}{ImagePointSetFileReader}:
  //
  // Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  typedef otb::PointSetFileReader<PointSetType> PointSetFileReaderType;
  PointSetFileReaderType::Pointer reader = PointSetFileReaderType::New();

  reader->SetFileName(argv[1]);
  reader->Update();
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  //
  // We can now prepare the parameters to pass to the interpolation filter:
  // you have to be aware that the origin of the image is on the upper left
  // corner and thus correspond to the minimun easting but the maximum northing.
  //
  // Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  double resolution = atof(argv[4]);
  int splineOrder = atoi(argv[5]);
  int level = atoi(argv[6]);
  // Software Guide : EndCodeSnippet

  std::cout << "*** Data area *** " << std::endl;
  std::cout << std::setprecision(15);
  std::cout << " - Easting min:  " << reader->GetMinX() << std::endl;
  std::cout << " - Easting max:  " << reader->GetMaxX() << std::endl;
  std::cout << " - Northing min: " << reader->GetMinY() << std::endl;
  std::cout << " - Northing max: " << reader->GetMaxY() << std::endl;
  std::cout << "Data points: " << reader->GetNumberOfPoints() << std::endl;

  // Software Guide : BeginCodeSnippet
  ImageType::IndexType start;
  start[0] =  0;
  start[1] =  0;

  ImageType::SizeType  size;
  size[0]  = static_cast<long int >(
         ceil((reader->GetMaxX()-reader->GetMinX()+1)/resolution))+1;
  size[1]  = static_cast<long int >(
         ceil((reader->GetMaxY()-reader->GetMinY()+1)/resolution))+1;

  ImageType::PointType origin;
  origin[0] = reader->GetMinX();
  origin[1] = reader->GetMaxY();

  ImageType::SpacingType spacing;
  spacing[0] = resolution;
  spacing[1] = -resolution;
  // Software Guide : EndCodeSnippet

//   std::cout << "Size: " << size << std::endl;
//   std::cout << "Origin: " << origin << std::endl;
//   std::cout << "Spacing: " << spacing << std::endl;

  // Software Guide : BeginLatex
  //
  // All these parameters are passed to the interpolation filter:
  //
  // Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
   typedef itk::BSplineScatteredDataPointSetToImageFilter
      <PointSetType, VectorImageType> FilterType;

  FilterType::Pointer filter = FilterType::New();

  filter->SetSplineOrder( splineOrder );
  FilterType::ArrayType ncps;
  ncps.Fill( 6 );
  filter->SetNumberOfControlPoints( ncps );
  filter->SetNumberOfLevels( level );

  filter->SetOrigin( origin );
  filter->SetSpacing( spacing );
  filter->SetSize( size );

  filter->SetInput( reader->GetOutput() );

  filter->Update();
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  //
  // The result of this filter is an image in which every pixel
  // is a vector (with only one element). For now, the otb writer
  // does not know how to process that (hopefully soon!). So we
  // have to manually copy the element in a standard image:
  //
  // Software Guide : EndLatex

  //TODO ImageOfVectorsToVectorImage needed !
    // Write the output to an image.

   // Software Guide : BeginCodeSnippet
  typedef otb::Image<RealType, 2> RealImageType;
  RealImageType::Pointer image = RealImageType::New();
  ImageType::RegionType region;
  region.SetSize( size );
  region.SetIndex( start );
  image->SetRegions( region );
  image->Allocate();
  itk::ImageRegionIteratorWithIndex<RealImageType>
      Itt( image, image->GetLargestPossibleRegion() );

  for ( Itt.GoToBegin(); !Itt.IsAtEnd(); ++Itt )
  {
    Itt.Set( filter->GetOutput()->GetPixel( Itt.GetIndex() )[0] );
  }
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  //
  // Everything is ready so we can just write the image:
  //
  // Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  typedef otb::ImageFileWriter<ImageType> WriterType;
  WriterType::Pointer writer = WriterType::New();
  writer->SetInput( image );
  writer->SetFileName( argv[2] );
  writer->Update();
  // Software Guide : EndCodeSnippet

  typedef otb::Image<unsigned char, 2> UCharImageType;
  typedef itk::RescaleIntensityImageFilter<ImageType,
               UCharImageType> RescalerType;
  RescalerType::Pointer rescaler = RescalerType::New();
  rescaler->SetInput(image);
  rescaler->SetOutputMaximum(255);
  rescaler->SetOutputMinimum(0);

  typedef otb::ImageFileWriter<UCharImageType> WriterUCharType;
  WriterUCharType::Pointer writerUChar = WriterUCharType::New();
  writerUChar->SetInput(rescaler->GetOutput());
  writerUChar->SetFileName(argv[3]);
  writerUChar->Update();

    // Software Guide : BeginLatex
  //
  // Figure~\ref{fig:LIDARTOIMAGEEXAMPLE} shows the output images with two sets of parameters
  //
  //
  // \begin{figure}
  // \center
  // \includegraphics[width=0.40\textwidth]{lidar-image-4.png}
  // \includegraphics[width=0.40\textwidth]{lidar-image-8.png}
  // \itkcaption[Image from lidar data]{Image obtained with 4 level spline interpolation (left) and 8 levels (right)}
  // \label{fig:LIDARTOIMAGEEXAMPLE}
  // \end{figure}
  //
  // Software Guide : EndLatex

  return EXIT_SUCCESS;

}

<commit_msg>DOC: adding Lidar example<commit_after>/*=========================================================================

  Program:   ORFEO Toolbox
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

  Copyright (c) Centre National d'Etudes Spatiales. All rights reserved.
  See OTBCopyright.txt for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notices for more information.

=========================================================================*/
#if defined(_MSC_VER)
#pragma warning ( disable : 4786 )
#endif

//  Software Guide : BeginCommandLineArgs
//    INPUTS: {TO_core_last_zoom.las}
//    OUTPUTS: {lidar-image-4.hdr}, {lidar-image-4-pretty.png}
//    1.0 5 4
//  Software Guide : EndCommandLineArgs

//  Software Guide : BeginCommandLineArgs
//    INPUTS: {TO_core_last_zoom.las}
//    OUTPUTS: {lidar-image-8.hdr}, {lidar-image-8-pretty.png}
//    1.0 5 8
//  Software Guide : EndCommandLineArgs

// Software Guide : BeginLatex
//
// This example describes how to convert a point set obtained from some lidar data
// to an image file. Lidar produce a point set which is irregular in terms of spatial
// sampling. To be able to generate an image, an interpolation is required.
//
// The interpolation is done using the
// \doxygen{itk}{BSplineScatteredDataPointSetToImageFilter} which uses BSplines. The
// method is fully describes in \cite{Tustison2005} and \cite{Lee1997}.
//
// The first step toward the use of these filters is to include the proper header files.
//
// Software Guide : EndLatex

// Software Guide : BeginCodeSnippet

#include "itkPointSet.h"
#include "otbImage.h"
#include "otbPointSetFileReader.h"
#include "itkBSplineScatteredDataPointSetToImageFilter.h"
#include "otbImageFileWriter.h"

// Software Guide : EndCodeSnippet

#include "itkRescaleIntensityImageFilter.h"

int main( int argc, char* argv[] )
{

  if(argc!=7)
  {
    std::cout << argv[0] <<" <input_lidar_filename> <output_image_filename(double)>"
              << " <output_image_filename(unsigned char)>"
	      << " <output_resolution> <spline_order>"
              << " <number_of_level>"
	      << std::endl;
    return EXIT_FAILURE;
  }

  // Software Guide : BeginLatex
  //
  // Then, we declare the type of the data that we are going to use. As
  // lidar decribes the altitude of the point (often to centimeter accuracy),
  // it is required to use real type to represent it.
  //
  // Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  typedef double RealType;
  typedef itk::Vector<RealType, 1> VectorType;
  typedef itk::PointSet <VectorType, 2> PointSetType;
  typedef otb::Image<RealType, 2> ImageType;
  typedef itk::Image<VectorType, 2> VectorImageType;
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  //
  // Lidar data are read into a point set using the
  // \doxygen{otb}{PointSetFileReader}. Its usage is very similar to
  // the \doxygen{otb}{ImagePointSetFileReader}:
  //
  // Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  typedef otb::PointSetFileReader<PointSetType> PointSetFileReaderType;
  PointSetFileReaderType::Pointer reader = PointSetFileReaderType::New();

  reader->SetFileName(argv[1]);
  reader->Update();
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  //
  // We can now prepare the parameters to pass to the interpolation filter:
  // you have to be aware that the origin of the image is on the upper left
  // corner and thus correspond to the minimun easting but the maximum northing.
  //
  // Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  double resolution = atof(argv[4]);
  int splineOrder = atoi(argv[5]);
  int level = atoi(argv[6]);
  // Software Guide : EndCodeSnippet

  std::cout << "*** Data area *** " << std::endl;
  std::cout << std::setprecision(15);
  std::cout << " - Easting min:  " << reader->GetMinX() << std::endl;
  std::cout << " - Easting max:  " << reader->GetMaxX() << std::endl;
  std::cout << " - Northing min: " << reader->GetMinY() << std::endl;
  std::cout << " - Northing max: " << reader->GetMaxY() << std::endl;
  std::cout << "Data points: " << reader->GetNumberOfPoints() << std::endl;

  // Software Guide : BeginCodeSnippet
  ImageType::IndexType start;
  start[0] =  0;
  start[1] =  0;

  ImageType::SizeType  size;
  size[0]  = static_cast<long int >(
         ceil((reader->GetMaxX()-reader->GetMinX()+1)/resolution))+1;
  size[1]  = static_cast<long int >(
         ceil((reader->GetMaxY()-reader->GetMinY()+1)/resolution))+1;

  ImageType::PointType origin;
  origin[0] = reader->GetMinX();
  origin[1] = reader->GetMaxY();

  ImageType::SpacingType spacing;
  spacing[0] = resolution;
  spacing[1] = -resolution;
  // Software Guide : EndCodeSnippet

//   std::cout << "Size: " << size << std::endl;
//   std::cout << "Origin: " << origin << std::endl;
//   std::cout << "Spacing: " << spacing << std::endl;

  // Software Guide : BeginLatex
  //
  // All these parameters are passed to the interpolation filter:
  //
  // Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
   typedef itk::BSplineScatteredDataPointSetToImageFilter
      <PointSetType, VectorImageType> FilterType;

  FilterType::Pointer filter = FilterType::New();

  filter->SetSplineOrder( splineOrder );
  FilterType::ArrayType ncps;
  ncps.Fill( 6 );
  filter->SetNumberOfControlPoints( ncps );
  filter->SetNumberOfLevels( level );

  filter->SetOrigin( origin );
  filter->SetSpacing( spacing );
  filter->SetSize( size );

  filter->SetInput( reader->GetOutput() );

  filter->Update();
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  //
  // The result of this filter is an image in which every pixel
  // is a vector (with only one element). For now, the otb writer
  // does not know how to process that (hopefully soon!). So we
  // have to manually copy the element in a standard image:
  //
  // Software Guide : EndLatex

  //TODO ImageOfVectorsToVectorImage needed !
    // Write the output to an image.

   // Software Guide : BeginCodeSnippet
  typedef otb::Image<RealType, 2> RealImageType;
  RealImageType::Pointer image = RealImageType::New();
  ImageType::RegionType region;
  region.SetSize( size );
  region.SetIndex( start );
  image->SetRegions( region );
  image->Allocate();
  itk::ImageRegionIteratorWithIndex<RealImageType>
      Itt( image, image->GetLargestPossibleRegion() );

  for ( Itt.GoToBegin(); !Itt.IsAtEnd(); ++Itt )
  {
    Itt.Set( filter->GetOutput()->GetPixel( Itt.GetIndex() )[0] );
  }
  // Software Guide : EndCodeSnippet

  // Software Guide : BeginLatex
  //
  // Everything is ready so we can just write the image:
  //
  // Software Guide : EndLatex

  // Software Guide : BeginCodeSnippet
  typedef otb::ImageFileWriter<ImageType> WriterType;
  WriterType::Pointer writer = WriterType::New();
  writer->SetInput( image );
  writer->SetFileName( argv[2] );
  writer->Update();
  // Software Guide : EndCodeSnippet

  typedef otb::Image<unsigned char, 2> UCharImageType;
  typedef itk::RescaleIntensityImageFilter<ImageType,
               UCharImageType> RescalerType;
  RescalerType::Pointer rescaler = RescalerType::New();
  rescaler->SetInput(image);
  rescaler->SetOutputMaximum(255);
  rescaler->SetOutputMinimum(0);

  typedef otb::ImageFileWriter<UCharImageType> WriterUCharType;
  WriterUCharType::Pointer writerUChar = WriterUCharType::New();
  writerUChar->SetInput(rescaler->GetOutput());
  writerUChar->SetFileName(argv[3]);
  writerUChar->Update();

    // Software Guide : BeginLatex
  //
  // Figure~\ref{fig:LIDARTOIMAGEEXAMPLE} shows the output images with two sets of parameters
  //
  //
  // \begin{figure}
  // \center
  // \includegraphics[width=0.40\textwidth]{lidar-image-4-pretty.eps}
  // \includegraphics[width=0.40\textwidth]{lidar-image-8-pretty.eps}
  // \itkcaption[Image from lidar data]{Image obtained with 4 level spline interpolation (left) and 8 levels (right)}
  // \label{fig:LIDARTOIMAGEEXAMPLE}
  // \end{figure}
  //
  // Software Guide : EndLatex

  return EXIT_SUCCESS;

}

<|endoftext|>


<commit_before>/****************************************************************************
 *
 *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. 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.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file ets_airspeed.cpp
 * @author Simon Wilks
 *
 * Driver for the Eagle Tree Airspeed V3 connected via I2C.
 */

#include <px4_config.h>

#include <drivers/device/i2c.h>

#include <sys/types.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>

#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>

#include <board_config.h>

#include <systemlib/airspeed.h>
#include <systemlib/err.h>
#include <systemlib/param/param.h>
#include <systemlib/perf_counter.h>

#include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/ringbuffer.h>

#include <uORB/uORB.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/subsystem_info.h>
#include <drivers/airspeed/airspeed.h>

/* I2C bus address */
#define I2C_ADDRESS	0x75	/* 7-bit address. 8-bit address is 0xEA */
#define ETS_PATH	"/dev/ets_airspeed"

/* Register address */
#define READ_CMD	0x07	/* Read the data */

/**
 * The Eagle Tree Airspeed V3 cannot provide accurate reading below speeds of 15km/h.
 * You can set this value to 12 if you want a zero reading below 15km/h.
 */
#define MIN_ACCURATE_DIFF_PRES_PA 0

/* Measurement rate is 100Hz */
#define CONVERSION_INTERVAL	(1000000 / 100)	/* microseconds */

class ETSAirspeed : public Airspeed
{
public:
	ETSAirspeed(int bus, int address = I2C_ADDRESS, const char *path = ETS_PATH);

protected:

	/**
	* Perform a poll cycle; collect from the previous measurement
	* and start a new one.
	*/
	virtual void	cycle();
	virtual int	measure();
	virtual int	collect();

};

/*
 * Driver 'main' command.
 */
extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]);

ETSAirspeed::ETSAirspeed(int bus, int address, const char *path) : Airspeed(bus, address,
			CONVERSION_INTERVAL, path)
{

}

int
ETSAirspeed::measure()
{
	int ret;

	/*
	 * Send the command to begin a measurement.
	 */
	uint8_t cmd = READ_CMD;
	ret = transfer(&cmd, 1, nullptr, 0);

	if (OK != ret) {
		perf_count(_comms_errors);
	}

	return ret;
}

int
ETSAirspeed::collect()
{
	int	ret = -EIO;

	/* read from the sensor */
	uint8_t val[2] = {0, 0};

	perf_begin(_sample_perf);

	ret = transfer(nullptr, 0, &val[0], 2);

	if (ret < 0) {
		perf_count(_comms_errors);
		return ret;
	}

	uint16_t diff_pres_pa_raw = val[1] << 8 | val[0];

	if (diff_pres_pa_raw == 0) {
		// a zero value means the pressure sensor cannot give us a
		// value. We need to return, and not report a value or the
		// caller could end up using this value as part of an
		// average
		perf_count(_comms_errors);
		DEVICE_LOG("zero value from sensor");
		return -1;
	}

	// The raw value still should be compensated for the known offset
	diff_pres_pa_raw -= _diff_pres_offset;

	// Track maximum differential pressure measured (so we can work out top speed).
	if (diff_pres_pa_raw > _max_differential_pressure_pa) {
		_max_differential_pressure_pa = diff_pres_pa_raw;
	}

	differential_pressure_s report;
	report.timestamp = hrt_absolute_time();
	report.error_count = perf_event_count(_comms_errors);

	// XXX we may want to smooth out the readings to remove noise.
	report.differential_pressure_filtered_pa = diff_pres_pa_raw;
	report.differential_pressure_raw_pa = diff_pres_pa_raw;
	report.temperature = -1000.0f;
	report.max_differential_pressure_pa = _max_differential_pressure_pa;

	if (_airspeed_pub != nullptr && !(_pub_blocked)) {
		/* publish it */
		orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
	}

	new_report(report);

	/* notify anyone waiting for data */
	poll_notify(POLLIN);

	ret = OK;

	perf_end(_sample_perf);

	return ret;
}

void
ETSAirspeed::cycle()
{
	int ret;

	/* collection phase? */
	if (_collect_phase) {

		/* perform collection */
		ret = collect();

		if (OK != ret) {
			perf_count(_comms_errors);
			/* restart the measurement state machine */
			start();
			_sensor_ok = false;
			return;
		}

		/* next phase is measurement */
		_collect_phase = false;

		/*
		 * Is there a collect->measure gap?
		 */
		if (_measure_ticks > USEC2TICK(CONVERSION_INTERVAL)) {

			/* schedule a fresh cycle call when we are ready to measure again */
			work_queue(HPWORK,
				   &_work,
				   (worker_t)&Airspeed::cycle_trampoline,
				   this,
				   _measure_ticks - USEC2TICK(CONVERSION_INTERVAL));

			return;
		}
	}

	/* measurement phase */
	ret = measure();

	if (OK != ret) {
		DEVICE_DEBUG("measure error");
	}

	_sensor_ok = (ret == OK);

	/* next phase is collection */
	_collect_phase = true;

	/* schedule a fresh cycle call when the measurement is done */
	work_queue(HPWORK,
		   &_work,
		   (worker_t)&Airspeed::cycle_trampoline,
		   this,
		   USEC2TICK(CONVERSION_INTERVAL));
}

/**
 * Local functions in support of the shell command.
 */
namespace ets_airspeed
{

ETSAirspeed	*g_dev;

void	start(int i2c_bus);
void	stop();
void	test();
void	reset();
void	info();

/**
 * Start the driver.
 *
 * This function only returns if the sensor is up and running
 * or could not be detected successfully.
 */
void
start(int i2c_bus)
{
	int fd;

	if (g_dev != nullptr) {
		errx(1, "already started");
	}

	/* create the driver */
	g_dev = new ETSAirspeed(i2c_bus);

	if (g_dev == nullptr) {
		goto fail;
	}

	if (OK != g_dev->Airspeed::init()) {
		goto fail;
	}

	/* set the poll rate to default, starts automatic data collection */
	fd = open(AIRSPEED0_DEVICE_PATH, O_RDONLY);

	if (fd < 0) {
		goto fail;
	}

	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
		goto fail;
	}

	exit(0);

fail:

	if (g_dev != nullptr) {
		delete g_dev;
		g_dev = nullptr;
	}

	errx(1, "no ETS airspeed sensor connected");
}

/**
 * Stop the driver
 */
void
stop()
{
	if (g_dev != nullptr) {
		delete g_dev;
		g_dev = nullptr;

	} else {
		errx(1, "driver not running");
	}

	exit(0);
}

/**
 * Perform some basic functional tests on the driver;
 * make sure we can collect data from the sensor in polled
 * and automatic modes.
 */
void
test()
{
	struct differential_pressure_s report;
	ssize_t sz;
	int ret;

	int fd = open(ETS_PATH, O_RDONLY);

	if (fd < 0) {
		err(1, "%s open failed (try 'ets_airspeed start' if the driver is not running", ETS_PATH);
	}

	/* do a simple demand read */
	sz = read(fd, &report, sizeof(report));

	if (sz != sizeof(report)) {
		err(1, "immediate read failed");
	}

	warnx("single read");
	warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);

	/* start the sensor polling at 2Hz */
	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
		errx(1, "failed to set 2Hz poll rate");
	}

	/* read the sensor 5x and report each value */
	for (unsigned i = 0; i < 5; i++) {
		struct pollfd fds;

		/* wait for data to be ready */
		fds.fd = fd;
		fds.events = POLLIN;
		ret = poll(&fds, 1, 2000);

		if (ret != 1) {
			errx(1, "timed out waiting for sensor data");
		}

		/* now go get it */
		sz = read(fd, &report, sizeof(report));

		if (sz != sizeof(report)) {
			err(1, "periodic read failed");
		}

		warnx("periodic read %u", i);
		warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);
	}

	/* reset the sensor polling to its default rate */
	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
		errx(1, "failed to set default rate");
	}

	errx(0, "PASS");
}

/**
 * Reset the driver.
 */
void
reset()
{
	int fd = open(ETS_PATH, O_RDONLY);

	if (fd < 0) {
		err(1, "failed ");
	}

	if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
		err(1, "driver reset failed");
	}

	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
		err(1, "driver poll restart failed");
	}

	exit(0);
}

/**
 * Print a little info about the driver.
 */
void
info()
{
	if (g_dev == nullptr) {
		errx(1, "driver not running");
	}

	printf("state @ %p\n", g_dev);
	g_dev->print_info();

	exit(0);
}

} // namespace

static void
ets_airspeed_usage()
{
	warnx("usage: ets_airspeed command [options]");
	warnx("options:");
	warnx("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT);
	warnx("command:");
	warnx("\tstart|stop|reset|test|info");
}

int
ets_airspeed_main(int argc, char *argv[])
{
	int i2c_bus = PX4_I2C_BUS_DEFAULT;

	int i;

	for (i = 1; i < argc; i++) {
		if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) {
			if (argc > i + 1) {
				i2c_bus = atoi(argv[i + 1]);
			}
		}
	}

	/*
	 * Start/load the driver.
	 */
	if (!strcmp(argv[1], "start")) {
		ets_airspeed::start(i2c_bus);
	}

	/*
	 * Stop the driver
	 */
	if (!strcmp(argv[1], "stop")) {
		ets_airspeed::stop();
	}

	/*
	 * Test the driver/device.
	 */
	if (!strcmp(argv[1], "test")) {
		ets_airspeed::test();
	}

	/*
	 * Reset the driver.
	 */
	if (!strcmp(argv[1], "reset")) {
		ets_airspeed::reset();
	}

	/*
	 * Print driver information.
	 */
	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
		ets_airspeed::info();
	}

	ets_airspeed_usage();
	exit(0);
}
<commit_msg>ETS airspeed: Work around weird choice of manufacturer zero level cutoff<commit_after>/****************************************************************************
 *
 *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. 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.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file ets_airspeed.cpp
 * @author Simon Wilks
 *
 * Driver for the Eagle Tree Airspeed V3 connected via I2C.
 */

#include <px4_config.h>

#include <drivers/device/i2c.h>

#include <sys/types.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>

#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <nuttx/clock.h>

#include <board_config.h>

#include <systemlib/airspeed.h>
#include <systemlib/err.h>
#include <systemlib/param/param.h>
#include <systemlib/perf_counter.h>

#include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/ringbuffer.h>

#include <uORB/uORB.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/subsystem_info.h>
#include <drivers/airspeed/airspeed.h>

/* I2C bus address */
#define I2C_ADDRESS	0x75	/* 7-bit address. 8-bit address is 0xEA */
#define ETS_PATH	"/dev/ets_airspeed"

/* Register address */
#define READ_CMD	0x07	/* Read the data */

/**
 * The Eagle Tree Airspeed V3 cannot provide accurate reading below speeds of 15km/h.
 * You can set this value to 12 if you want a zero reading below 15km/h.
 */
#define MIN_ACCURATE_DIFF_PRES_PA 0

/* Measurement rate is 100Hz */
#define CONVERSION_INTERVAL	(1000000 / 100)	/* microseconds */

class ETSAirspeed : public Airspeed
{
public:
	ETSAirspeed(int bus, int address = I2C_ADDRESS, const char *path = ETS_PATH);

protected:

	/**
	* Perform a poll cycle; collect from the previous measurement
	* and start a new one.
	*/
	virtual void	cycle();
	virtual int	measure();
	virtual int	collect();

};

/*
 * Driver 'main' command.
 */
extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]);

ETSAirspeed::ETSAirspeed(int bus, int address, const char *path) : Airspeed(bus, address,
			CONVERSION_INTERVAL, path)
{

}

int
ETSAirspeed::measure()
{
	int ret;

	/*
	 * Send the command to begin a measurement.
	 */
	uint8_t cmd = READ_CMD;
	ret = transfer(&cmd, 1, nullptr, 0);

	if (OK != ret) {
		perf_count(_comms_errors);
	}

	return ret;
}

int
ETSAirspeed::collect()
{
	int	ret = -EIO;

	/* read from the sensor */
	uint8_t val[2] = {0, 0};

	perf_begin(_sample_perf);

	ret = transfer(nullptr, 0, &val[0], 2);

	if (ret < 0) {
		perf_count(_comms_errors);
		return ret;
	}

	uint16_t diff_pres_pa_raw = val[1] << 8 | val[0];

	differential_pressure_s report;
	report.timestamp = hrt_absolute_time();

	if (diff_pres_pa_raw == 0) {
		// a zero value indicates no measurement
		// since the noise floor has been arbitrarily killed
		// it defeats our stuck sensor detection - the best we
		// can do is to output some numerical noise to show
		// that we are still correctly sampling.
		diff_pres_pa_raw = 0.001f * (report.timestamp & 0x01);
	}

	// The raw value still should be compensated for the known offset
	diff_pres_pa_raw -= _diff_pres_offset;

	// Track maximum differential pressure measured (so we can work out top speed).
	if (diff_pres_pa_raw > _max_differential_pressure_pa) {
		_max_differential_pressure_pa = diff_pres_pa_raw;
	}

	report.error_count = perf_event_count(_comms_errors);

	// XXX we may want to smooth out the readings to remove noise.
	report.differential_pressure_filtered_pa = diff_pres_pa_raw;
	report.differential_pressure_raw_pa = diff_pres_pa_raw;
	report.temperature = -1000.0f;
	report.max_differential_pressure_pa = _max_differential_pressure_pa;

	if (_airspeed_pub != nullptr && !(_pub_blocked)) {
		/* publish it */
		orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
	}

	new_report(report);

	/* notify anyone waiting for data */
	poll_notify(POLLIN);

	ret = OK;

	perf_end(_sample_perf);

	return ret;
}

void
ETSAirspeed::cycle()
{
	int ret;

	/* collection phase? */
	if (_collect_phase) {

		/* perform collection */
		ret = collect();

		if (OK != ret) {
			perf_count(_comms_errors);
			/* restart the measurement state machine */
			start();
			_sensor_ok = false;
			return;
		}

		/* next phase is measurement */
		_collect_phase = false;

		/*
		 * Is there a collect->measure gap?
		 */
		if (_measure_ticks > USEC2TICK(CONVERSION_INTERVAL)) {

			/* schedule a fresh cycle call when we are ready to measure again */
			work_queue(HPWORK,
				   &_work,
				   (worker_t)&Airspeed::cycle_trampoline,
				   this,
				   _measure_ticks - USEC2TICK(CONVERSION_INTERVAL));

			return;
		}
	}

	/* measurement phase */
	ret = measure();

	if (OK != ret) {
		DEVICE_DEBUG("measure error");
	}

	_sensor_ok = (ret == OK);

	/* next phase is collection */
	_collect_phase = true;

	/* schedule a fresh cycle call when the measurement is done */
	work_queue(HPWORK,
		   &_work,
		   (worker_t)&Airspeed::cycle_trampoline,
		   this,
		   USEC2TICK(CONVERSION_INTERVAL));
}

/**
 * Local functions in support of the shell command.
 */
namespace ets_airspeed
{

ETSAirspeed	*g_dev;

void	start(int i2c_bus);
void	stop();
void	test();
void	reset();
void	info();

/**
 * Start the driver.
 *
 * This function only returns if the sensor is up and running
 * or could not be detected successfully.
 */
void
start(int i2c_bus)
{
	int fd;

	if (g_dev != nullptr) {
		errx(1, "already started");
	}

	/* create the driver */
	g_dev = new ETSAirspeed(i2c_bus);

	if (g_dev == nullptr) {
		goto fail;
	}

	if (OK != g_dev->Airspeed::init()) {
		goto fail;
	}

	/* set the poll rate to default, starts automatic data collection */
	fd = open(AIRSPEED0_DEVICE_PATH, O_RDONLY);

	if (fd < 0) {
		goto fail;
	}

	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
		goto fail;
	}

	exit(0);

fail:

	if (g_dev != nullptr) {
		delete g_dev;
		g_dev = nullptr;
	}

	errx(1, "no ETS airspeed sensor connected");
}

/**
 * Stop the driver
 */
void
stop()
{
	if (g_dev != nullptr) {
		delete g_dev;
		g_dev = nullptr;

	} else {
		errx(1, "driver not running");
	}

	exit(0);
}

/**
 * Perform some basic functional tests on the driver;
 * make sure we can collect data from the sensor in polled
 * and automatic modes.
 */
void
test()
{
	struct differential_pressure_s report;
	ssize_t sz;
	int ret;

	int fd = open(ETS_PATH, O_RDONLY);

	if (fd < 0) {
		err(1, "%s open failed (try 'ets_airspeed start' if the driver is not running", ETS_PATH);
	}

	/* do a simple demand read */
	sz = read(fd, &report, sizeof(report));

	if (sz != sizeof(report)) {
		err(1, "immediate read failed");
	}

	warnx("single read");
	warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);

	/* start the sensor polling at 2Hz */
	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
		errx(1, "failed to set 2Hz poll rate");
	}

	/* read the sensor 5x and report each value */
	for (unsigned i = 0; i < 5; i++) {
		struct pollfd fds;

		/* wait for data to be ready */
		fds.fd = fd;
		fds.events = POLLIN;
		ret = poll(&fds, 1, 2000);

		if (ret != 1) {
			errx(1, "timed out waiting for sensor data");
		}

		/* now go get it */
		sz = read(fd, &report, sizeof(report));

		if (sz != sizeof(report)) {
			err(1, "periodic read failed");
		}

		warnx("periodic read %u", i);
		warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);
	}

	/* reset the sensor polling to its default rate */
	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
		errx(1, "failed to set default rate");
	}

	errx(0, "PASS");
}

/**
 * Reset the driver.
 */
void
reset()
{
	int fd = open(ETS_PATH, O_RDONLY);

	if (fd < 0) {
		err(1, "failed ");
	}

	if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
		err(1, "driver reset failed");
	}

	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
		err(1, "driver poll restart failed");
	}

	exit(0);
}

/**
 * Print a little info about the driver.
 */
void
info()
{
	if (g_dev == nullptr) {
		errx(1, "driver not running");
	}

	printf("state @ %p\n", g_dev);
	g_dev->print_info();

	exit(0);
}

} // namespace

static void
ets_airspeed_usage()
{
	warnx("usage: ets_airspeed command [options]");
	warnx("options:");
	warnx("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT);
	warnx("command:");
	warnx("\tstart|stop|reset|test|info");
}

int
ets_airspeed_main(int argc, char *argv[])
{
	int i2c_bus = PX4_I2C_BUS_DEFAULT;

	int i;

	for (i = 1; i < argc; i++) {
		if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) {
			if (argc > i + 1) {
				i2c_bus = atoi(argv[i + 1]);
			}
		}
	}

	/*
	 * Start/load the driver.
	 */
	if (!strcmp(argv[1], "start")) {
		ets_airspeed::start(i2c_bus);
	}

	/*
	 * Stop the driver
	 */
	if (!strcmp(argv[1], "stop")) {
		ets_airspeed::stop();
	}

	/*
	 * Test the driver/device.
	 */
	if (!strcmp(argv[1], "test")) {
		ets_airspeed::test();
	}

	/*
	 * Reset the driver.
	 */
	if (!strcmp(argv[1], "reset")) {
		ets_airspeed::reset();
	}

	/*
	 * Print driver information.
	 */
	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
		ets_airspeed::info();
	}

	ets_airspeed_usage();
	exit(0);
}
<|endoftext|>


<commit_before>#include <pcl/point_types.h>
#include <opencv2/core/core.hpp>

#ifndef MY_TEMPLATE_TYPES_H_
#define MY_TEMPLATE_TYPES_H_

template <typename PointT>
struct Projected_Point{
	Projected_Point(PointT pcl, cv::Point2i cv){
		this->pcl = pcl;
		this->cv = cv;
	}
	PointT pcl;
	cv::Point2i cv;

	std::string to_string(){
		std::stringstream ss;
			ss << "pcl: " << pcl << "cv: " << cv;
		return ss.str();
	}
};

struct Image_size{
	int width;
	int height;
};
template <typename PointT>
struct Projected_pointcloud{
	std::vector<Projected_Point<PointT> > points;
	Image_size image_size;

	std::string to_string(){
		std::stringstream ss;
		ss << "width: " << image_size.width << " height: " << image_size.height << "\n";
		for(int i = 0; i < points.size(); ++i){
			ss << "i: " << i << " " << points.at(i).to_string() << "\n";
		}
		return ss.str();
	}
};

#endif /* MY_TEMPLATE_TYPES_H_ */
<commit_msg>added list helpers to struct<commit_after>#include <pcl/point_types.h>
#include <opencv2/core/core.hpp>

#ifndef MY_TEMPLATE_TYPES_H_
#define MY_TEMPLATE_TYPES_H_

template <typename PointT>
struct Projected_Point{
	Projected_Point(PointT pcl, cv::Point2i cv){
		this->pcl = pcl;
		this->cv = cv;
	}
	PointT pcl;
	cv::Point2i cv;

	std::string to_string(){
		std::stringstream ss;
			ss << "pcl: " << pcl << "cv: " << cv;
		return ss.str();
	}
};

struct Image_size{
	int width;
	int height;
};

template <typename PointT>
struct Projected_pointcloud{
	std::vector<Projected_Point<PointT> > points;
	Image_size image_size;

	std::string to_string(){
		std::stringstream ss;
		ss << "width: " << image_size.width << " height: " << image_size.height << "\n";
		for(int i = 0; i < points.size(); ++i){
			ss << "i: " << i << " " << points.at(i).to_string() << "\n";
		}
		return ss.str();
	}

	size_t size(){
		return points.size();
	}

	Projected_Point<PointT> at(long unsigned int i){
		return points.at(i);
	}

	void get_pointcloud(pcl::PointCloud<PointT> &out){
		assert(out.points.empty());

		for(int i = 0; i < size(); ++i){
			out.push_back(points.at(i).pcl);
		}
	}

};

#endif /* MY_TEMPLATE_TYPES_H_ */
<|endoftext|>


<commit_before>// Licensed to the Apache Software Foundation (ASF) under one
// or more contributor license agreements.  See the NOTICE file
// distributed with this work for additional information
// regarding copyright ownership.  The ASF licenses this file
// to you 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.

#ifndef __EXECUTOR_PROTO_HPP__
#define __EXECUTOR_PROTO_HPP__

// NOTE: This header only becomes valid after running protoc
// and generating the equivalent .ph.h files
// See: src/messages/mesos.proto
#include <executor/executor.pb.h>

#endif // __EXECUTOR_PROTO_HPP__
<commit_msg>Added output operator for unversioned executor protobuf.<commit_after>// Licensed to the Apache Software Foundation (ASF) under one
// or more contributor license agreements.  See the NOTICE file
// distributed with this work for additional information
// regarding copyright ownership.  The ASF licenses this file
// to you 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.

#ifndef __EXECUTOR_PROTO_HPP__
#define __EXECUTOR_PROTO_HPP__

// NOTE: This header only becomes valid after running protoc
// and generating the equivalent .ph.h files
// See: src/messages/mesos.proto
#include <executor/executor.pb.h>

namespace mesos {
namespace executor {

inline std::ostream& operator<<(std::ostream& stream, const Call::Type& type)
{
  return stream << Call::Type_Name(type);
}

inline std::ostream& operator<<(std::ostream& stream, const Event::Type& type)
{
  return stream << Event::Type_Name(type);
}

} // namespace executor {
} // namespace mesos {

#endif // __EXECUTOR_PROTO_HPP__
<|endoftext|>