<commit_before>/*
 -----------------------------------------------------------------------------
 This source file is part of OGRE
 (Object-oriented Graphics Rendering Engine)
 For the latest info, see http://www.ogre3d.org/

 Copyright (c) 2000-2013 Torus Knot Software Ltd

 Permission is hereby granted, free of charge, to any person obtaining a copy
 of this software and associated documentation files (the "Software"), to deal
 in the Software without restriction, including without limitation the rights
 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 copies of the Software, and to permit persons to whom the Software is
 furnished to do so, subject to the following conditions:

 The above copyright notice and this permission notice shall be included in
 all copies or substantial portions of the Software.

 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 THE SOFTWARE.
 -----------------------------------------------------------------------------
 */

#include "OgreStableHeaders.h"
#include "OgreGLES2StateCacheManagerImp.h"
#include "OgreGLES2RenderSystem.h"
#include "OgreLogManager.h"
#include "OgreRoot.h"

namespace Ogre {

    GLES2StateCacheManagerImp::GLES2StateCacheManagerImp(void)
    {
        clearCache();
    }

    void GLES2StateCacheManagerImp::initializeCache()
    {
        OGRE_CHECK_GL_ERROR(glBlendEquation(GL_FUNC_ADD));

        OGRE_CHECK_GL_ERROR(glBlendFunc(GL_ONE, GL_ZERO));

        OGRE_CHECK_GL_ERROR(glCullFace(mCullFace));

        OGRE_CHECK_GL_ERROR(glDepthFunc(mDepthFunc));

        OGRE_CHECK_GL_ERROR(glDepthMask(mDepthMask));

        OGRE_CHECK_GL_ERROR(glStencilMask(mStencilMask));

        OGRE_CHECK_GL_ERROR(glClearDepthf(mClearDepth));

        OGRE_CHECK_GL_ERROR(glBindTexture(GL_TEXTURE_2D, 0));

        OGRE_CHECK_GL_ERROR(glBindBuffer(GL_ARRAY_BUFFER, 0));

        OGRE_CHECK_GL_ERROR(glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0));

        OGRE_CHECK_GL_ERROR(glBindFramebuffer(GL_FRAMEBUFFER, 0));

        OGRE_CHECK_GL_ERROR(glBindRenderbuffer(GL_RENDERBUFFER, 0));

        OGRE_CHECK_GL_ERROR(glActiveTexture(GL_TEXTURE0));

        OGRE_CHECK_GL_ERROR(glClearColor(mClearColour[0], mClearColour[1], mClearColour[2], mClearColour[3]));

        OGRE_CHECK_GL_ERROR(glColorMask(mColourMask[0], mColourMask[1], mColourMask[2], mColourMask[3]));
    }

    void GLES2StateCacheManagerImp::clearCache()
    {
        mDepthMask = GL_TRUE;
        mPolygonMode = GL_FILL;
        mBlendEquation = GL_FUNC_ADD;
        mCullFace = GL_BACK;
        mDepthFunc = GL_LESS;
        mStencilMask = 0xFFFFFFFF;
        mActiveTextureUnit = 0;
        mDiscardBuffers = 0;
        mClearDepth = 1.0f;
        mLastBoundTexID = 0;

        // Initialize our cache variables and also the GL so that the
        // stored values match the GL state
        mBlendFuncSource = GL_ONE;
        mBlendFuncDest = GL_ZERO;

        mClearColour.resize(4);
        mClearColour[0] = mClearColour[1] = mClearColour[2] = mClearColour[3] = 0.0f;

        mColourMask.resize(4);
        mColourMask[0] = mColourMask[1] = mColourMask[2] = mColourMask[3] = GL_TRUE;

        mEnableVector.reserve(25);
        mEnableVector.clear();
        mActiveBufferMap.clear();
        mTexUnitsMap.clear();

        mEnableVector.reserve(64);
        mEnabledVertexAttribs.clear();
    }

    GLES2StateCacheManagerImp::~GLES2StateCacheManagerImp(void)
    {
        mColourMask.clear();
        mClearColour.clear();
        mActiveBufferMap.clear();
        mEnableVector.clear();
        mTexUnitsMap.clear();
    }

    void GLES2StateCacheManagerImp::bindGLBuffer(GLenum target, GLuint buffer, GLenum attach, bool force)
    {
		bool update = false;
        BindBufferMap::iterator i = mActiveBufferMap.find(target);
        if (i == mActiveBufferMap.end())
        {
            // Haven't cached this state yet.  Insert it into the map
            mActiveBufferMap.insert(BindBufferMap::value_type(target, buffer));
			update = true;
        }
        else if((*i).second != buffer || force) // Update the cached value if needed
        {
			(*i).second = buffer;
			update = true;
        }

		// Update GL
		if(update)
		{
            if(target == GL_FRAMEBUFFER)
            {
                OGRE_CHECK_GL_ERROR(glBindFramebuffer(target, buffer));
            }
            else if(target == GL_RENDERBUFFER)
            {
                OGRE_CHECK_GL_ERROR(glBindRenderbuffer(target, buffer));
            }
            else
            {
                OGRE_CHECK_GL_ERROR(glBindBuffer(target, buffer));
            }
		}
    }

    void GLES2StateCacheManagerImp::deleteGLBuffer(GLenum target, GLuint buffer, GLenum attach, bool force)
    {
        // Buffer name 0 is reserved and we should never try to delete it
        if(buffer == 0)
            return;

        BindBufferMap::iterator i = mActiveBufferMap.find(target);

        if (i != mActiveBufferMap.end() && ((*i).second == buffer || force))
        {
			if(target == GL_FRAMEBUFFER)
            {
				OGRE_CHECK_GL_ERROR(glDeleteFramebuffers(1, &buffer));
            }
            else if(target == GL_RENDERBUFFER)
            {
				OGRE_CHECK_GL_ERROR(glDeleteRenderbuffers(1, &buffer));
            }
            else
            {
				OGRE_CHECK_GL_ERROR(glDeleteBuffers(1, &buffer));
            }

            // Currently bound buffer is being deleted, update the cached value to 0,
            // which it likely the buffer that will be bound by the driver.
            // An update will be forced next time we try to bind on this target.
            (*i).second = 0;
        }
    }

    void GLES2StateCacheManagerImp::invalidateStateForTexture(GLuint texture)
    {
        TexUnitsMap::iterator it = mTexUnitsMap.find(texture);
        mTexUnitsMap.erase(it);
    }

    // TODO: Store as high/low bits of a GLuint, use vector instead of map for TexParameteriMap
    void GLES2StateCacheManagerImp::setTexParameteri(GLenum target, GLenum pname, GLint param)
    {
        // Check if we have a map entry for this texture id. If not, create a blank one and insert it.
        TexUnitsMap::iterator it = mTexUnitsMap.find(mLastBoundTexID);
        if (it == mTexUnitsMap.end())
        {
            TextureUnitParams unit;
            mTexUnitsMap[mLastBoundTexID] = unit;

            // Update the iterator
            it = mTexUnitsMap.find(mLastBoundTexID);
        }

        // Get a local copy of the parameter map and search for this parameter
        TexParameteriMap &myMap = (*it).second.mTexParameteriMap;
        TexParameteriMap::iterator i = myMap.find(pname);

        if (i == myMap.end())
        {
            // Haven't cached this state yet.  Insert it into the map
            myMap.insert(TexParameteriMap::value_type(pname, param));

            // Update GL
            OGRE_CHECK_GL_ERROR(glTexParameteri(target, pname, param));
        }
        else
        {
            // Update the cached value if needed
            if((*i).second != param)
            {
                (*i).second = param;

                // Update GL
                OGRE_CHECK_GL_ERROR(glTexParameteri(target, pname, param));
            }
        }
    }

    void GLES2StateCacheManagerImp::setTexParameterf(GLenum target, GLenum pname, GLfloat param)
    {
        // Check if we have a map entry for this texture id. If not, create a blank one and insert it.
        TexUnitsMap::iterator it = mTexUnitsMap.find(mLastBoundTexID);
        if (it == mTexUnitsMap.end())
        {
            TextureUnitParams unit;
            mTexUnitsMap[mLastBoundTexID] = unit;

            // Update the iterator
            it = mTexUnitsMap.find(mLastBoundTexID);
        }

        // Get a local copy of the parameter map and search for this parameter
        TexParameterfMap &myMap = (*it).second.mTexParameterfMap;
        TexParameterfMap::iterator i = myMap.find(pname);

        if (i == myMap.end())
        {
            // Haven't cached this state yet.  Insert it into the map
            myMap.insert(TexParameterfMap::value_type(pname, param));

            // Update GL
            OGRE_CHECK_GL_ERROR(glTexParameterf(target, pname, param));
        }
        else
        {
            // Update the cached value if needed
            if((*i).second != param)
            {
                (*i).second = param;

                // Update GL
                OGRE_CHECK_GL_ERROR(glTexParameterf(target, pname, param));
            }
        }
    }

    void GLES2StateCacheManagerImp::getTexParameterfv(GLenum target, GLenum pname, GLfloat *params)
    {
        // Check if we have a map entry for this texture id.
        TexUnitsMap::iterator it = mTexUnitsMap.find(mLastBoundTexID);

        // Get a local copy of the parameter map and search for this parameter
        TexParameterfMap::iterator i = (*it).second.mTexParameterfMap.find(pname);

        params = &(*i).second;
    }

    void GLES2StateCacheManagerImp::bindGLTexture(GLenum target, GLuint texture)
    {
        mLastBoundTexID = texture;

        // Update GL
        OGRE_CHECK_GL_ERROR(glBindTexture(target, texture));
    }

    bool GLES2StateCacheManagerImp::activateGLTextureUnit(size_t unit)
	{
		if (mActiveTextureUnit != unit)
		{
			if (unit < dynamic_cast<GLES2RenderSystem*>(Root::getSingleton().getRenderSystem())->getCapabilities()->getNumTextureUnits())
			{
				OGRE_CHECK_GL_ERROR(glActiveTexture(GL_TEXTURE0 + unit));
				mActiveTextureUnit = unit;
				return true;
			}
			else if (!unit)
			{
				// always ok to use the first unit
				return true;
			}
			else
			{
				return false;
			}
		}
		else
		{
			return true;
		}
	}

    // TODO: Store as high/low bits of a GLuint
    void GLES2StateCacheManagerImp::setBlendFunc(GLenum source, GLenum dest)
    {
        if(mBlendFuncSource != source || mBlendFuncDest != dest)
        {
            mBlendFuncSource = source;
            mBlendFuncDest = dest;

            OGRE_CHECK_GL_ERROR(glBlendFunc(source, dest));
        }
    }

    void GLES2StateCacheManagerImp::setBlendEquation(GLenum eq)
    {
        if(mBlendEquation != eq)
        {
            mBlendEquation = eq;

            OGRE_CHECK_GL_ERROR(glBlendEquation(eq));
        }
    }

    void GLES2StateCacheManagerImp::setDepthMask(GLboolean mask)
    {
        if(mDepthMask != mask)
        {
            mDepthMask = mask;

            OGRE_CHECK_GL_ERROR(glDepthMask(mask));
        }
    }

    void GLES2StateCacheManagerImp::setDepthFunc(GLenum func)
    {
        if(mDepthFunc != func)
        {
            mDepthFunc = func;

            OGRE_CHECK_GL_ERROR(glDepthFunc(func));
        }
    }

    void GLES2StateCacheManagerImp::setClearDepth(GLclampf depth)
    {
        if(mClearDepth != depth)
        {
            mClearDepth = depth;

            OGRE_CHECK_GL_ERROR(glClearDepthf(depth));
        }
    }

    void GLES2StateCacheManagerImp::setClearColour(GLclampf red, GLclampf green, GLclampf blue, GLclampf alpha)
    {
        if((mClearColour[0] != red) ||
           (mClearColour[1] != green) ||
           (mClearColour[2] != blue) ||
           (mClearColour[3] != alpha))
        {
            mClearColour[0] = red;
            mClearColour[1] = green;
            mClearColour[2] = blue;
            mClearColour[3] = alpha;

            OGRE_CHECK_GL_ERROR(glClearColor(mClearColour[0], mClearColour[1], mClearColour[2], mClearColour[3]));
        }
    }

    void GLES2StateCacheManagerImp::setColourMask(GLboolean red, GLboolean green, GLboolean blue, GLboolean alpha)
    {
        if((mColourMask[0] != red) ||
           (mColourMask[1] != green) ||
           (mColourMask[2] != blue) ||
           (mColourMask[3] != alpha))
        {
            mColourMask[0] = red;
            mColourMask[1] = green;
            mColourMask[2] = blue;
            mColourMask[3] = alpha;

            OGRE_CHECK_GL_ERROR(glColorMask(mColourMask[0], mColourMask[1], mColourMask[2], mColourMask[3]));
        }
    }

    void GLES2StateCacheManagerImp::setStencilMask(GLuint mask)
    {
        if(mStencilMask != mask)
        {
            mStencilMask = mask;

            OGRE_CHECK_GL_ERROR(glStencilMask(mask));
        }
    }

    void GLES2StateCacheManagerImp::setEnabled(GLenum flag)
    {
        bool found = std::find(mEnableVector.begin(), mEnableVector.end(), flag) != mEnableVector.end();
        if(!found)
        {
            mEnableVector.push_back(flag);

            OGRE_CHECK_GL_ERROR(glEnable(flag));
        }
    }

    void GLES2StateCacheManagerImp::setDisabled(GLenum flag)
    {
        vector<GLenum>::iterator iter = std::find(mEnableVector.begin(), mEnableVector.end(), flag);
        if(iter != mEnableVector.end())
        {
            mEnableVector.erase(iter);

            OGRE_CHECK_GL_ERROR(glDisable(flag));
        }
    }

    void GLES2StateCacheManagerImp::setVertexAttribEnabled(GLuint attrib)
    {
        bool found = std::find(mEnabledVertexAttribs.begin(), mEnabledVertexAttribs.end(), attrib) != mEnabledVertexAttribs.end();
        if(!found)
        {
            mEnabledVertexAttribs.push_back(attrib);

            OGRE_CHECK_GL_ERROR(glEnableVertexAttribArray(attrib));
        }
    }

    void GLES2StateCacheManagerImp::setVertexAttribDisabled(GLuint attrib)
    {
        vector<GLuint>::iterator iter = std::find(mEnabledVertexAttribs.begin(), mEnabledVertexAttribs.end(), attrib);
        if(iter != mEnabledVertexAttribs.end())
        {
            mEnabledVertexAttribs.erase(iter);

            OGRE_CHECK_GL_ERROR(glDisableVertexAttribArray(attrib));
        }
    }

    void GLES2StateCacheManagerImp::setCullFace(GLenum face)
    {
        if(mCullFace != face)
        {
            mCullFace = face;

            OGRE_CHECK_GL_ERROR(glCullFace(face));
        }
    }
}
<commit_msg>[OGRE-300] Apply GL state cache crash fix to GLES2 state cache.<commit_after>/*
 -----------------------------------------------------------------------------
 This source file is part of OGRE
 (Object-oriented Graphics Rendering Engine)
 For the latest info, see http://www.ogre3d.org/

 Copyright (c) 2000-2013 Torus Knot Software Ltd

 Permission is hereby granted, free of charge, to any person obtaining a copy
 of this software and associated documentation files (the "Software"), to deal
 in the Software without restriction, including without limitation the rights
 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 copies of the Software, and to permit persons to whom the Software is
 furnished to do so, subject to the following conditions:

 The above copyright notice and this permission notice shall be included in
 all copies or substantial portions of the Software.

 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 THE SOFTWARE.
 -----------------------------------------------------------------------------
 */

#include "OgreStableHeaders.h"
#include "OgreGLES2StateCacheManagerImp.h"
#include "OgreGLES2RenderSystem.h"
#include "OgreLogManager.h"
#include "OgreRoot.h"

namespace Ogre {

    GLES2StateCacheManagerImp::GLES2StateCacheManagerImp(void)
    {
        clearCache();
    }

    void GLES2StateCacheManagerImp::initializeCache()
    {
        OGRE_CHECK_GL_ERROR(glBlendEquation(GL_FUNC_ADD));

        OGRE_CHECK_GL_ERROR(glBlendFunc(GL_ONE, GL_ZERO));

        OGRE_CHECK_GL_ERROR(glCullFace(mCullFace));

        OGRE_CHECK_GL_ERROR(glDepthFunc(mDepthFunc));

        OGRE_CHECK_GL_ERROR(glDepthMask(mDepthMask));

        OGRE_CHECK_GL_ERROR(glStencilMask(mStencilMask));

        OGRE_CHECK_GL_ERROR(glClearDepthf(mClearDepth));

        OGRE_CHECK_GL_ERROR(glBindTexture(GL_TEXTURE_2D, 0));

        OGRE_CHECK_GL_ERROR(glBindBuffer(GL_ARRAY_BUFFER, 0));

        OGRE_CHECK_GL_ERROR(glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0));

        OGRE_CHECK_GL_ERROR(glBindFramebuffer(GL_FRAMEBUFFER, 0));

        OGRE_CHECK_GL_ERROR(glBindRenderbuffer(GL_RENDERBUFFER, 0));

        OGRE_CHECK_GL_ERROR(glActiveTexture(GL_TEXTURE0));

        OGRE_CHECK_GL_ERROR(glClearColor(mClearColour[0], mClearColour[1], mClearColour[2], mClearColour[3]));

        OGRE_CHECK_GL_ERROR(glColorMask(mColourMask[0], mColourMask[1], mColourMask[2], mColourMask[3]));
    }

    void GLES2StateCacheManagerImp::clearCache()
    {
        mDepthMask = GL_TRUE;
        mPolygonMode = GL_FILL;
        mBlendEquation = GL_FUNC_ADD;
        mCullFace = GL_BACK;
        mDepthFunc = GL_LESS;
        mStencilMask = 0xFFFFFFFF;
        mActiveTextureUnit = 0;
        mDiscardBuffers = 0;
        mClearDepth = 1.0f;
        mLastBoundTexID = 0;

        // Initialize our cache variables and also the GL so that the
        // stored values match the GL state
        mBlendFuncSource = GL_ONE;
        mBlendFuncDest = GL_ZERO;

        mClearColour.resize(4);
        mClearColour[0] = mClearColour[1] = mClearColour[2] = mClearColour[3] = 0.0f;

        mColourMask.resize(4);
        mColourMask[0] = mColourMask[1] = mColourMask[2] = mColourMask[3] = GL_TRUE;

        mEnableVector.reserve(25);
        mEnableVector.clear();
        mActiveBufferMap.clear();
        mTexUnitsMap.clear();

        mEnableVector.reserve(64);
        mEnabledVertexAttribs.clear();
    }

    GLES2StateCacheManagerImp::~GLES2StateCacheManagerImp(void)
    {
        mColourMask.clear();
        mClearColour.clear();
        mActiveBufferMap.clear();
        mEnableVector.clear();
        mTexUnitsMap.clear();
    }

    void GLES2StateCacheManagerImp::bindGLBuffer(GLenum target, GLuint buffer, GLenum attach, bool force)
    {
		bool update = false;
        BindBufferMap::iterator i = mActiveBufferMap.find(target);
        if (i == mActiveBufferMap.end())
        {
            // Haven't cached this state yet.  Insert it into the map
            mActiveBufferMap.insert(BindBufferMap::value_type(target, buffer));
			update = true;
        }
        else if((*i).second != buffer || force) // Update the cached value if needed
        {
			(*i).second = buffer;
			update = true;
        }

		// Update GL
		if(update)
		{
            if(target == GL_FRAMEBUFFER)
            {
                OGRE_CHECK_GL_ERROR(glBindFramebuffer(target, buffer));
            }
            else if(target == GL_RENDERBUFFER)
            {
                OGRE_CHECK_GL_ERROR(glBindRenderbuffer(target, buffer));
            }
            else
            {
                OGRE_CHECK_GL_ERROR(glBindBuffer(target, buffer));
            }
		}
    }

    void GLES2StateCacheManagerImp::deleteGLBuffer(GLenum target, GLuint buffer, GLenum attach, bool force)
    {
        // Buffer name 0 is reserved and we should never try to delete it
        if(buffer == 0)
            return;

        BindBufferMap::iterator i = mActiveBufferMap.find(target);

        if (i != mActiveBufferMap.end() && ((*i).second == buffer || force))
        {
			if(target == GL_FRAMEBUFFER)
            {
				OGRE_CHECK_GL_ERROR(glDeleteFramebuffers(1, &buffer));
            }
            else if(target == GL_RENDERBUFFER)
            {
				OGRE_CHECK_GL_ERROR(glDeleteRenderbuffers(1, &buffer));
            }
            else
            {
				OGRE_CHECK_GL_ERROR(glDeleteBuffers(1, &buffer));
            }

            // Currently bound buffer is being deleted, update the cached value to 0,
            // which it likely the buffer that will be bound by the driver.
            // An update will be forced next time we try to bind on this target.
            (*i).second = 0;
        }
    }

    void GLES2StateCacheManagerImp::invalidateStateForTexture(GLuint texture)
    {
        mTexUnitsMap.erase(texture);
    }

    // TODO: Store as high/low bits of a GLuint, use vector instead of map for TexParameteriMap
    void GLES2StateCacheManagerImp::setTexParameteri(GLenum target, GLenum pname, GLint param)
    {
        // Check if we have a map entry for this texture id. If not, create a blank one and insert it.
        TexUnitsMap::iterator it = mTexUnitsMap.find(mLastBoundTexID);
        if (it == mTexUnitsMap.end())
        {
            TextureUnitParams unit;
            mTexUnitsMap[mLastBoundTexID] = unit;

            // Update the iterator
            it = mTexUnitsMap.find(mLastBoundTexID);
        }

        // Get a local copy of the parameter map and search for this parameter
        TexParameteriMap &myMap = (*it).second.mTexParameteriMap;
        TexParameteriMap::iterator i = myMap.find(pname);

        if (i == myMap.end())
        {
            // Haven't cached this state yet.  Insert it into the map
            myMap.insert(TexParameteriMap::value_type(pname, param));

            // Update GL
            OGRE_CHECK_GL_ERROR(glTexParameteri(target, pname, param));
        }
        else
        {
            // Update the cached value if needed
            if((*i).second != param)
            {
                (*i).second = param;

                // Update GL
                OGRE_CHECK_GL_ERROR(glTexParameteri(target, pname, param));
            }
        }
    }

    void GLES2StateCacheManagerImp::setTexParameterf(GLenum target, GLenum pname, GLfloat param)
    {
        // Check if we have a map entry for this texture id. If not, create a blank one and insert it.
        TexUnitsMap::iterator it = mTexUnitsMap.find(mLastBoundTexID);
        if (it == mTexUnitsMap.end())
        {
            TextureUnitParams unit;
            mTexUnitsMap[mLastBoundTexID] = unit;

            // Update the iterator
            it = mTexUnitsMap.find(mLastBoundTexID);
        }

        // Get a local copy of the parameter map and search for this parameter
        TexParameterfMap &myMap = (*it).second.mTexParameterfMap;
        TexParameterfMap::iterator i = myMap.find(pname);

        if (i == myMap.end())
        {
            // Haven't cached this state yet.  Insert it into the map
            myMap.insert(TexParameterfMap::value_type(pname, param));

            // Update GL
            OGRE_CHECK_GL_ERROR(glTexParameterf(target, pname, param));
        }
        else
        {
            // Update the cached value if needed
            if((*i).second != param)
            {
                (*i).second = param;

                // Update GL
                OGRE_CHECK_GL_ERROR(glTexParameterf(target, pname, param));
            }
        }
    }

    void GLES2StateCacheManagerImp::getTexParameterfv(GLenum target, GLenum pname, GLfloat *params)
    {
        // Check if we have a map entry for this texture id.
        TexUnitsMap::iterator it = mTexUnitsMap.find(mLastBoundTexID);

        // Get a local copy of the parameter map and search for this parameter
        TexParameterfMap::iterator i = (*it).second.mTexParameterfMap.find(pname);

        params = &(*i).second;
    }

    void GLES2StateCacheManagerImp::bindGLTexture(GLenum target, GLuint texture)
    {
        mLastBoundTexID = texture;

        // Update GL
        OGRE_CHECK_GL_ERROR(glBindTexture(target, texture));
    }

    bool GLES2StateCacheManagerImp::activateGLTextureUnit(size_t unit)
	{
		if (mActiveTextureUnit != unit)
		{
			if (unit < dynamic_cast<GLES2RenderSystem*>(Root::getSingleton().getRenderSystem())->getCapabilities()->getNumTextureUnits())
			{
				OGRE_CHECK_GL_ERROR(glActiveTexture(GL_TEXTURE0 + unit));
				mActiveTextureUnit = unit;
				return true;
			}
			else if (!unit)
			{
				// always ok to use the first unit
				return true;
			}
			else
			{
				return false;
			}
		}
		else
		{
			return true;
		}
	}

    // TODO: Store as high/low bits of a GLuint
    void GLES2StateCacheManagerImp::setBlendFunc(GLenum source, GLenum dest)
    {
        if(mBlendFuncSource != source || mBlendFuncDest != dest)
        {
            mBlendFuncSource = source;
            mBlendFuncDest = dest;

            OGRE_CHECK_GL_ERROR(glBlendFunc(source, dest));
        }
    }

    void GLES2StateCacheManagerImp::setBlendEquation(GLenum eq)
    {
        if(mBlendEquation != eq)
        {
            mBlendEquation = eq;

            OGRE_CHECK_GL_ERROR(glBlendEquation(eq));
        }
    }

    void GLES2StateCacheManagerImp::setDepthMask(GLboolean mask)
    {
        if(mDepthMask != mask)
        {
            mDepthMask = mask;

            OGRE_CHECK_GL_ERROR(glDepthMask(mask));
        }
    }

    void GLES2StateCacheManagerImp::setDepthFunc(GLenum func)
    {
        if(mDepthFunc != func)
        {
            mDepthFunc = func;

            OGRE_CHECK_GL_ERROR(glDepthFunc(func));
        }
    }

    void GLES2StateCacheManagerImp::setClearDepth(GLclampf depth)
    {
        if(mClearDepth != depth)
        {
            mClearDepth = depth;

            OGRE_CHECK_GL_ERROR(glClearDepthf(depth));
        }
    }

    void GLES2StateCacheManagerImp::setClearColour(GLclampf red, GLclampf green, GLclampf blue, GLclampf alpha)
    {
        if((mClearColour[0] != red) ||
           (mClearColour[1] != green) ||
           (mClearColour[2] != blue) ||
           (mClearColour[3] != alpha))
        {
            mClearColour[0] = red;
            mClearColour[1] = green;
            mClearColour[2] = blue;
            mClearColour[3] = alpha;

            OGRE_CHECK_GL_ERROR(glClearColor(mClearColour[0], mClearColour[1], mClearColour[2], mClearColour[3]));
        }
    }

    void GLES2StateCacheManagerImp::setColourMask(GLboolean red, GLboolean green, GLboolean blue, GLboolean alpha)
    {
        if((mColourMask[0] != red) ||
           (mColourMask[1] != green) ||
           (mColourMask[2] != blue) ||
           (mColourMask[3] != alpha))
        {
            mColourMask[0] = red;
            mColourMask[1] = green;
            mColourMask[2] = blue;
            mColourMask[3] = alpha;

            OGRE_CHECK_GL_ERROR(glColorMask(mColourMask[0], mColourMask[1], mColourMask[2], mColourMask[3]));
        }
    }

    void GLES2StateCacheManagerImp::setStencilMask(GLuint mask)
    {
        if(mStencilMask != mask)
        {
            mStencilMask = mask;

            OGRE_CHECK_GL_ERROR(glStencilMask(mask));
        }
    }

    void GLES2StateCacheManagerImp::setEnabled(GLenum flag)
    {
        bool found = std::find(mEnableVector.begin(), mEnableVector.end(), flag) != mEnableVector.end();
        if(!found)
        {
            mEnableVector.push_back(flag);

            OGRE_CHECK_GL_ERROR(glEnable(flag));
        }
    }

    void GLES2StateCacheManagerImp::setDisabled(GLenum flag)
    {
        vector<GLenum>::iterator iter = std::find(mEnableVector.begin(), mEnableVector.end(), flag);
        if(iter != mEnableVector.end())
        {
            mEnableVector.erase(iter);

            OGRE_CHECK_GL_ERROR(glDisable(flag));
        }
    }

    void GLES2StateCacheManagerImp::setVertexAttribEnabled(GLuint attrib)
    {
        bool found = std::find(mEnabledVertexAttribs.begin(), mEnabledVertexAttribs.end(), attrib) != mEnabledVertexAttribs.end();
        if(!found)
        {
            mEnabledVertexAttribs.push_back(attrib);

            OGRE_CHECK_GL_ERROR(glEnableVertexAttribArray(attrib));
        }
    }

    void GLES2StateCacheManagerImp::setVertexAttribDisabled(GLuint attrib)
    {
        vector<GLuint>::iterator iter = std::find(mEnabledVertexAttribs.begin(), mEnabledVertexAttribs.end(), attrib);
        if(iter != mEnabledVertexAttribs.end())
        {
            mEnabledVertexAttribs.erase(iter);

            OGRE_CHECK_GL_ERROR(glDisableVertexAttribArray(attrib));
        }
    }

    void GLES2StateCacheManagerImp::setCullFace(GLenum face)
    {
        if(mCullFace != face)
        {
            mCullFace = face;

            OGRE_CHECK_GL_ERROR(glCullFace(face));
        }
    }
}
<|endoftext|>


<commit_before>// This file is a part of the IncludeOS unikernel - www.includeos.org
//
// Copyright 2015 Oslo and Akershus University College of Applied Sciences
// and Alfred Bratterud
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <cmath>   // rand()
#include <sstream>

#include <os>
#include <net/inet4>
#include <net/dhcp/dh4client.hpp>

using namespace std::chrono;

std::string HTML_RESPONSE();

void create_server(net::TCP::Connection& conn)
{
  // Add a TCP connection handler - here a hardcoded HTTP-service
  conn.onAccept(
  [] (auto conn) -> bool
  {
    printf("<Service> @onAccept - Connection attempt from: %s \n",
           conn->to_string().c_str());
    return true; // allow all connections

  }).onConnect(
  [] (auto conn) {
    printf("<Service> @onConnect - Connection successfully established.\n");
    // read async with a buffer size of 1024 bytes
    // define what to do when data is read
    conn->read(1024, [conn](net::TCP::buffer_t buf, size_t n) {
        // create string from buffer
        std::string data { (char*)buf.get(), n };
        printf("<Service> @read:\n%s\n", data.c_str());

        // create response
        std::string response = HTML_RESPONSE();
        // write the data from the string with the strings size
        conn->write(response.data(), response.size(), [](size_t n) {
            printf("<Service> @write: %u bytes written\n", n);
          });
      });

  }).onDisconnect(
  [] (auto conn, auto reason) {
      printf("<Service> @onDisconnect - Reason: %s \n", reason.to_string().c_str());
      conn->close();
  });
}

void Service::start() {
  srand(OS::cycles_since_boot());

  // Two IP-stack objects

  // Stack with network interface (eth0) driven by VirtioNet
  // DNS address defaults to 8.8.8.8
  // Static IP configuration, until we (possibly) get DHCP
  // @note : Mostly to get a robust demo service that it works with and without DHCP
  static auto inet1 = new_ipv4_stack<>({ 10,0,0,42 },      // IP
                                       { 255,255,255,0 },  // Netmask
                                       { 10,0,0,1 });      // Gateway

  // Assign a driver (VirtioNet) to a network interface (eth1)
  // @note: We could determine the appropirate driver dynamically, but then we'd
  // have to include all the drivers into the image, which  we want to avoid.
  static auto inet2 = new_ipv4_stack<1,VirtioNet>({ 20,0,0,42 },      // IP
                                                  { 255,255,255,0 },  // Netmask
                                                  { 20,0,0,1 });      // Gateway
  // Set up a TCP server on port 80
  auto& server1 = inet1->tcp().bind(80);
  create_server(server1);

  auto& server2 = inet2->tcp().bind(80);
  create_server(server2);

  // print some useful netstats every 30 secs
  hw::PIT::instance().onRepeatedTimeout(30s, [] {
    printf("<INET_1> TCP STATUS:\n%s \n", inet1->tcp().status().c_str());
    printf("<INET_2> TCP STATUS:\n%s \n", inet2->tcp().status().c_str());
  });

  printf("*** TEST SERVICE STARTED *** \n");
}

std::string HTML_RESPONSE() {
  int color = rand();
  std::stringstream stream;

  /* HTML Fonts */
  std::string ubuntu_medium  = "font-family: \'Ubuntu\', sans-serif; font-weight: 500; ";
  std::string ubuntu_normal  = "font-family: \'Ubuntu\', sans-serif; font-weight: 400; ";
  std::string ubuntu_light  = "font-family: \'Ubuntu\', sans-serif; font-weight: 300; ";

  /* HTML */
  stream << ""
         << "<link href='https://fonts.googleapis.com/css?family=Ubuntu:500,300' rel='stylesheet' type='text/css'>"
         << ""
         << "<h1 style= \"color: " << "#" << std::hex << (color >> 8) << "\">"
         <<  "IncludeOS
"
         <<  "
Now speaks TCP!
"
    // .... generate more dynamic content
         << "
  ...and can improvise http. With limitations of course, but it's been easier than expected so far
"
         << "
 © 2015, Oslo and Akershus University College of Applied Sciences "
         << "\n";

  std::string html = stream.str();

  std::string header="HTTP/1.1 200 OK \n "        \
    "Date: Mon, 01 Jan 1970 00:00:01 GMT \n"      \
    "Server: IncludeOS prototype 4.0 \n"        \
    "Last-Modified: Wed, 08 Jan 2003 23:11:55 GMT \n"   \
    "Content-Type: text/html; charset=UTF-8 \n"     \
    "Content-Length: "+std::to_string(html.size())+"\n"   \
    "Accept-Ranges: bytes\n"          \
    "Connection: close\n\n";
  return header + html;
}
<commit_msg>Refactored function HTML_RESPONSE<commit_after>// This file is a part of the IncludeOS unikernel - www.includeos.org
//
// Copyright 2015 Oslo and Akershus University College of Applied Sciences
// and Alfred Bratterud
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <cmath>   // rand()
#include <sstream>

#include <os>
#include <net/inet4>
#include <net/dhcp/dh4client.hpp>

using namespace std::chrono;

std::string HTML_RESPONSE();

void create_server(net::TCP::Connection& conn)
{
  // Add a TCP connection handler - here a hardcoded HTTP-service
  conn.onAccept(
  [] (auto conn) -> bool
  {
    printf("<Service> @onAccept - Connection attempt from: %s \n",
           conn->to_string().c_str());
    return true; // allow all connections

  }).onConnect(
  [] (auto conn) {
    printf("<Service> @onConnect - Connection successfully established.\n");
    // read async with a buffer size of 1024 bytes
    // define what to do when data is read
    conn->read(1024, [conn](net::TCP::buffer_t buf, size_t n) {
        // create string from buffer
        std::string data { (char*)buf.get(), n };
        printf("<Service> @read:\n%s\n", data.c_str());

        // create response
        std::string response = HTML_RESPONSE();
        // write the data from the string with the strings size
        conn->write(response.data(), response.size(), [](size_t n) {
            printf("<Service> @write: %u bytes written\n", n);
          });
      });

  }).onDisconnect(
  [] (auto conn, auto reason) {
      printf("<Service> @onDisconnect - Reason: %s \n", reason.to_string().c_str());
      conn->close();
  });
}

void Service::start() {
  srand(OS::cycles_since_boot());

  // Two IP-stack objects

  // Stack with network interface (eth0) driven by VirtioNet
  // DNS address defaults to 8.8.8.8
  // Static IP configuration, until we (possibly) get DHCP
  // @note : Mostly to get a robust demo service that it works with and without DHCP
  static auto inet1 = new_ipv4_stack<>({ 10,0,0,42 },      // IP
                                       { 255,255,255,0 },  // Netmask
                                       { 10,0,0,1 });      // Gateway

  // Assign a driver (VirtioNet) to a network interface (eth1)
  // @note: We could determine the appropirate driver dynamically, but then we'd
  // have to include all the drivers into the image, which  we want to avoid.
  static auto inet2 = new_ipv4_stack<1,VirtioNet>({ 20,0,0,42 },      // IP
                                                  { 255,255,255,0 },  // Netmask
                                                  { 20,0,0,1 });      // Gateway
  // Set up a TCP server on port 80
  auto& server1 = inet1->tcp().bind(80);
  create_server(server1);

  auto& server2 = inet2->tcp().bind(80);
  create_server(server2);

  // print some useful netstats every 30 secs
  hw::PIT::instance().onRepeatedTimeout(30s, [] {
    printf("<INET_1> TCP STATUS:\n%s \n", inet1->tcp().status().c_str());
    printf("<INET_2> TCP STATUS:\n%s \n", inet2->tcp().status().c_str());
  });

  printf("*** TEST SERVICE STARTED *** \n");
}

std::string HTML_RESPONSE() {
  const int color = rand();

  /* HTML Fonts */
  const std::string ubuntu_medium = "font-family: \'Ubuntu\', sans-serif; font-weight: 500; ";
  const std::string ubuntu_normal = "font-family: \'Ubuntu\', sans-serif; font-weight: 400; ";
  const std::string ubuntu_light  = "font-family: \'Ubuntu\', sans-serif; font-weight: 300; ";

  /* HTML */
  std::stringstream stream;
  stream << "<!DOCTYPE html>"
         << "<link href='https://fonts.googleapis.com/css?family=Ubuntu:500,300' rel='stylesheet' type='text/css'>"
         << ""
         << "<h1 style='color: #" << std::hex << (color >> 8) << "'>"
         <<  "IncludeOS
"
         <<  "
Now speaks TCP!
"
         // .... generate more dynamic content
         << "
  ...and can improvise http. With limitations of course, but it's been easier than expected so far
"
         << "
 © 2015, Oslo and Akershus University College of Applied Sciences "
         << "";

  const std::string html = stream.str();

  std::string header
  {
    "HTTP/1.1 200 OK\n"
    "Date: Mon, 01 Jan 1970 00:00:01 GMT\n"
    "Server: IncludeOS prototype 4.0\n"
    "Last-Modified: Wed, 08 Jan 2003 23:11:55 GMT\n"
    "Content-Type: text/html; charset=UTF-8\n"
    "Content-Length: "+std::to_string(html.size())+"\n"
    "Accept-Ranges: bytes\n"
    "Connection: close\n\n"
  };

  return header + html;
}
<|endoftext|>


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

/**
 * @file
 **/

#include "modules/planning/scenarios/side_pass/stage_approach_obstacle.h"

#include <algorithm>
#include <string>
#include <vector>

#include "modules/common/proto/pnc_point.pb.h"

#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/common/obstacle_blocking_analyzer.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/speed_profile_generator.h"

namespace apollo {
namespace planning {
namespace scenario {
namespace side_pass {

using apollo::common::TrajectoryPoint;
using apollo::common::VehicleConfigHelper;

/*
 * @brief: STAGE ApproachObstacle in side_pass scenario
 */
Stage::StageStatus StageApproachObstacle::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  ADEBUG << "SIDEPASS: Approaching obstacle.";
  std::string blocking_obstacle_id = GetContext()->front_blocking_obstacle_id_;
  const SLBoundary& adc_sl_boundary =
      frame->reference_line_info().front().AdcSlBoundary();
  const PathDecision& path_decision =
      frame->reference_line_info().front().path_decision();

  double obstacle_start_s = -1.0;
  for (const auto* obstacle : path_decision.obstacles().Items()) {
    if (obstacle->Id() == blocking_obstacle_id) {
      obstacle_start_s = obstacle->PerceptionSLBoundary().start_s();
      break;
    }
  }
  if (obstacle_start_s < 0.0) {
    AWARN << "front blocking obstacle: " << blocking_obstacle_id
          << " is not found";
    next_stage_ = ScenarioConfig::NO_STAGE;
    return Stage::FINISHED;
  }

  if ((obstacle_start_s - adc_sl_boundary.end_s()) <
      GetContext()->scenario_config_.min_front_obstacle_distance()) {
    next_stage_ = ScenarioConfig::NO_STAGE;
    return Stage::FINISHED;
  }

  double kBlockingObstacleDistance = 5.0;
  double stop_fence_s = obstacle_start_s - kBlockingObstacleDistance;
  stop_fence_s = std::max(stop_fence_s, adc_sl_boundary.end_s() + 0.2);
  std::string virtual_obstacle_id = blocking_obstacle_id + "_virtual_stop";
  for (auto& reference_line_info : *frame->mutable_reference_line_info()) {
    auto* obstacle = frame->CreateStopObstacle(
        &reference_line_info, virtual_obstacle_id, stop_fence_s);
    if (!obstacle) {
      AERROR << "Failed to create virtual stop obstacle["
             << blocking_obstacle_id << "]";
      next_stage_ = ScenarioConfig::NO_STAGE;
      return Stage::FINISHED;
    }
    Obstacle* stop_wall = reference_line_info.AddObstacle(obstacle);
    if (!stop_wall) {
      AERROR << "Failed to create stop obstacle for: " << blocking_obstacle_id;
      next_stage_ = ScenarioConfig::NO_STAGE;
      return Stage::FINISHED;
    }

    const double stop_distance = 0.2;
    const double stop_s = stop_fence_s - stop_distance;
    const auto& reference_line = reference_line_info.reference_line();
    auto stop_point = reference_line.GetReferencePoint(stop_s);
    double stop_heading = reference_line.GetReferencePoint(stop_s).heading();

    ObjectDecisionType stop;
    auto stop_decision = stop.mutable_stop();
    stop_decision->set_reason_code(StopReasonCode::STOP_REASON_OBSTACLE);
    stop_decision->set_distance_s(-stop_distance);
    stop_decision->set_stop_heading(stop_heading);
    stop_decision->mutable_stop_point()->set_x(stop_point.x());
    stop_decision->mutable_stop_point()->set_y(stop_point.y());
    stop_decision->mutable_stop_point()->set_z(0.0);

    auto* path_decision = reference_line_info.path_decision();
    path_decision->AddLongitudinalDecision("SidePass", stop_wall->Id(), stop);

    break;
  }

  // check the status of side pass scenario
  bool has_blocking_obstacle = false;
  for (const auto* obstacle : path_decision.obstacles().Items()) {
    if (obstacle->IsVirtual() || !obstacle->IsStatic()) {
      continue;
    }
    CHECK(obstacle->IsStatic());
    if (obstacle->speed() >
        GetContext()->scenario_config_.block_obstacle_min_speed()) {
      continue;
    }
    if (obstacle->PerceptionSLBoundary().start_s() <=
        adc_sl_boundary.end_s()) {  // such vehicles are behind the ego car.
      continue;
    }
    if (obstacle->PerceptionSLBoundary().start_s() >
        adc_sl_boundary.end_s() +
            GetContext()->scenario_config_.max_front_obstacle_distance()) {
      // vehicles are far away
      continue;
    }

    // check driving_width
    const auto& reference_line =
        frame->reference_line_info().front().reference_line();
    const double driving_width =
        reference_line.GetDrivingWidth(obstacle->PerceptionSLBoundary());
    const double adc_width =
        VehicleConfigHelper::GetConfig().vehicle_param().width();
    if (driving_width - adc_width - FLAGS_static_decision_nudge_l_buffer >
        GetContext()->scenario_config_.min_l_nudge_buffer()) {
      continue;
    }
  }

  if (!has_blocking_obstacle) {
    next_stage_ = ScenarioConfig::NO_STAGE;
    ADEBUG << "There is no blocking obstacle.";
    return Stage::FINISHED;
  }
  // do path planning
  bool plan_ok = ExecuteTaskOnReferenceLine(planning_start_point, frame);
  if (!plan_ok) {
    AERROR << "Stage " << Name() << " error: "
           << "planning on reference line failed.";
    return Stage::ERROR;
  }

  const ReferenceLineInfo& reference_line_info =
      frame->reference_line_info().front();
  double adc_velocity = frame->vehicle_state().linear_velocity();
  double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();

  double front_obstacle_distance = 1000;
  for (const auto* obstacle : path_decision.obstacles().Items()) {
    if (obstacle->IsVirtual()) {
      continue;
    }

    bool is_on_road = reference_line_info.reference_line().HasOverlap(
        obstacle->PerceptionBoundingBox());
    if (!is_on_road) {
      continue;
    }

    const auto& obstacle_sl = obstacle->PerceptionSLBoundary();
    if (obstacle_sl.end_s() <= reference_line_info.AdcSlBoundary().start_s()) {
      continue;
    }

    double distance = obstacle_sl.start_s() - adc_front_edge_s;
    if (distance < front_obstacle_distance) {
      front_obstacle_distance = distance;
    }
  }

  if ((front_obstacle_distance) < 0) {
    AERROR << "Stage " << Name() << " error: "
           << "front obstacle has wrong position.";
    return Stage::ERROR;
  }

  double max_stop_velocity =
      GetContext()->scenario_config_.approach_obstacle_max_stop_speed();
  double min_stop_obstacle_distance =
      GetContext()->scenario_config_.approach_obstacle_min_stop_distance();

  ADEBUG << "front_obstacle_distance = " << front_obstacle_distance;
  ADEBUG << "adc_velocity = " << adc_velocity;
  if (adc_velocity < max_stop_velocity &&
      front_obstacle_distance > min_stop_obstacle_distance) {
    next_stage_ = ScenarioConfig::SIDE_PASS_GENERATE_PATH;
    return Stage::FINISHED;
  }

  return Stage::RUNNING;
}

}  // namespace side_pass
}  // namespace scenario
}  // namespace planning
}  // namespace apollo
<commit_msg>Planning: Bug fix on side-pass.<commit_after>/******************************************************************************
 * Copyright 2018 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

/**
 * @file
 **/

#include "modules/planning/scenarios/side_pass/stage_approach_obstacle.h"

#include <algorithm>
#include <string>
#include <vector>

#include "modules/common/proto/pnc_point.pb.h"

#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/common/obstacle_blocking_analyzer.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/common/speed_profile_generator.h"

namespace apollo {
namespace planning {
namespace scenario {
namespace side_pass {

using apollo::common::TrajectoryPoint;
using apollo::common::VehicleConfigHelper;

/*
 * @brief: STAGE ApproachObstacle in side_pass scenario
 */
Stage::StageStatus StageApproachObstacle::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  ADEBUG << "SIDEPASS: Approaching obstacle.";
  std::string blocking_obstacle_id = GetContext()->front_blocking_obstacle_id_;
  const SLBoundary& adc_sl_boundary =
      frame->reference_line_info().front().AdcSlBoundary();
  const PathDecision& path_decision =
      frame->reference_line_info().front().path_decision();

  double obstacle_start_s = -1.0;
  for (const auto* obstacle : path_decision.obstacles().Items()) {
    if (obstacle->Id() == blocking_obstacle_id) {
      obstacle_start_s = obstacle->PerceptionSLBoundary().start_s();
      break;
    }
  }
  if (obstacle_start_s < 0.0) {
    AWARN << "front blocking obstacle: " << blocking_obstacle_id
          << " is not found";
    next_stage_ = ScenarioConfig::NO_STAGE;
    return Stage::FINISHED;
  }

  if ((obstacle_start_s - adc_sl_boundary.end_s()) <
      GetContext()->scenario_config_.min_front_obstacle_distance()) {
    next_stage_ = ScenarioConfig::NO_STAGE;
    return Stage::FINISHED;
  }

  double kBlockingObstacleDistance = 5.0;
  double stop_fence_s = obstacle_start_s - kBlockingObstacleDistance;
  stop_fence_s = std::max(stop_fence_s, adc_sl_boundary.end_s() + 0.2);
  std::string virtual_obstacle_id = blocking_obstacle_id + "_virtual_stop";
  for (auto& reference_line_info : *frame->mutable_reference_line_info()) {
    auto* obstacle = frame->CreateStopObstacle(
        &reference_line_info, virtual_obstacle_id, stop_fence_s);
    if (!obstacle) {
      AERROR << "Failed to create virtual stop obstacle["
             << blocking_obstacle_id << "]";
      next_stage_ = ScenarioConfig::NO_STAGE;
      return Stage::FINISHED;
    }
    Obstacle* stop_wall = reference_line_info.AddObstacle(obstacle);
    if (!stop_wall) {
      AERROR << "Failed to create stop obstacle for: " << blocking_obstacle_id;
      next_stage_ = ScenarioConfig::NO_STAGE;
      return Stage::FINISHED;
    }

    const double stop_distance = 0.2;
    const double stop_s = stop_fence_s - stop_distance;
    const auto& reference_line = reference_line_info.reference_line();
    auto stop_point = reference_line.GetReferencePoint(stop_s);
    double stop_heading = reference_line.GetReferencePoint(stop_s).heading();

    ObjectDecisionType stop;
    auto stop_decision = stop.mutable_stop();
    stop_decision->set_reason_code(StopReasonCode::STOP_REASON_OBSTACLE);
    stop_decision->set_distance_s(-stop_distance);
    stop_decision->set_stop_heading(stop_heading);
    stop_decision->mutable_stop_point()->set_x(stop_point.x());
    stop_decision->mutable_stop_point()->set_y(stop_point.y());
    stop_decision->mutable_stop_point()->set_z(0.0);

    auto* path_decision = reference_line_info.path_decision();
    path_decision->AddLongitudinalDecision("SidePass", stop_wall->Id(), stop);

    break;
  }

  // check the status of side pass scenario
  bool has_blocking_obstacle = false;
  for (const auto* obstacle : path_decision.obstacles().Items()) {
    if (obstacle->IsVirtual() || !obstacle->IsStatic()) {
      continue;
    }
    CHECK(obstacle->IsStatic());
    if (obstacle->speed() >
        GetContext()->scenario_config_.block_obstacle_min_speed()) {
      continue;
    }
    if (obstacle->PerceptionSLBoundary().start_s() <=
        adc_sl_boundary.end_s()) {  // such vehicles are behind the ego car.
      continue;
    }
    if (obstacle->PerceptionSLBoundary().start_s() >
        adc_sl_boundary.end_s() +
            GetContext()->scenario_config_.max_front_obstacle_distance()) {
      // vehicles are far away
      continue;
    }

    // check driving_width
    const auto& reference_line =
        frame->reference_line_info().front().reference_line();
    const double driving_width =
        reference_line.GetDrivingWidth(obstacle->PerceptionSLBoundary());
    const double adc_width =
        VehicleConfigHelper::GetConfig().vehicle_param().width();
    if (driving_width - adc_width - FLAGS_static_decision_nudge_l_buffer >
        GetContext()->scenario_config_.min_l_nudge_buffer()) {
      continue;
    }
    has_blocking_obstacle = true;
    break;
  }

  if (!has_blocking_obstacle) {
    next_stage_ = ScenarioConfig::NO_STAGE;
    ADEBUG << "There is no blocking obstacle.";
    return Stage::FINISHED;
  }
  // do path planning
  bool plan_ok = ExecuteTaskOnReferenceLine(planning_start_point, frame);
  if (!plan_ok) {
    AERROR << "Stage " << Name() << " error: "
           << "planning on reference line failed.";
    return Stage::ERROR;
  }

  const ReferenceLineInfo& reference_line_info =
      frame->reference_line_info().front();
  double adc_velocity = frame->vehicle_state().linear_velocity();
  double adc_front_edge_s = reference_line_info.AdcSlBoundary().end_s();

  double front_obstacle_distance = 1000;
  for (const auto* obstacle : path_decision.obstacles().Items()) {
    if (obstacle->IsVirtual()) {
      continue;
    }

    bool is_on_road = reference_line_info.reference_line().HasOverlap(
        obstacle->PerceptionBoundingBox());
    if (!is_on_road) {
      continue;
    }

    const auto& obstacle_sl = obstacle->PerceptionSLBoundary();
    if (obstacle_sl.end_s() <= reference_line_info.AdcSlBoundary().start_s()) {
      continue;
    }

    double distance = obstacle_sl.start_s() - adc_front_edge_s;
    if (distance < front_obstacle_distance) {
      front_obstacle_distance = distance;
    }
  }

  if ((front_obstacle_distance) < 0) {
    AERROR << "Stage " << Name() << " error: "
           << "front obstacle has wrong position.";
    return Stage::ERROR;
  }

  double max_stop_velocity =
      GetContext()->scenario_config_.approach_obstacle_max_stop_speed();
  double min_stop_obstacle_distance =
      GetContext()->scenario_config_.approach_obstacle_min_stop_distance();

  ADEBUG << "front_obstacle_distance = " << front_obstacle_distance;
  ADEBUG << "adc_velocity = " << adc_velocity;
  if (adc_velocity < max_stop_velocity &&
      front_obstacle_distance > min_stop_obstacle_distance) {
    next_stage_ = ScenarioConfig::SIDE_PASS_GENERATE_PATH;
    return Stage::FINISHED;
  }

  return Stage::RUNNING;
}

}  // namespace side_pass
}  // namespace scenario
}  // namespace planning
}  // namespace apollo
<|endoftext|>


<commit_before><commit_msg>Add on to the basic framework of answer to question 3.4<commit_after><|endoftext|>


<commit_before>//=============================================================================
//
// *** AddTaskTRDCalib
//
// This macros setup the TRD calibration task
//
//=============================================================================

AliAnalysisTask  *AddTaskTRDCalib(Int_t runNumber)
{
  gSystem->Load("libTRDcalib");
  // pointer to the analysis manager
  AliAnalysisManager *mgr = AliAnalysisManager::GetAnalysisManager();
  if (!mgr) {
    Error("AddTaskTRDCalib", "No analysis manager to connect to.");
    return NULL;
  }

  // check the input handler
  if (!mgr->GetInputEventHandler()) {
    ::Error("AddTask", "This task requires an input event handler");
    return NULL;
  }

  ////////////////////////////////////////////
  // Number of timebins
  ///////////////////////////////////////////
  AliTRDcalibDB *calib = AliTRDcalibDB::Instance();
  Int_t nbOfTimeBins = calib->GetNumberOfTimeBinsDCS();
  if(nbOfTimeBins < 0) nbOfTimeBins = 27;
  ////////////////////////////////////////////
  //
  /////////////////////////////////////////////
  Int_t versiongain, subversiongain, versionvdrift, subversionvdrift;

  /////////////////////////
  // The TRD calib Task
  /////////////////////////
  AliTRDCalibTask *calibTask = new AliTRDCalibTask();

  // Disabling TRD CPAss0 as per https://savannah.cern.ch/bugs/?88813
  //calibTask->SetMaxEvent(-1);

  calibTask->SetMaxNbTracks(600);
  calibTask->SetHisto2d(kTRUE);
  calibTask->SetVector2d(kFALSE);
  calibTask->SetVdriftLinear(kTRUE);
  calibTask->SetExbAlt(kTRUE);
  calibTask->SetNz(0,0);
  calibTask->SetNrphi(0,0);
  calibTask->SetNz(0,1);
  calibTask->SetNrphi(0,1);
  calibTask->SetNz(0,2);
  calibTask->SetNrphi(0,2);
  calibTask->SetLow(0);
  calibTask->SetHigh(30);
  calibTask->SetFillZero(kFALSE);
  // now
  calibTask->AddSelectedTriggerClass("C0OB0-ABCE-NOPF-ALL");
  calibTask->AddSelectedTriggerClass("CTRDCO2-ABCE-NOPF-CENT");
  calibTask->AddSelectedTriggerClass("CTRDCO2-ABCE-NOPF-TRD");
  calibTask->AddSelectedTriggerClass("CTRDCO2-ABCE-NOPF-ALL");
  calibTask->SetReject(kTRUE);
  // before
  //calibTask->AddSelectedTriggerClass("CINT1B-ABCE-NOPF-ALL");
  //calibTask->AddSelectedTriggerClass("CINT1WU-B-NOPF-ALL");
  //calibTask->AddSelectedTriggerClass("CINT7WU-B-NOPF-ALL");
  //calibTask->AddSelectedTriggerClass("CINT7WU-I-NOPF-ALL");
  //calibTask->SetReject(kFALSE);
  //calibTask->SetDebug(2);
  calibTask->SetNbTimeBins(nbOfTimeBins);
  //calibTask->SetMaxEvent(10);
  //calibTask->SetThresholdP(1.0);
  calibTask->SetRequirePrimaryVertex(kTRUE);
  calibTask->SetMinNbOfContributors(1);
  calibTask->SetMaxCluster(100.0);
  calibTask->SetNbMaxCluster(2);
  //calibTask->SetLimitChargeIntegration(kTRUE);

  /////////////////////////////
  // Track cuts
  /////////////////////////////
  AliESDtrackCuts *trackCuts = new AliESDtrackCuts("trackcuts","trackcuts");
  trackCuts->SetMinNClustersTPC(50);
  trackCuts->SetMaxChi2PerClusterTPC(3.5);
  //trackCuts->SetMaxCovDiagonalElements(2,2,0.5,0.5,2);
  trackCuts->SetRequireTPCRefit(kTRUE);
  //trackCuts->SetRequireITSRefit(kTRUE);
  //trackCuts->SetMinNsigmaToVertex(10);
  trackCuts->SetRequireSigmaToVertex(kFALSE);
  trackCuts->SetAcceptKinkDaughters(kFALSE);
  trackCuts->SetMaxDCAToVertexZ(30.0);
  trackCuts->SetMaxDCAToVertexXY(3.0);
  trackCuts->SetDCAToVertex2D(kFALSE);

  calibTask->SetESDtrackCuts(trackCuts);

  mgr->AddTask(calibTask);

  AliAnalysisDataContainer *cinput = mgr->GetCommonInputContainer();
  //AliAnalysisDataContainer *cinput = mgr->GetCommonOutputContainer();

  if (!cinput) cinput = mgr->CreateContainer("cchain",TChain::Class(),
                                      AliAnalysisManager::kInputContainer);

  AliAnalysisDataContainer *coutput =mgr->CreateContainer("TRDCalib",TList::Class(), AliAnalysisManager::kOutputContainer, "AliESDfriends_v1.root");

  mgr->ConnectInput(calibTask,0,cinput);
  mgr->ConnectOutput(calibTask,1,coutput);
  return calibTask;

}

<commit_msg>TRD update<commit_after>//=============================================================================
//
// *** AddTaskTRDCalib
//
// This macros setup the TRD calibration task
//
//=============================================================================

AliAnalysisTask  *AddTaskTRDCalib(Int_t runNumber)
{
  gSystem->Load("libTRDcalib");
  // pointer to the analysis manager
  AliAnalysisManager *mgr = AliAnalysisManager::GetAnalysisManager();
  if (!mgr) {
    Error("AddTaskTRDCalib", "No analysis manager to connect to.");
    return NULL;
  }

  // check the input handler
  if (!mgr->GetInputEventHandler()) {
    ::Error("AddTask", "This task requires an input event handler");
    return NULL;
  }

  ////////////////////////////////////////////
  // Number of timebins
  ///////////////////////////////////////////
  AliTRDcalibDB *calib = AliTRDcalibDB::Instance();
  Int_t nbOfTimeBins = calib->GetNumberOfTimeBinsDCS();
  if(nbOfTimeBins < 0) nbOfTimeBins = 27;
  ////////////////////////////////////////////
  //
  /////////////////////////////////////////////
  Int_t versiongain, subversiongain, versionvdrift, subversionvdrift;

  /////////////////////////
  // The TRD calib Task
  /////////////////////////
  AliTRDCalibTask *calibTask = new AliTRDCalibTask();

  // Disabling TRD CPAss0 as per https://savannah.cern.ch/bugs/?88813
  //calibTask->SetMaxEvent(-1);

  calibTask->SetMaxNbTracks(1600);
  calibTask->SetHisto2d(kTRUE);
  calibTask->SetVector2d(kFALSE);
  calibTask->SetVdriftLinear(kTRUE);
  calibTask->SetExbAlt(kFALSE);
  calibTask->SetNz(0,0);
  calibTask->SetNrphi(0,0);
  calibTask->SetNz(0,1);
  calibTask->SetNrphi(0,1);
  calibTask->SetNz(0,2);
  calibTask->SetNrphi(0,2);
  calibTask->SetLow(0);
  calibTask->SetHigh(30);
  calibTask->SetFillZero(kFALSE);
  // now
  calibTask->AddSelectedTriggerClass("C0OB0-ABCE-NOPF-ALL");
  calibTask->AddSelectedTriggerClass("CTRDCO2-ABCE-NOPF-CENT");
  calibTask->AddSelectedTriggerClass("CTRDCO2-ABCE-NOPF-TRD");
  calibTask->AddSelectedTriggerClass("CTRDCO2-ABCE-NOPF-ALL");
  calibTask->SetReject(kTRUE);
  // before
  //calibTask->AddSelectedTriggerClass("CINT1B-ABCE-NOPF-ALL");
  //calibTask->AddSelectedTriggerClass("CINT1WU-B-NOPF-ALL");
  //calibTask->AddSelectedTriggerClass("CINT7WU-B-NOPF-ALL");
  //calibTask->AddSelectedTriggerClass("CINT7WU-I-NOPF-ALL");
  //calibTask->SetReject(kFALSE);
  //calibTask->SetDebug(2);
  calibTask->SetNbTimeBins(nbOfTimeBins);
  //calibTask->SetMaxEvent(10);
  //calibTask->SetThresholdP(1.0);
  calibTask->SetRequirePrimaryVertex(kTRUE);
  calibTask->SetMinNbOfContributors(1);
  calibTask->SetMaxCluster(100.0);
  calibTask->SetNbMaxCluster(2);
  //calibTask->SetLimitChargeIntegration(kTRUE);

  /////////////////////////////
  // Track cuts
  /////////////////////////////
  AliESDtrackCuts *trackCuts = new AliESDtrackCuts("trackcuts","trackcuts");
  trackCuts->SetMinNClustersTPC(50);
  trackCuts->SetMaxChi2PerClusterTPC(3.5);
  //trackCuts->SetMaxCovDiagonalElements(2,2,0.5,0.5,2);
  trackCuts->SetRequireTPCRefit(kTRUE);
  //trackCuts->SetRequireITSRefit(kTRUE);
  //trackCuts->SetMinNsigmaToVertex(10);
  trackCuts->SetRequireSigmaToVertex(kFALSE);
  trackCuts->SetAcceptKinkDaughters(kFALSE);
  trackCuts->SetMaxDCAToVertexZ(30.0);
  trackCuts->SetMaxDCAToVertexXY(3.0);
  trackCuts->SetDCAToVertex2D(kFALSE);

  calibTask->SetESDtrackCuts(trackCuts);

  mgr->AddTask(calibTask);

  AliAnalysisDataContainer *cinput = mgr->GetCommonInputContainer();
  //AliAnalysisDataContainer *cinput = mgr->GetCommonOutputContainer();

  if (!cinput) cinput = mgr->CreateContainer("cchain",TChain::Class(),
                                      AliAnalysisManager::kInputContainer);

  AliAnalysisDataContainer *coutput =mgr->CreateContainer("TRDCalib",TList::Class(), AliAnalysisManager::kOutputContainer, "AliESDfriends_v1.root");

  mgr->ConnectInput(calibTask,0,cinput);
  mgr->ConnectOutput(calibTask,1,coutput);
  return calibTask;

}
<|endoftext|>


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

  Program:   Insight Segmentation & Registration Toolkit (ITK)
  Module:
  Language:  C++
  Date:
  Version:

Copyright (c) 2000 National Library of Medicine
All rights reserved.

See COPYRIGHT.txt for copyright details.

=========================================================================*/
#include <iostream>
// This file has been generated by BuildHeaderTest.tcl
// Test to include each header file for Insight

#include "itkBalloonForce3DFilter.txx"
#include "itkBalloonForceFilter.txx"
#include "itkBinaryMinMaxCurvatureFlowFunction.txx"
#include "itkBinaryMinMaxCurvatureFlowImageFilter.txx"
#include "itkClassifier.txx"
#include "itkClassifierBase.txx"
#include "itkConnectedRegionsMeshFilter.txx"
#include "itkCurvatureFlowFunction.txx"
#include "itkCurvatureFlowImageFilter.txx"
#include "itkDeformableMesh.txx"
#include "itkDeformableMesh3DFilter.txx"
#include "itkDeformableMeshFilter.txx"
#include "itkDemonsRegistrationFilter.txx"
#include "itkDemonsRegistrationFunction.txx"
#include "itkExtensionVelocitiesImageFilter.txx"
#include "itkFastMarchingExtensionImageFilter.txx"
#include "itkFastMarchingImageFilter.txx"
#include "itkGaussianSupervisedClassifier.txx"
#include "itkGeodesicActiveContourImageFilter.txx"
#include "itkGibbsPriorFilter.txx"
#include "itkGradientVectorFlowImageFilter.txx"
#include "itkHistogramMatchingImageFilter.txx"
#include "itkHybridFilter.txx"
#include "itkImageClassifierBase.txx"
#include "itkImageGaussianModelEstimator.txx"
#include "itkImageKmeansModelEstimator.txx"
#include "itkImageModelEstimatorBase.txx"
#include "itkImageMomentsCalculator.txx"
#include "itkImageRegistrationMethod.txx"
#include "itkImageToImageMetric.txx"
#include "itkKLMRegionGrowImageFilter.txx"
#include "itkKalmanLinearEstimator.h"
#include "itkKmeansUnsupervisedClassifier.txx"
#include "itkLevelSet2DFunction.txx"
#include "itkLevelSet3DFunction.txx"
#include "itkLevelSetFunction.txx"
#include "itkLevelSetImageFilter.txx"
#include "itkLevelSetNeighborhoodExtractor.txx"
#include "itkLevelSetVelocityNeighborhoodExtractor.txx"
#include "itkMRASlabIdentifier.txx"
#include "itkMRFImageFilter.txx"
#include "itkMRIBiasFieldCorrectionFilter.txx"
#include "itkMeanSquaresImageToImageMetric.txx"
#include "itkMeanSquaresPointSetToImageMetric.txx"
#include "itkMinMaxCurvatureFlowFunction.txx"
#include "itkMinMaxCurvatureFlowImageFilter.txx"
#include "itkMinimumMaximumImageCalculator.txx"
#include "itkMultiResolutionImageRegistrationMethod.txx"
#include "itkMultiResolutionPDEDeformableRegistration.txx"
#include "itkMultiResolutionPyramidImageFilter.txx"
#include "itkMutualInformationImageToImageMetric.txx"
#include "itkNormalizedCorrelationImageToImageMetric.txx"
#include "itkNormalizedCorrelationPointSetToImageMetric.txx"
#include "itkOtsuThresholdImageCalculator.txx"
#include "itkPDEDeformableRegistrationFilter.txx"
#include "itkPDEDeformableRegistrationFunction.h"
#include "itkPatternIntensityImageToImageMetric.txx"
#include "itkPatternIntensityPointSetToImageMetric.txx"
#include "itkRGBGibbsPriorFilter.txx"
#include "itkRecursiveMultiResolutionPyramidImageFilter.txx"
#include "itkRegionGrowImageFilter.txx"
#include "itkRegistrationMethod.txx"
#include "itkReinitializeLevelSetImageFilter.txx"
#include "itkShapeDetectionLevelSetFilter.txx"
#include "itkSimpleFuzzyConnectednessImageFilterBase.txx"
#include "itkSimpleFuzzyConnectednessRGBImageFilter.txx"
#include "itkSimpleFuzzyConnectednessScalarImageFilter.txx"
#include "itkSphereMeshSource.txx"
#include "itkSphereSource.txx"
#include "itkSupervisedClassifier.txx"
#include "itkUnsupervisedClassifier.txx"
#include "itkVectorFuzzyConnectednessImageFilter.txx"
#include "itkVoronoiDiagram2D.txx"
#include "itkVoronoiDiagram2DGenerator.txx"
#include "itkVoronoiSegmentationImageFilter.txx"
#include "itkVoronoiSegmentationImageFilterBase.txx"
#include "itkVoronoiSegmentationRGBImageFilter.txx"
#include "itkWatershedBoundary.txx"
#include "itkWatershedBoundaryResolver.txx"
#include "itkWatershedEquivalenceRelabeler.txx"
#include "itkWatershedEquivalencyTable.txx"
#include "itkWatershedImageFilter.txx"
#include "itkWatershedOneWayEquivalencyTable.txx"
#include "itkWatershedRelabeler.txx"
#include "itkWatershedSegmentTable.txx"
#include "itkWatershedSegmentTree.txx"
#include "itkWatershedSegmentTreeGenerator.txx"
#include "itkWatershedSegmenter.txx"

int main ( int argc, char* argv )
{

  return 0;
}

<commit_msg>ENH: Updated to latest headers<commit_after>/*=========================================================================

  Program:   Insight Segmentation & Registration Toolkit (ITK)
  Module:
  Language:  C++
  Date:
  Version:

Copyright (c) 2000 National Library of Medicine
All rights reserved.

See COPYRIGHT.txt for copyright details.

=========================================================================*/
#include <iostream>
// This file has been generated by BuildHeaderTest.tcl
// Test to include each header file for Insight

#include "itkBalloonForce3DFilter.txx"
#include "itkBalloonForceFilter.txx"
#include "itkBinaryMask3DMeshSource.txx"
#include "itkBinaryMinMaxCurvatureFlowFunction.txx"
#include "itkBinaryMinMaxCurvatureFlowImageFilter.txx"
#include "itkClassifier.txx"
#include "itkClassifierBase.txx"
#include "itkConnectedRegionsMeshFilter.txx"
#include "itkCurvatureFlowFunction.txx"
#include "itkCurvatureFlowImageFilter.txx"
#include "itkDeformableMesh.txx"
#include "itkDeformableMesh3DFilter.txx"
#include "itkDeformableMeshFilter.txx"
#include "itkDemonsRegistrationFilter.txx"
#include "itkDemonsRegistrationFunction.txx"
#include "itkExtensionVelocitiesImageFilter.txx"
#include "itkFastMarchingExtensionImageFilter.txx"
#include "itkFastMarchingImageFilter.txx"
#include "itkGaussianSupervisedClassifier.txx"
#include "itkGeodesicActiveContourImageFilter.txx"
#include "itkGibbsPriorFilter.txx"
#include "itkGradientVectorFlowImageFilter.txx"
#include "itkHistogramMatchingImageFilter.txx"
#include "itkHybridFilter.txx"
#include "itkImageClassifierBase.txx"
#include "itkImageGaussianModelEstimator.txx"
#include "itkImageKmeansModelEstimator.txx"
#include "itkImageModelEstimatorBase.txx"
#include "itkImageMomentsCalculator.txx"
#include "itkImageRegistrationMethod.txx"
#include "itkImageToImageMetric.txx"
#include "itkKLMRegionGrowImageFilter.txx"
#include "itkKalmanLinearEstimator.h"
#include "itkKmeansUnsupervisedClassifier.txx"
#include "itkLevelSet2DFunction.txx"
#include "itkLevelSet3DFunction.txx"
#include "itkLevelSetFunction.txx"
#include "itkLevelSetImageFilter.txx"
#include "itkLevelSetNeighborhoodExtractor.txx"
#include "itkLevelSetVelocityNeighborhoodExtractor.txx"
#include "itkMRASlabIdentifier.txx"
#include "itkMRFImageFilter.txx"
#include "itkMRIBiasFieldCorrectionFilter.txx"
#include "itkMeanSquaresImageToImageMetric.txx"
#include "itkMeanSquaresPointSetToImageMetric.txx"
#include "itkMinMaxCurvatureFlowFunction.txx"
#include "itkMinMaxCurvatureFlowImageFilter.txx"
#include "itkMinimumMaximumImageCalculator.txx"
#include "itkMultiResolutionImageRegistrationMethod.txx"
#include "itkMultiResolutionPDEDeformableRegistration.txx"
#include "itkMultiResolutionPyramidImageFilter.txx"
#include "itkMutualInformationImageToImageMetric.txx"
#include "itkNormalizedCorrelationImageToImageMetric.txx"
#include "itkNormalizedCorrelationPointSetToImageMetric.txx"
#include "itkOtsuThresholdImageCalculator.txx"
#include "itkPDEDeformableRegistrationFilter.txx"
#include "itkPDEDeformableRegistrationFunction.h"
#include "itkPatternIntensityImageToImageMetric.txx"
#include "itkPatternIntensityPointSetToImageMetric.txx"
#include "itkRGBGibbsPriorFilter.txx"
#include "itkRecursiveMultiResolutionPyramidImageFilter.txx"
#include "itkRegionGrowImageFilter.txx"
#include "itkRegistrationMethod.txx"
#include "itkReinitializeLevelSetImageFilter.txx"
#include "itkShapeDetectionLevelSetFilter.txx"
#include "itkSimpleFuzzyConnectednessImageFilterBase.txx"
#include "itkSimpleFuzzyConnectednessRGBImageFilter.txx"
#include "itkSimpleFuzzyConnectednessScalarImageFilter.txx"
#include "itkSphereMeshSource.txx"
#include "itkSphereSource.txx"
#include "itkSupervisedClassifier.txx"
#include "itkUnsupervisedClassifier.txx"
#include "itkVectorFuzzyConnectednessImageFilter.txx"
#include "itkVoronoiDiagram2D.txx"
#include "itkVoronoiDiagram2DGenerator.txx"
#include "itkVoronoiSegmentationImageFilter.txx"
#include "itkVoronoiSegmentationImageFilterBase.txx"
#include "itkVoronoiSegmentationRGBImageFilter.txx"
#include "itkWatershedBoundary.txx"
#include "itkWatershedBoundaryResolver.txx"
#include "itkWatershedEquivalenceRelabeler.txx"
#include "itkWatershedEquivalencyTable.txx"
#include "itkWatershedImageFilter.txx"
#include "itkWatershedOneWayEquivalencyTable.txx"
#include "itkWatershedRelabeler.txx"
#include "itkWatershedSegmentTable.txx"
#include "itkWatershedSegmentTree.txx"
#include "itkWatershedSegmentTreeGenerator.txx"
#include "itkWatershedSegmenter.txx"

int main ( int argc, char* argv )
{

  return 0;
}

<|endoftext|>


<commit_before>#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/imgcodecs.hpp>
#include <iostream>
#include <math.h>
#include <stdio.h>

using namespace cv;
using namespace std;

 /*sxlp;'
 '
string type2str(int type) {
  string r;

  uchar depth = type & CV_MAT_DEPTH_MASK;
  uchar chans = 1 + (type >> CV_CN_SHIFT);

  switch ( depth ) {
    case CV_8U:  r = "8U"; break;
    case CV_8S:  r = "8S"; break;
    case CV_16U: r = "16U"; break;
    case CV_16S: r = "16S"; break;
    case CV_32S: r = "32S"; break;
    case CV_32F: r = "32F"; break;
    case CV_64F: r = "64F"; break;
    default:     r = "User"; break;
  }

  r += "C";
  r += (chans+'0');

  return r;
}
*/

int main()
{
	Mat img_bw=imread("tumtum.jpg", 0); // reading the rgb image in Mat data struct

/*
note: we may have to either pre process the image or not work with the custom bgr to greyscale or both
	  as they might not be able to detect edge very well
*/

	Mat grad_x, grad_y;    // derivative along x and y direction respectively
    Mat square_grad_x, square_grad_y;   // absoute value of derivative

    /// Gradient  along X direction
	Sobel( img_bw, grad_x,CV_16S, 1, 0, 3, 1,0, BORDER_DEFAULT );
	/// Gradient  along y direction
	Sobel( img_bw, grad_y,CV_16S, 0, 1, 3, 1, 0, BORDER_DEFAULT );

	square_grad_y=grad_y.mul(grad_y);
	square_grad_x=grad_x.mul(grad_x);

	Mat img_edge;

    addWeighted( square_grad_x,1, square_grad_y, 0.5, 0,img_edge);    // more weight is provided in the horizontal direction for more importance to lane detection

    Mat img_edge_dilated;
    Point anchor=Point(-1,-1);

    Size str_elem_dim_55;
    str_elem_dim_55=Size(5,5);

    Size str_elem_dim_11;
    str_elem_dim_11=Size(11,11);

    Mat rect_str_elem=getStructuringElement(MORPH_CROSS,str_elem_dim_55,anchor);

    dilate(img_edge,img_edge_dilated,rect_str_elem);
    //Mat mask;
    img_edge_dilated.convertTo(img_edge_dilated, CV_32S);
    Mat img_flood_fill=Mat::zeros( img_edge_dilated.rows + 2, img_edge_dilated.cols + 2, CV_8UC1 );
    Point origin=Point(2,24);  // is this have to be origin or (-1,-1) ?
    Rect rect;
    floodFill(img_edge_dilated,img_flood_fill,origin,Scalar(255),&rect,Scalar(20),Scalar(20),4);

    Mat img_openloops_removed;
    morphologyEx(img_flood_fill,img_openloops_removed,MORPH_OPEN,rect_str_elem);  // <------ confirm it

    Mat img_compliment;
    bitwise_not(img_openloops_removed,img_compliment);

    Mat img_eroded0,img_eroded1;
    Mat circ_str_elem=getStructuringElement(MORPH_ELLIPSE,str_elem_dim_55,anchor);
    erode(img_compliment,img_eroded0,circ_str_elem);
    erode(img_eroded0,img_eroded1,circ_str_elem);

    Mat img_flood_fill2=Mat::zeros( img_edge_dilated.rows + 2, img_edge_dilated.cols + 2, CV_8UC1 );
    img_eroded1.convertTo(img_eroded1,CV32S);
    floodFill(img_eroded1,img_flood_fill2,origin,Scalar(255),&rect,Scalar(20),Scalar(20),4);

      /// got an issue with arguments SCALAR
    //floodFill(img_edge_dilated,,Scalar(255,255,255),rect,Scalar(0),Scalar(0),4);   /// got an issue with arguments SCALAR
    //floodFill(img_edge_dilated,origin,Scalar(255));

    imshow("asdf",img_edge_dilated);
	waitKey(0);

    /*Mat mask;
    img_edge_dilated.convertTo(img_edge_dilated, CV_32S);*/
    //cvtColor(img_edge_dilated, mask, CV_BGR2GRAY);

	/*string ty =  type2str( img_edge_dilated.type() );
    cout<<ty<<endl;
*/

/*
    vector<vector<Point> > contours;
    vector<Vec4i> hierarchy;
    RNG rng(12345);

    findContours( img_edge_dilated, contours, hierarchy, CV_RETR_FLOODFILL, CV_CHAIN_APPROX_SIMPLE, Point(0, 0) );
    Mat drawing = Mat::zeros( img_edge_dilated.size(), CV_32SC1 );
    for( int i = 0; i< contours.size(); i++ )
    {
        Scalar color = Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) );
        drawContours( drawing, contours, i, color, 2, 8, hierarchy, 0, Point() );
    }

    imshow( "Result window", drawing );
    waitKey(0);
*/

    //= Mat::zeros( img_edge_dilated.rows + 2, img_edge_dilated.cols + 2, CV_8UC1 );

    imshow("asdf",img_openloops_removed);
	waitKey(0);
/*
    Mat img_small_closedloops_removed;
	morphologyEx(img_openloops_removed,img_small_closedloops_removed,MORPH_CLOSE,rect_str_elem);  // comfirm it

	Mat img_erode_post_fill1,img_erode_post_erode;

	erode(img_small_closedloops_removed,img_erode_post_fill1,circ_str_elem);
	erode(img_erode_post_fill1,img_erode_post_erode,circ_str_elem);

	Mat img_final_filled=Mat::zeros( img_erode_post_erode.rows + 2, img_erode_post_erode.cols + 2, CV_8UC1 );
    //Point origin=Point(2,24);  // is this have to be origin or (-1,-1) ?
    //ect rect;

	floodFill(img_erode_post_erode,img_final_filled,origin,Scalar(255),&rect,Scalar(20),Scalar(20),4);   /// got an issue with arguments SCALAR
	//floodFill(img_erode_post_erode,origin,Scalar(255,0,0),rect,Scalar(),Scalar(),4);   /// got an issue with arguments SCALAR
	//floodFill(img_erode_post_erode,origin,Scalar(255));
	//imshow("asdf",img_final_filled);
	//waitKey(0);
*/
////////////////////////////////////////////////////////////////////////////
/*
	int connected_components_count;

	Mat img_label;
	connected_components_count=onnectedComponents(img_final_filled,img_label,8,int ltype=cv_32s);

	Mat label;
	label = // add matlab labelmatrix function equivalent of opencv

	Mat final_color_segmented;
	applyColorMap(label,final_color_segmented, COLORMAP_JET);*/

	return 0;
}
<commit_msg>all mat functions mapped<commit_after>#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/imgcodecs.hpp>
#include <iostream>
#include <math.h>
#include <stdio.h>

using namespace cv;
using namespace std;

 /*sxlp;'
 '
string type2str(int type) {
  string r;

  uchar depth = type & CV_MAT_DEPTH_MASK;
  uchar chans = 1 + (type >> CV_CN_SHIFT);

  switch ( depth ) {
    case CV_8U:  r = "8U"; break;
    case CV_8S:  r = "8S"; break;
    case CV_16U: r = "16U"; break;
    case CV_16S: r = "16S"; break;
    case CV_32S: r = "32S"; break;
    case CV_32F: r = "32F"; break;
    case CV_64F: r = "64F"; break;
    default:     r = "User"; break;
  }

  r += "C";
  r += (chans+'0');

  return r;
}
*/

int main()
{
	Mat img_bw=imread("tumtum.jpg", 0); // reading the rgb image in Mat data struct

/*
note: we may have to either pre process the image or not work with the custom bgr to greyscale or both
	  as they might not be able to detect edge very well
*/

	Mat grad_x, grad_y;    // derivative along x and y direction respectively
    Mat square_grad_x, square_grad_y;   // absoute value of derivative

    /// Gradient  along X direction
	Sobel( img_bw, grad_x,CV_16S, 1, 0, 3, 1,0, BORDER_DEFAULT );
	/// Gradient  along y direction
	Sobel( img_bw, grad_y,CV_16S, 0, 1, 3, 1, 0, BORDER_DEFAULT );

	square_grad_y=grad_y.mul(grad_y);
	square_grad_x=grad_x.mul(grad_x);

	Mat img_edge;

    addWeighted( square_grad_x,1, square_grad_y, 0.5, 0,img_edge);    // more weight is provided in the horizontal direction for more importance to lane detection

    Mat img_edge_dilated;
    Point anchor=Point(-1,-1);

    Size str_elem_dim_55;
    str_elem_dim_55=Size(5,5);

    Size str_elem_dim_11;
    str_elem_dim_11=Size(11,11);

    Mat rect_str_elem=getStructuringElement(MORPH_CROSS,str_elem_dim_55,anchor);

    dilate(img_edge,img_edge_dilated,rect_str_elem);
    //Mat mask;
    img_edge_dilated.convertTo(img_edge_dilated, CV_32S);
    Mat img_flood_fill=Mat::zeros( img_edge_dilated.rows + 2, img_edge_dilated.cols + 2, CV_8UC1 );
    Point origin=Point(2,24);  // is this have to be origin or (-1,-1) ?
    Rect rect;
    floodFill(img_edge_dilated,img_flood_fill,origin,Scalar(255),&rect,Scalar(20),Scalar(20),4);

    Mat img_openloops_removed;
    morphologyEx(img_flood_fill,img_openloops_removed,MORPH_OPEN,rect_str_elem);  // <------ confirm it

    Mat img_compliment;
    bitwise_not(img_openloops_removed,img_compliment);

    Mat img_eroded0,img_eroded1;
    Mat circ_str_elem=getStructuringElement(MORPH_ELLIPSE,str_elem_dim_55,anchor);
    erode(img_compliment,img_eroded0,circ_str_elem);
    erode(img_eroded0,img_eroded1,circ_str_elem);

    Mat img_flood_fill2=Mat::zeros( img_edge_dilated.rows + 2, img_edge_dilated.cols + 2, CV_8UC1 );
    img_eroded1.convertTo(img_eroded1,CV32S);
    floodFill(img_eroded1,img_flood_fill2,origin,Scalar(255),&rect,Scalar(20),Scalar(20),4);

    Mat labelImage(img_flood_fill2.size(), CV_8UC1);
    int nLabels = connectedComponents(img_flood_fill2, labelImage, 8);

    Mat img_color_coded;
    applyColorMap(labelImage,img_color_coded,COLORMAP_JET);

      /// got an issue with arguments SCALAR
    //floodFill(img_edge_dilated,,Scalar(255,255,255),rect,Scalar(0),Scalar(0),4);   /// got an issue with arguments SCALAR
    //floodFill(img_edge_dilated,origin,Scalar(255));

    imshow("asdf",img_edge_dilated);
	waitKey(0);

    /*Mat mask;
    img_edge_dilated.convertTo(img_edge_dilated, CV_32S);*/
    //cvtColor(img_edge_dilated, mask, CV_BGR2GRAY);

	/*string ty =  type2str( img_edge_dilated.type() );
    cout<<ty<<endl;
*/

/*
    vector<vector<Point> > contours;
    vector<Vec4i> hierarchy;
    RNG rng(12345);

    findContours( img_edge_dilated, contours, hierarchy, CV_RETR_FLOODFILL, CV_CHAIN_APPROX_SIMPLE, Point(0, 0) );
    Mat drawing = Mat::zeros( img_edge_dilated.size(), CV_32SC1 );
    for( int i = 0; i< contours.size(); i++ )
    {
        Scalar color = Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) );
        drawContours( drawing, contours, i, color, 2, 8, hierarchy, 0, Point() );
    }

    imshow( "Result window", drawing );
    waitKey(0);
*/

    //= Mat::zeros( img_edge_dilated.rows + 2, img_edge_dilated.cols + 2, CV_8UC1 );

    imshow("asdf",img_openloops_removed);
	waitKey(0);
/*
    Mat img_small_closedloops_removed;
	morphologyEx(img_openloops_removed,img_small_closedloops_removed,MORPH_CLOSE,rect_str_elem);  // comfirm it

	Mat img_erode_post_fill1,img_erode_post_erode;

	erode(img_small_closedloops_removed,img_erode_post_fill1,circ_str_elem);
	erode(img_erode_post_fill1,img_erode_post_erode,circ_str_elem);

	Mat img_final_filled=Mat::zeros( img_erode_post_erode.rows + 2, img_erode_post_erode.cols + 2, CV_8UC1 );
    //Point origin=Point(2,24);  // is this have to be origin or (-1,-1) ?
    //ect rect;

	floodFill(img_erode_post_erode,img_final_filled,origin,Scalar(255),&rect,Scalar(20),Scalar(20),4);   /// got an issue with arguments SCALAR
	//floodFill(img_erode_post_erode,origin,Scalar(255,0,0),rect,Scalar(),Scalar(),4);   /// got an issue with arguments SCALAR
	//floodFill(img_erode_post_erode,origin,Scalar(255));
	//imshow("asdf",img_final_filled);
	//waitKey(0);
*/
////////////////////////////////////////////////////////////////////////////
/*
	int connected_components_count;

	Mat img_label;
	connected_components_count=onnectedComponents(img_final_filled,img_label,8,int ltype=cv_32s);

	Mat label;
	label = // add matlab labelmatrix function equivalent of opencv

	Mat final_color_segmented;
	applyColorMap(label,final_color_segmented, COLORMAP_JET);*/

	return 0;
}
<|endoftext|>


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

  Program:   Insight Segmentation & Registration Toolkit
  Module:    itkKdTreeTest1.cxx
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

  Copyright (c) Insight Software Consortium. All rights reserved.
  See ITKCopyright.txt or http://www.itk.org/HTML/Copyright.htm 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

#include "itkVector.h"
#include "itkMersenneTwisterRandomVariateGenerator.h"
#include "itkListSample.h"
#include "itkKdTree.h"
#include "itkKdTreeGenerator.h"
#include "itkEuclideanDistance.h"
#include <fstream>

int itkKdTreeTest1(int argc , char * argv [] )
{
  if( argc < 5 )
    {
    std::cerr << "Missing parameters" << std::endl;
    std::cerr << "Usage: " << std::endl;
    std::cerr << argv[0] << "numberOfDataPoints numberOfTestPoints bucketSize graphvizDotOutputFile" << std::endl;
    return EXIT_FAILURE;
    }

  // Random number generator
  typedef itk::Statistics::MersenneTwisterRandomVariateGenerator NumberGeneratorType;

  NumberGeneratorType::Pointer randomNumberGenerator = NumberGeneratorType::New();
  randomNumberGenerator->Initialize();

  typedef itk::Array< double > MeasurementVectorType ;
  typedef itk::Statistics::ListSample< MeasurementVectorType > SampleType ;

  const SampleType::MeasurementVectorSizeType measurementVectorSize = 2;

  SampleType::Pointer sample = SampleType::New() ;
  sample->SetMeasurementVectorSize( measurementVectorSize );

  //
  // Generate a sample of random points
  //
  const unsigned int numberOfDataPoints = atoi( argv[1] );
  MeasurementVectorType mv( measurementVectorSize ) ;
  for (unsigned int i = 0 ; i < numberOfDataPoints ; ++i )
    {
    mv[0] = randomNumberGenerator->GetNormalVariate( 0.0, 1.0 );
    mv[1] = randomNumberGenerator->GetNormalVariate( 0.0, 1.0 );
    sample->PushBack( mv ) ;
    }

  typedef itk::Statistics::KdTreeGenerator< SampleType > TreeGeneratorType ;
  TreeGeneratorType::Pointer treeGenerator = TreeGeneratorType::New() ;

  const unsigned int bucketSize = atoi( argv[3] );

  treeGenerator->SetSample( sample ) ;
  treeGenerator->SetBucketSize( bucketSize );
  treeGenerator->Update() ;

  typedef TreeGeneratorType::KdTreeType TreeType ;
  typedef TreeType::NearestNeighbors NeighborsType ;
  typedef TreeType::KdTreeNodeType NodeType ;

  TreeType::Pointer tree = treeGenerator->GetOutput() ;

  MeasurementVectorType queryPoint( measurementVectorSize ) ;

  unsigned int numberOfNeighbors = 1 ;
  TreeType::InstanceIdentifierVectorType neighbors ;

  MeasurementVectorType result( measurementVectorSize ) ;
  MeasurementVectorType test_point( measurementVectorSize ) ;
  MeasurementVectorType min_point( measurementVectorSize ) ;

  unsigned int numberOfFailedPoints = 0;

  const unsigned int numberOfTestPoints = atoi( argv[2] );

  //
  // Generate a second sample of random points
  // and use them to query the tree
  //
  for (unsigned int i = 0 ; i < numberOfTestPoints ; ++i )
    {

    double min_dist = itk::NumericTraits< double >::max();

    queryPoint[0] = randomNumberGenerator->GetNormalVariate( 0.0, 1.0 );
    queryPoint[1] = randomNumberGenerator->GetNormalVariate( 0.0, 1.0 );

    tree->Search( queryPoint, numberOfNeighbors, neighbors ) ;

    //
    // The first neighbor should be the closest point.
    //
    result = tree->GetMeasurementVector( neighbors[0] );

    //
    // Compute the distance to the "presumed" nearest neighbor
    //
    double result_dist = sqrt(
          (result[0] - queryPoint[0]) *
          (result[0] - queryPoint[0]) +
          (result[1] - queryPoint[1]) *
          (result[1] - queryPoint[1])
          );

    //
    // Compute the distance to all other points, to verify
    // whether the first neighbor was the closest one or not.
    //
    for( unsigned int i = 0 ; i < numberOfDataPoints; ++i )
      {
      test_point = tree->GetMeasurementVector( i );

      const double dist = sqrt(
          (test_point[0] - queryPoint[0]) *
          (test_point[0] - queryPoint[0]) +
          (test_point[1] - queryPoint[1]) *
          (test_point[1] - queryPoint[1])
          );

      if( dist < min_dist )
        {
        min_dist = dist;
        min_point = test_point;
        }
      }

    if( min_dist < result_dist )
      {
      std::cerr << "Problem found " << std::endl;
      std::cerr << "Query point " << queryPoint << std::endl;
      std::cerr << "Reported closest point " << result
                << " distance " << result_dist << std::endl;
      std::cerr << "Actual   closest point " << min_point
                << " distance " << min_dist << std::endl;
      std::cerr << std::endl;
      std::cerr << "Test FAILED." << std::endl;
      numberOfFailedPoints++;
      }

    }

  //
  // Plot out the tree structure to the console in the format used by Graphviz dot
  //
  std::ofstream plotFile;
  plotFile.open( argv[2] );
  tree->PlotTree( plotFile );
  plotFile.close();

  if( numberOfFailedPoints )
    {
    std::cerr << numberOfFailedPoints << " failed out of "
              << numberOfTestPoints << " points " << std::endl;
    return EXIT_FAILURE;
    }

  std::cout << "Test PASSED." << std::endl;
  return EXIT_SUCCESS;
}
<commit_msg>BUG: Fixing the numbering of argv command line arguments.<commit_after>/*=========================================================================

  Program:   Insight Segmentation & Registration Toolkit
  Module:    itkKdTreeTest1.cxx
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

  Copyright (c) Insight Software Consortium. All rights reserved.
  See ITKCopyright.txt or http://www.itk.org/HTML/Copyright.htm 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

#include "itkVector.h"
#include "itkMersenneTwisterRandomVariateGenerator.h"
#include "itkListSample.h"
#include "itkKdTree.h"
#include "itkKdTreeGenerator.h"
#include "itkEuclideanDistance.h"
#include <fstream>

int itkKdTreeTest1(int argc , char * argv [] )
{
  if( argc < 5 )
    {
    std::cerr << "Missing parameters" << std::endl;
    std::cerr << "Usage: " << std::endl;
    std::cerr << argv[0] << " numberOfDataPoints numberOfTestPoints bucketSize graphvizDotOutputFile" << std::endl;
    return EXIT_FAILURE;
    }

  // Random number generator
  typedef itk::Statistics::MersenneTwisterRandomVariateGenerator NumberGeneratorType;

  NumberGeneratorType::Pointer randomNumberGenerator = NumberGeneratorType::New();
  randomNumberGenerator->Initialize();

  typedef itk::Array< double > MeasurementVectorType ;
  typedef itk::Statistics::ListSample< MeasurementVectorType > SampleType ;

  const SampleType::MeasurementVectorSizeType measurementVectorSize = 2;

  SampleType::Pointer sample = SampleType::New() ;
  sample->SetMeasurementVectorSize( measurementVectorSize );

  //
  // Generate a sample of random points
  //
  const unsigned int numberOfDataPoints = atoi( argv[1] );
  MeasurementVectorType mv( measurementVectorSize ) ;
  for (unsigned int i = 0 ; i < numberOfDataPoints ; ++i )
    {
    mv[0] = randomNumberGenerator->GetNormalVariate( 0.0, 1.0 );
    mv[1] = randomNumberGenerator->GetNormalVariate( 0.0, 1.0 );
    sample->PushBack( mv ) ;
    }

  typedef itk::Statistics::KdTreeGenerator< SampleType > TreeGeneratorType ;
  TreeGeneratorType::Pointer treeGenerator = TreeGeneratorType::New() ;

  const unsigned int bucketSize = atoi( argv[3] );

  treeGenerator->SetSample( sample ) ;
  treeGenerator->SetBucketSize( bucketSize );
  treeGenerator->Update() ;

  typedef TreeGeneratorType::KdTreeType TreeType ;
  typedef TreeType::NearestNeighbors NeighborsType ;
  typedef TreeType::KdTreeNodeType NodeType ;

  TreeType::Pointer tree = treeGenerator->GetOutput() ;

  MeasurementVectorType queryPoint( measurementVectorSize ) ;

  unsigned int numberOfNeighbors = 1 ;
  TreeType::InstanceIdentifierVectorType neighbors ;

  MeasurementVectorType result( measurementVectorSize ) ;
  MeasurementVectorType test_point( measurementVectorSize ) ;
  MeasurementVectorType min_point( measurementVectorSize ) ;

  unsigned int numberOfFailedPoints = 0;

  const unsigned int numberOfTestPoints = atoi( argv[2] );

  //
  // Generate a second sample of random points
  // and use them to query the tree
  //
  for (unsigned int i = 0 ; i < numberOfTestPoints ; ++i )
    {

    double min_dist = itk::NumericTraits< double >::max();

    queryPoint[0] = randomNumberGenerator->GetNormalVariate( 0.0, 1.0 );
    queryPoint[1] = randomNumberGenerator->GetNormalVariate( 0.0, 1.0 );

    tree->Search( queryPoint, numberOfNeighbors, neighbors ) ;

    //
    // The first neighbor should be the closest point.
    //
    result = tree->GetMeasurementVector( neighbors[0] );

    //
    // Compute the distance to the "presumed" nearest neighbor
    //
    double result_dist = sqrt(
          (result[0] - queryPoint[0]) *
          (result[0] - queryPoint[0]) +
          (result[1] - queryPoint[1]) *
          (result[1] - queryPoint[1])
          );

    //
    // Compute the distance to all other points, to verify
    // whether the first neighbor was the closest one or not.
    //
    for( unsigned int i = 0 ; i < numberOfDataPoints; ++i )
      {
      test_point = tree->GetMeasurementVector( i );

      const double dist = sqrt(
          (test_point[0] - queryPoint[0]) *
          (test_point[0] - queryPoint[0]) +
          (test_point[1] - queryPoint[1]) *
          (test_point[1] - queryPoint[1])
          );

      if( dist < min_dist )
        {
        min_dist = dist;
        min_point = test_point;
        }
      }

    if( min_dist < result_dist )
      {
      std::cerr << "Problem found " << std::endl;
      std::cerr << "Query point " << queryPoint << std::endl;
      std::cerr << "Reported closest point " << result
                << " distance " << result_dist << std::endl;
      std::cerr << "Actual   closest point " << min_point
                << " distance " << min_dist << std::endl;
      std::cerr << std::endl;
      std::cerr << "Test FAILED." << std::endl;
      numberOfFailedPoints++;
      }

    }

  //
  // Plot out the tree structure to the console in the format used by Graphviz dot
  //
  std::ofstream plotFile;
  plotFile.open( argv[4] );
  tree->PlotTree( plotFile );
  plotFile.close();

  if( numberOfFailedPoints )
    {
    std::cerr << numberOfFailedPoints << " failed out of "
              << numberOfTestPoints << " points " << std::endl;
    return EXIT_FAILURE;
    }

  std::cout << "Test PASSED." << std::endl;
  return EXIT_SUCCESS;
}
<|endoftext|>