<commit_before>/* Copyright (C) 2000-2006 MySQL AB

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA */

/**
  @file

  @brief
  Get hostname for an IP.

    Hostnames are checked with reverse name lookup and
    checked that they doesn't resemble an ip.
*/

#include "mysql_priv.h"
#include "hash_filo.h"
#include <m_ctype.h>
#ifdef	__cplusplus
extern "C" {					// Because of SCO 3.2V4.2
#endif
#if !defined( __WIN__)
#ifdef HAVE_SYS_UN_H
#include <sys/un.h>
#endif
#include <netdb.h>
#include <sys/utsname.h>
#endif // __WIN__
#ifdef	__cplusplus
}
#endif

class host_entry :public hash_filo_element
{
public:
  char	 ip[sizeof(((struct in_addr *) 0)->s_addr)];
  uint	 errors;
  char	 *hostname;
};

static hash_filo *hostname_cache;
static pthread_mutex_t LOCK_hostname;

void hostname_cache_refresh()
{
  hostname_cache->clear();
}

bool hostname_cache_init()
{
  host_entry tmp;
  uint offset= (uint) ((char*) (&tmp.ip) - (char*) &tmp);
  if (!(hostname_cache=new hash_filo(HOST_CACHE_SIZE, offset,
				     sizeof(struct in_addr),NULL,
				     (hash_free_key) free,
				     &my_charset_bin)))
    return 1;
  hostname_cache->clear();
  (void) pthread_mutex_init(&LOCK_hostname,MY_MUTEX_INIT_SLOW);
  return 0;
}

void hostname_cache_free()
{
  if (hostname_cache)
  {
    (void) pthread_mutex_destroy(&LOCK_hostname);
    delete hostname_cache;
    hostname_cache= 0;
  }
}

static void add_hostname(struct in_addr *in,const char *name)
{
  if (!(specialflag & SPECIAL_NO_HOST_CACHE))
  {
    VOID(pthread_mutex_lock(&hostname_cache->lock));
    host_entry *entry;
    if (!(entry=(host_entry*) hostname_cache->search((uchar*) &in->s_addr,0)))
    {
      uint length=name ? (uint) strlen(name) : 0;

      if ((entry=(host_entry*) malloc(sizeof(host_entry)+length+1)))
      {
	char *new_name;
	memcpy_fixed(&entry->ip, &in->s_addr, sizeof(in->s_addr));
	if (length)
	  memcpy(new_name= (char *) (entry+1), name, length+1);
	else
	  new_name=0;
	entry->hostname=new_name;
	entry->errors=0;
	(void) hostname_cache->add(entry);
      }
    }
    VOID(pthread_mutex_unlock(&hostname_cache->lock));
  }
}

inline void add_wrong_ip(struct in_addr *in)
{
  add_hostname(in,NullS);
}

void inc_host_errors(struct in_addr *in)
{
  VOID(pthread_mutex_lock(&hostname_cache->lock));
  host_entry *entry;
  if ((entry=(host_entry*) hostname_cache->search((uchar*) &in->s_addr,0)))
    entry->errors++;
  VOID(pthread_mutex_unlock(&hostname_cache->lock));
}

void reset_host_errors(struct in_addr *in)
{
  VOID(pthread_mutex_lock(&hostname_cache->lock));
  host_entry *entry;
  if ((entry=(host_entry*) hostname_cache->search((uchar*) &in->s_addr,0)))
    entry->errors=0;
  VOID(pthread_mutex_unlock(&hostname_cache->lock));
}

/* Deal with systems that don't defined INADDR_LOOPBACK */
#ifndef INADDR_LOOPBACK
#define INADDR_LOOPBACK 0x7f000001UL
#endif

char * ip_to_hostname(struct in_addr *in, uint *errors)
{
  uint i;
  host_entry *entry;
  DBUG_ENTER("ip_to_hostname");
  *errors=0;

  /* We always treat the loopback address as "localhost". */
  if (in->s_addr == htonl(INADDR_LOOPBACK))   // is expanded inline by gcc
    DBUG_RETURN((char *)my_localhost);

  /* Check first if we have name in cache */
  if (!(specialflag & SPECIAL_NO_HOST_CACHE))
  {
    VOID(pthread_mutex_lock(&hostname_cache->lock));
    if ((entry=(host_entry*) hostname_cache->search((uchar*) &in->s_addr,0)))
    {
      char *name;
      if (!entry->hostname)
	name=0;					// Don't allow connection
      else
	name=my_strdup(entry->hostname,MYF(0));
      *errors= entry->errors;
      VOID(pthread_mutex_unlock(&hostname_cache->lock));
      DBUG_RETURN(name);
    }
    VOID(pthread_mutex_unlock(&hostname_cache->lock));
  }

  struct hostent *hp, *check;
  char *name;
  LINT_INIT(check);
#if defined(HAVE_GETHOSTBYADDR_R) && defined(HAVE_SOLARIS_STYLE_GETHOST)
  char buff[GETHOSTBYADDR_BUFF_SIZE],buff2[GETHOSTBYNAME_BUFF_SIZE];
  int tmp_errno;
  struct hostent tmp_hostent, tmp_hostent2;
#ifdef HAVE_purify
  bzero(buff,sizeof(buff));		// Bug in purify
#endif
  if (!(hp=gethostbyaddr_r((char*) in,sizeof(*in),
			   AF_INET,
			   &tmp_hostent,buff,sizeof(buff),&tmp_errno)))
  {
    DBUG_PRINT("error",("gethostbyaddr_r returned %d",tmp_errno));
    return 0;
  }
  if (!(check=my_gethostbyname_r(hp->h_name,&tmp_hostent2,buff2,sizeof(buff2),
				 &tmp_errno)))
  {
    DBUG_PRINT("error",("gethostbyname_r returned %d",tmp_errno));
    /*
      Don't cache responses when the DSN server is down, as otherwise
      transient DNS failure may leave any number of clients (those
      that attempted to connect during the outage) unable to connect
      indefinitely.
    */
    if (tmp_errno == HOST_NOT_FOUND || tmp_errno == NO_DATA)
      add_wrong_ip(in);
    my_gethostbyname_r_free();
    DBUG_RETURN(0);
  }
  if (!hp->h_name[0])
  {
    DBUG_PRINT("error",("Got an empty hostname"));
    add_wrong_ip(in);
    my_gethostbyname_r_free();
    DBUG_RETURN(0);				// Don't allow empty hostnames
  }
  if (!(name=my_strdup(hp->h_name,MYF(0))))
  {
    my_gethostbyname_r_free();
    DBUG_RETURN(0);				// out of memory
  }
  my_gethostbyname_r_free();
#else
  VOID(pthread_mutex_lock(&LOCK_hostname));
  if (!(hp=gethostbyaddr((char*) in,sizeof(*in), AF_INET)))
  {
    VOID(pthread_mutex_unlock(&LOCK_hostname));
    DBUG_PRINT("error",("gethostbyaddr returned %d",errno));

    if (errno == HOST_NOT_FOUND || errno == NO_DATA)
      goto add_wrong_ip_and_return;
    /* Failure, don't cache responce */
    DBUG_RETURN(0);
  }
  if (!hp->h_name[0])				// Don't allow empty hostnames
  {
    VOID(pthread_mutex_unlock(&LOCK_hostname));
    DBUG_PRINT("error",("Got an empty hostname"));
    goto add_wrong_ip_and_return;
  }
  if (!(name=my_strdup(hp->h_name,MYF(0))))
  {
    VOID(pthread_mutex_unlock(&LOCK_hostname));
    DBUG_RETURN(0);				// out of memory
  }
  check=gethostbyname(name);
  VOID(pthread_mutex_unlock(&LOCK_hostname));
  if (!check)
  {
    DBUG_PRINT("error",("gethostbyname returned %d",errno));
    my_free(name,MYF(0));
    DBUG_RETURN(0);
  }
#endif

  /* Don't accept hostnames that starts with digits because they may be
     false ip:s */
  if (my_isdigit(&my_charset_latin1,name[0]))
  {
    char *pos;
    for (pos= name+1 ; my_isdigit(&my_charset_latin1,*pos); pos++) ;
    if (*pos == '.')
    {
      DBUG_PRINT("error",("mysqld doesn't accept hostnames that starts with a number followed by a '.'"));
      my_free(name,MYF(0));
      goto add_wrong_ip_and_return;
    }
  }

  /* Check that 'gethostbyname' returned the used ip */
  for (i=0; check->h_addr_list[i]; i++)
  {
    if (*(uint32*)(check->h_addr_list)[i] == in->s_addr)
    {
      add_hostname(in,name);
      DBUG_RETURN(name);
    }
  }
  DBUG_PRINT("error",("Couldn't verify hostname with gethostbyname"));
  my_free(name,MYF(0));

add_wrong_ip_and_return:
  add_wrong_ip(in);
  DBUG_RETURN(0);
}
<commit_msg>BUG#12377872 ASSERTION FAILED: !_ENTERED WHEN GETHOSTBYADDR_R              FAILS ON SOLARIS<commit_after>/* Copyright (c) 2000, 2011, Oracle and/or its affiliates. All rights reserved.

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA */

/**
  @file

  @brief
  Get hostname for an IP.

    Hostnames are checked with reverse name lookup and
    checked that they doesn't resemble an ip.
*/

#include "mysql_priv.h"
#include "hash_filo.h"
#include <m_ctype.h>
#ifdef	__cplusplus
extern "C" {					// Because of SCO 3.2V4.2
#endif
#if !defined( __WIN__)
#ifdef HAVE_SYS_UN_H
#include <sys/un.h>
#endif
#include <netdb.h>
#include <sys/utsname.h>
#endif // __WIN__
#ifdef	__cplusplus
}
#endif

class host_entry :public hash_filo_element
{
public:
  char	 ip[sizeof(((struct in_addr *) 0)->s_addr)];
  uint	 errors;
  char	 *hostname;
};

static hash_filo *hostname_cache;
static pthread_mutex_t LOCK_hostname;

void hostname_cache_refresh()
{
  hostname_cache->clear();
}

bool hostname_cache_init()
{
  host_entry tmp;
  uint offset= (uint) ((char*) (&tmp.ip) - (char*) &tmp);
  if (!(hostname_cache=new hash_filo(HOST_CACHE_SIZE, offset,
				     sizeof(struct in_addr),NULL,
				     (hash_free_key) free,
				     &my_charset_bin)))
    return 1;
  hostname_cache->clear();
  (void) pthread_mutex_init(&LOCK_hostname,MY_MUTEX_INIT_SLOW);
  return 0;
}

void hostname_cache_free()
{
  if (hostname_cache)
  {
    (void) pthread_mutex_destroy(&LOCK_hostname);
    delete hostname_cache;
    hostname_cache= 0;
  }
}

static void add_hostname(struct in_addr *in,const char *name)
{
  if (!(specialflag & SPECIAL_NO_HOST_CACHE))
  {
    VOID(pthread_mutex_lock(&hostname_cache->lock));
    host_entry *entry;
    if (!(entry=(host_entry*) hostname_cache->search((uchar*) &in->s_addr,0)))
    {
      uint length=name ? (uint) strlen(name) : 0;

      if ((entry=(host_entry*) malloc(sizeof(host_entry)+length+1)))
      {
	char *new_name;
	memcpy_fixed(&entry->ip, &in->s_addr, sizeof(in->s_addr));
	if (length)
	  memcpy(new_name= (char *) (entry+1), name, length+1);
	else
	  new_name=0;
	entry->hostname=new_name;
	entry->errors=0;
	(void) hostname_cache->add(entry);
      }
    }
    VOID(pthread_mutex_unlock(&hostname_cache->lock));
  }
}

inline void add_wrong_ip(struct in_addr *in)
{
  add_hostname(in,NullS);
}

void inc_host_errors(struct in_addr *in)
{
  VOID(pthread_mutex_lock(&hostname_cache->lock));
  host_entry *entry;
  if ((entry=(host_entry*) hostname_cache->search((uchar*) &in->s_addr,0)))
    entry->errors++;
  VOID(pthread_mutex_unlock(&hostname_cache->lock));
}

void reset_host_errors(struct in_addr *in)
{
  VOID(pthread_mutex_lock(&hostname_cache->lock));
  host_entry *entry;
  if ((entry=(host_entry*) hostname_cache->search((uchar*) &in->s_addr,0)))
    entry->errors=0;
  VOID(pthread_mutex_unlock(&hostname_cache->lock));
}

/* Deal with systems that don't defined INADDR_LOOPBACK */
#ifndef INADDR_LOOPBACK
#define INADDR_LOOPBACK 0x7f000001UL
#endif

char * ip_to_hostname(struct in_addr *in, uint *errors)
{
  uint i;
  host_entry *entry;
  DBUG_ENTER("ip_to_hostname");
  *errors=0;

  /* We always treat the loopback address as "localhost". */
  if (in->s_addr == htonl(INADDR_LOOPBACK))   // is expanded inline by gcc
    DBUG_RETURN((char *)my_localhost);

  /* Check first if we have name in cache */
  if (!(specialflag & SPECIAL_NO_HOST_CACHE))
  {
    VOID(pthread_mutex_lock(&hostname_cache->lock));
    if ((entry=(host_entry*) hostname_cache->search((uchar*) &in->s_addr,0)))
    {
      char *name;
      if (!entry->hostname)
	name=0;					// Don't allow connection
      else
	name=my_strdup(entry->hostname,MYF(0));
      *errors= entry->errors;
      VOID(pthread_mutex_unlock(&hostname_cache->lock));
      DBUG_RETURN(name);
    }
    VOID(pthread_mutex_unlock(&hostname_cache->lock));
  }

  struct hostent *hp, *check;
  char *name;
  LINT_INIT(check);
#if defined(HAVE_GETHOSTBYADDR_R) && defined(HAVE_SOLARIS_STYLE_GETHOST)
  char buff[GETHOSTBYADDR_BUFF_SIZE],buff2[GETHOSTBYNAME_BUFF_SIZE];
  int tmp_errno;
  struct hostent tmp_hostent, tmp_hostent2;
#ifdef HAVE_purify
  bzero(buff,sizeof(buff));		// Bug in purify
#endif
  if (!(hp=gethostbyaddr_r((char*) in,sizeof(*in),
			   AF_INET,
			   &tmp_hostent,buff,sizeof(buff),&tmp_errno)))
  {
    DBUG_PRINT("error",("gethostbyaddr_r returned %d",tmp_errno));
    DBUG_RETURN(0);
  }
  if (!(check=my_gethostbyname_r(hp->h_name,&tmp_hostent2,buff2,sizeof(buff2),
				 &tmp_errno)))
  {
    DBUG_PRINT("error",("gethostbyname_r returned %d",tmp_errno));
    /*
      Don't cache responses when the DSN server is down, as otherwise
      transient DNS failure may leave any number of clients (those
      that attempted to connect during the outage) unable to connect
      indefinitely.
    */
    if (tmp_errno == HOST_NOT_FOUND || tmp_errno == NO_DATA)
      add_wrong_ip(in);
    my_gethostbyname_r_free();
    DBUG_RETURN(0);
  }
  if (!hp->h_name[0])
  {
    DBUG_PRINT("error",("Got an empty hostname"));
    add_wrong_ip(in);
    my_gethostbyname_r_free();
    DBUG_RETURN(0);				// Don't allow empty hostnames
  }
  if (!(name=my_strdup(hp->h_name,MYF(0))))
  {
    my_gethostbyname_r_free();
    DBUG_RETURN(0);				// out of memory
  }
  my_gethostbyname_r_free();
#else
  VOID(pthread_mutex_lock(&LOCK_hostname));
  if (!(hp=gethostbyaddr((char*) in,sizeof(*in), AF_INET)))
  {
    VOID(pthread_mutex_unlock(&LOCK_hostname));
    DBUG_PRINT("error",("gethostbyaddr returned %d",errno));

    if (errno == HOST_NOT_FOUND || errno == NO_DATA)
      goto add_wrong_ip_and_return;
    /* Failure, don't cache responce */
    DBUG_RETURN(0);
  }
  if (!hp->h_name[0])				// Don't allow empty hostnames
  {
    VOID(pthread_mutex_unlock(&LOCK_hostname));
    DBUG_PRINT("error",("Got an empty hostname"));
    goto add_wrong_ip_and_return;
  }
  if (!(name=my_strdup(hp->h_name,MYF(0))))
  {
    VOID(pthread_mutex_unlock(&LOCK_hostname));
    DBUG_RETURN(0);				// out of memory
  }
  check=gethostbyname(name);
  VOID(pthread_mutex_unlock(&LOCK_hostname));
  if (!check)
  {
    DBUG_PRINT("error",("gethostbyname returned %d",errno));
    my_free(name,MYF(0));
    DBUG_RETURN(0);
  }
#endif

  /* Don't accept hostnames that starts with digits because they may be
     false ip:s */
  if (my_isdigit(&my_charset_latin1,name[0]))
  {
    char *pos;
    for (pos= name+1 ; my_isdigit(&my_charset_latin1,*pos); pos++) ;
    if (*pos == '.')
    {
      DBUG_PRINT("error",("mysqld doesn't accept hostnames that starts with a number followed by a '.'"));
      my_free(name,MYF(0));
      goto add_wrong_ip_and_return;
    }
  }

  /* Check that 'gethostbyname' returned the used ip */
  for (i=0; check->h_addr_list[i]; i++)
  {
    if (*(uint32*)(check->h_addr_list)[i] == in->s_addr)
    {
      add_hostname(in,name);
      DBUG_RETURN(name);
    }
  }
  DBUG_PRINT("error",("Couldn't verify hostname with gethostbyname"));
  my_free(name,MYF(0));

add_wrong_ip_and_return:
  add_wrong_ip(in);
  DBUG_RETURN(0);
}
<|endoftext|>


<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-2014 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 "OgreLog.h"
#include <iomanip>
#include <iostream>

#if OGRE_PLATFORM == OGRE_PLATFORM_WIN32 || OGRE_PLATFORM == OGRE_PLATFORM_WINRT
#   include <windows.h>
#endif

#if OGRE_PLATFORM == OGRE_PLATFORM_NACL
#   include "ppapi/cpp/var.h"
#   include "ppapi/cpp/instance.h"
#endif

namespace {
    const char* RED = "\x1b[31;1m";
    const char* YELLOW = "\x1b[33;1m";
    const char* RESET = "\x1b[0m";
}

namespace Ogre
{
#if OGRE_PLATFORM == OGRE_PLATFORM_NACL
    pp::Instance* Log::mInstance = NULL;
#endif

    //-----------------------------------------------------------------------
    Log::Log( const String& name, bool debuggerOuput, bool suppressFile ) :
        mLogLevel(LL_NORMAL), mDebugOut(debuggerOuput),
        mSuppressFile(suppressFile), mTimeStamp(true), mLogName(name), mTermHasColours(false)
    {
        if (!mSuppressFile)
        {
            mLog.open(name.c_str());
        }

#if OGRE_PLATFORM != OGRE_PLATFORM_WINRT
        if(mDebugOut)
        {
            char* val = getenv("TERM");
            mTermHasColours = val && String(val).find("xterm") != String::npos;
        }
#endif
    }
    //-----------------------------------------------------------------------
    Log::~Log()
    {
        OGRE_LOCK_AUTO_MUTEX;
        if (!mSuppressFile)
        {
            mLog.close();
        }
    }

    //-----------------------------------------------------------------------
    void Log::logMessage( const String& message, LogMessageLevel lml, bool maskDebug )
    {
        OGRE_LOCK_AUTO_MUTEX;
        if ((mLogLevel + lml) >= OGRE_LOG_THRESHOLD)
        {
            bool skipThisMessage = false;
            for( mtLogListener::iterator i = mListeners.begin(); i != mListeners.end(); ++i )
                (*i)->messageLogged( message, lml, maskDebug, mLogName, skipThisMessage);

            if (!skipThisMessage)
            {
#if OGRE_PLATFORM == OGRE_PLATFORM_NACL
                if(mInstance != NULL)
                {
                    mInstance->PostMessage(message.c_str());
                }
#else
                if (mDebugOut && !maskDebug)
                {
#    if (OGRE_PLATFORM == OGRE_PLATFORM_WIN32 || OGRE_PLATFORM == OGRE_PLATFORM_WINRT) && OGRE_DEBUG_MODE
                    OutputDebugStringA("Ogre: ");
                    OutputDebugStringA(message.c_str());
                    OutputDebugStringA("\n");
#    endif

                    std::ostream& os = int(lml) >= int(LML_WARNING) ? std::cerr : std::cout;

                    if(mTermHasColours) {
                        if(lml == LML_WARNING)
                            os << YELLOW;
                        if(lml == LML_CRITICAL)
                            os << RED;
                    }

                    os << message;

                    if(mTermHasColours) {
                        os << RESET;
                    }

                    os << std::endl;
                }
#endif

                // Write time into log
                if (!mSuppressFile)
                {
                    if (mTimeStamp)
                    {
                        struct tm *pTime;
                        time_t ctTime; time(&ctTime);
                        pTime = localtime( &ctTime );
                        mLog << std::setw(2) << std::setfill('0') << pTime->tm_hour
                            << ":" << std::setw(2) << std::setfill('0') << pTime->tm_min
                            << ":" << std::setw(2) << std::setfill('0') << pTime->tm_sec
                            << ": ";
                    }
                    mLog << message << std::endl;

                    // Flush stcmdream to ensure it is written (incase of a crash, we need log to be up to date)
                    mLog.flush();
                }
            }
        }
    }

    //-----------------------------------------------------------------------
    void Log::setTimeStampEnabled(bool timeStamp)
    {
        OGRE_LOCK_AUTO_MUTEX;
        mTimeStamp = timeStamp;
    }

    //-----------------------------------------------------------------------
    void Log::setDebugOutputEnabled(bool debugOutput)
    {
        OGRE_LOCK_AUTO_MUTEX;
        mDebugOut = debugOutput;
    }

    //-----------------------------------------------------------------------
    void Log::setLogDetail(LoggingLevel ll)
    {
        OGRE_LOCK_AUTO_MUTEX;
        mLogLevel = ll;
    }

    //-----------------------------------------------------------------------
    void Log::addListener(LogListener* listener)
    {
        OGRE_LOCK_AUTO_MUTEX;
        if (std::find(mListeners.begin(), mListeners.end(), listener) == mListeners.end())
            mListeners.push_back(listener);
    }

    //-----------------------------------------------------------------------
    void Log::removeListener(LogListener* listener)
    {
        OGRE_LOCK_AUTO_MUTEX;
        mtLogListener::iterator i = std::find(mListeners.begin(), mListeners.end(), listener);
        if (i != mListeners.end())
            mListeners.erase(i);
    }
    //---------------------------------------------------------------------
    Log::Stream Log::stream(LogMessageLevel lml, bool maskDebug)
    {
        return Stream(this, lml, maskDebug);

    }
}
<commit_msg>[Win32] Register Ogre log files to be collected by Windows Error Reporting and eventually passed to developer (would not happen under debugger)<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-2014 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 "OgreLog.h"
#include <iomanip>
#include <iostream>

#if OGRE_PLATFORM == OGRE_PLATFORM_WIN32 || OGRE_PLATFORM == OGRE_PLATFORM_WINRT
#   include <windows.h>
#   if _WIN32_WINNT >= _WIN32_WINNT_VISTA
#       include <werapi.h>
#   endif
#endif

#if OGRE_PLATFORM == OGRE_PLATFORM_NACL
#   include "ppapi/cpp/var.h"
#   include "ppapi/cpp/instance.h"
#endif

namespace {
    const char* RED = "\x1b[31;1m";
    const char* YELLOW = "\x1b[33;1m";
    const char* RESET = "\x1b[0m";
}

namespace Ogre
{
#if OGRE_PLATFORM == OGRE_PLATFORM_NACL
    pp::Instance* Log::mInstance = NULL;
#endif

    //-----------------------------------------------------------------------
    Log::Log( const String& name, bool debuggerOutput, bool suppressFile ) :
        mLogLevel(LL_NORMAL), mDebugOut(debuggerOutput),
        mSuppressFile(suppressFile), mTimeStamp(true), mLogName(name), mTermHasColours(false)
    {
        if (!mSuppressFile)
        {
            mLog.open(name.c_str());

#if (OGRE_PLATFORM == OGRE_PLATFORM_WIN32 || OGRE_PLATFORM == OGRE_PLATFORM_WINRT) && _WIN32_WINNT >= _WIN32_WINNT_VISTA
            // Register log file to be collected by Windows Error Reporting
            const int utf16Length = ::MultiByteToWideChar(CP_ACP, 0, name.c_str(), (int)name.size(), NULL, 0);
            if(utf16Length > 0)
            {
                std::wstring wname;
                wname.resize(utf16Length);
                if (0 != ::MultiByteToWideChar(CP_ACP, 0, name.c_str(), (int)name.size(), &wname[0], (int)wname.size()))
                    WerRegisterFile(wname.c_str(), WerRegFileTypeOther, WER_FILE_ANONYMOUS_DATA);
            }
#endif
        }

#if OGRE_PLATFORM != OGRE_PLATFORM_WINRT
        if(mDebugOut)
        {
            char* val = getenv("TERM");
            mTermHasColours = val && String(val).find("xterm") != String::npos;
        }
#endif
    }
    //-----------------------------------------------------------------------
    Log::~Log()
    {
        OGRE_LOCK_AUTO_MUTEX;
        if (!mSuppressFile)
        {
            mLog.close();
        }
    }

    //-----------------------------------------------------------------------
    void Log::logMessage( const String& message, LogMessageLevel lml, bool maskDebug )
    {
        OGRE_LOCK_AUTO_MUTEX;
        if ((mLogLevel + lml) >= OGRE_LOG_THRESHOLD)
        {
            bool skipThisMessage = false;
            for( mtLogListener::iterator i = mListeners.begin(); i != mListeners.end(); ++i )
                (*i)->messageLogged( message, lml, maskDebug, mLogName, skipThisMessage);

            if (!skipThisMessage)
            {
#if OGRE_PLATFORM == OGRE_PLATFORM_NACL
                if(mInstance != NULL)
                {
                    mInstance->PostMessage(message.c_str());
                }
#else
                if (mDebugOut && !maskDebug)
                {
#    if (OGRE_PLATFORM == OGRE_PLATFORM_WIN32 || OGRE_PLATFORM == OGRE_PLATFORM_WINRT) && OGRE_DEBUG_MODE
                    OutputDebugStringA("Ogre: ");
                    OutputDebugStringA(message.c_str());
                    OutputDebugStringA("\n");
#    endif

                    std::ostream& os = int(lml) >= int(LML_WARNING) ? std::cerr : std::cout;

                    if(mTermHasColours) {
                        if(lml == LML_WARNING)
                            os << YELLOW;
                        if(lml == LML_CRITICAL)
                            os << RED;
                    }

                    os << message;

                    if(mTermHasColours) {
                        os << RESET;
                    }

                    os << std::endl;
                }
#endif

                // Write time into log
                if (!mSuppressFile)
                {
                    if (mTimeStamp)
                    {
                        struct tm *pTime;
                        time_t ctTime; time(&ctTime);
                        pTime = localtime( &ctTime );
                        mLog << std::setw(2) << std::setfill('0') << pTime->tm_hour
                            << ":" << std::setw(2) << std::setfill('0') << pTime->tm_min
                            << ":" << std::setw(2) << std::setfill('0') << pTime->tm_sec
                            << ": ";
                    }
                    mLog << message << std::endl;

                    // Flush stcmdream to ensure it is written (incase of a crash, we need log to be up to date)
                    mLog.flush();
                }
            }
        }
    }

    //-----------------------------------------------------------------------
    void Log::setTimeStampEnabled(bool timeStamp)
    {
        OGRE_LOCK_AUTO_MUTEX;
        mTimeStamp = timeStamp;
    }

    //-----------------------------------------------------------------------
    void Log::setDebugOutputEnabled(bool debugOutput)
    {
        OGRE_LOCK_AUTO_MUTEX;
        mDebugOut = debugOutput;
    }

    //-----------------------------------------------------------------------
    void Log::setLogDetail(LoggingLevel ll)
    {
        OGRE_LOCK_AUTO_MUTEX;
        mLogLevel = ll;
    }

    //-----------------------------------------------------------------------
    void Log::addListener(LogListener* listener)
    {
        OGRE_LOCK_AUTO_MUTEX;
        if (std::find(mListeners.begin(), mListeners.end(), listener) == mListeners.end())
            mListeners.push_back(listener);
    }

    //-----------------------------------------------------------------------
    void Log::removeListener(LogListener* listener)
    {
        OGRE_LOCK_AUTO_MUTEX;
        mtLogListener::iterator i = std::find(mListeners.begin(), mListeners.end(), listener);
        if (i != mListeners.end())
            mListeners.erase(i);
    }
    //---------------------------------------------------------------------
    Log::Stream Log::stream(LogMessageLevel lml, bool maskDebug)
    {
        return Stream(this, lml, maskDebug);

    }
}
<|endoftext|>


<commit_before>#include "bulkDataNTReceiverImpl.h"
#include "bulkDataNTCallback.h"
#include <iostream>

class  TestCB:  public BulkDataCallback
{
public:
	int cbStart(unsigned char* userParam_p, unsigned  int size)
	{
		std::cout << "=>cbStart[" << recvName_m << "/" << flowName_m << "]: got " << size << " :";
		for(unsigned int i=0; i<size; i++)
		{
			std::cout <<  *(char*)(userParam_p+i);
		}
		std::cout << std::endl;
		return 0;
	}

	int cbReceive(unsigned char* userParam_p, unsigned  int size)
	{
		std::cout << "=>cbReceive[" << recvName_m << "/" << flowName_m << "]: got " << size << " :";
/*		for(unsigned int i=0; i<frame_p->length(); i++)
		{
			std::cout <<  *(char*)(frame_p->base()+i);
		}
	*/	std::cout << std::endl;
		return 0;
	}

	int cbStop()
	{
		std::cout << "=>cbStop[" << recvName_m << "/" << flowName_m << "]" << std::endl;
		return 0;
	}

};

/* --------------- [ MACI DLL support functions ] -----------------*/
#include <maciACSComponentDefines.h>
MACI_DLL_SUPPORT_FUNCTIONS(BulkDataNTReceiverImpl<TestCB>)
/* ----------------------------------------------------------------*/
<commit_msg>moved some commented code for testing seg faults and slow receivers<commit_after>#include "bulkDataNTReceiverImpl.h"
#include "bulkDataNTCallback.h"
#include <iostream>

class  TestCB:  public BulkDataCallback
{
public:
	int cbStart(unsigned char* userParam_p, unsigned  int size)
	{
		std::cout << "=>cbStart[" << recvName_m << "/" << flowName_m << "]: got " << size << " :";
		for(unsigned int i=0; i<size; i++)
		{
			std::cout <<  *(char*)(userParam_p+i);
		}
		std::cout << std::endl;
		return 0;
	}

	int cbReceive(unsigned char* userParam_p, unsigned  int size)
	{
		std::cout << "=>cbReceive[" << recvName_m << "/" << flowName_m << "]: got " << size << " :";
		/*		for(unsigned int i=0; i<frame_p->length(); i++)
		{
			std::cout <<  *(char*)(frame_p->base()+i);
		}
		 */	std::cout << std::endl;

	/*	 if (sleep_period!=0)
		 {
			 cout << listName << " Rest:" << message.restDataLength << " Going sleep for: " << sleep_period << endl;
			 //cout <<  message.data.length() << endl;
			 //cout <<  message.restDataLength << endl;
			 usleep(sleep_period);
		 }
		*/
		 /* simulate seg fault
    			if (data_length>100000) {
    			char *tt=0;
    			printf("XXX %s\n", tt);
    			printf("crash\n");
    			ACE_Time_Value *t=0;
    			t->sec();
    			DDS::DataReaderQos *ddr_qos=0;
    			ddr_qos->reliability.kind = 0;
    			printf("after crash\n");
    			} */

		 return 0;
	}

	int cbStop()
	{
		std::cout << "=>cbStop[" << recvName_m << "/" << flowName_m << "]" << std::endl;
		return 0;
	}

};

/* --------------- [ MACI DLL support functions ] -----------------*/
#include <maciACSComponentDefines.h>
MACI_DLL_SUPPORT_FUNCTIONS(BulkDataNTReceiverImpl<TestCB>)
/* ----------------------------------------------------------------*/
<|endoftext|>


<commit_before>/*
 * Copyright(c) Sophist Solutions, Inc. 1990-2016.  All rights reserved
 */
#ifndef _Stroika_Foundation_Memory_BlockAllocator_inl_
#define _Stroika_Foundation_Memory_BlockAllocator_inl_  1

/*
 ********************************************************************************
 ***************************** Implementation Details ***************************
 ********************************************************************************
 */
#include    <algorithm>
#include    <mutex>
#include    <vector>

#include    "../Debug/Assertions.h"
#include    "../Debug/Valgrind.h"
#include    "../Execution/Common.h"

#include    "Common.h"

/*
 *  qStroika_Foundation_Memory_BlockAllocator_UseSpinLock_ is probaly best true (empirical tests with the
 *  performance regression test indicated that this helped considerably).
 *
 *  It should be generally pretty safe because the locks are very narrow (so threads shoudlnt spend much time
 *  spinning. And we could reduce this further during mallocs of new blocks.
 */
#if     !defined (qStroika_Foundation_Memory_BlockAllocator_UseSpinLock_)
#define qStroika_Foundation_Memory_BlockAllocator_UseSpinLock_   1
#endif

#if    qStroika_Foundation_Memory_BlockAllocator_UseSpinLock_
#include    "../Execution/SpinLock.h"
#endif

/*
 *  qStroika_Foundation_Memory_BlockAllocator_UseMallocDirectly_ is currently experimental as
 *  a test to see if its faster than calling operator new (for blocks of RAM).
 *
 *  operator new[] does very little for us, and could in principle have call overhead and
 *  overhead to track the number of entries in the array (which would be pointless).
 */
#if     !defined (qStroika_Foundation_Memory_BlockAllocator_UseMallocDirectly_)
#define qStroika_Foundation_Memory_BlockAllocator_UseMallocDirectly_   1
#endif

#if     qStroika_Foundation_Memory_BlockAllocator_UseMallocDirectly_
#include    "../Execution/Exceptions.h"
#endif

namespace   Stroika {
    namespace   Foundation {
        namespace   Memory {

            namespace   Private_ {

                struct  BlockAllocator_ModuleInit_ {
                    BlockAllocator_ModuleInit_ ();
                    ~BlockAllocator_ModuleInit_ ();
                };

#if    qStroika_Foundation_Memory_BlockAllocator_UseSpinLock_
                using LockType_ = Execution::SpinLock;
#else
                using LockType_ = mutex;
#endif
                extern  LockType_*    sLock_;

                inline  LockType_&    GetLock_ ()
                {
                    AssertNotNull (sLock_);  // automatically initailized by BlockAllocated::Private_::ActualModuleInit
                    return *sLock_;
                }

                void    DoDeleteHandlingLocksExceptionsEtc_ (void* p, void** staticNextLinkP) noexcept;

            }

            namespace   Private_ {

                // not quite right - too much of a PITA to support both constexpr and non- just wait til all our compilers support constexpr and then fix!
                inline    constexpr size_t  BlockAllocation_Private_AdjustSizeForPool_ (size_t n)
                {
                    // when we really fix constexpr usage, we can use the below!
                    //return  Math::RoundUpTo (sizeof(T), sizeof (void*));
                    return (((n + sizeof (void*) - 1u) / sizeof (void*)) * sizeof (void*));
                }

                /*
                * Picked particular kTargetMallocSize since with malloc overhead likely to turn out to be
                * a chunk which memory allocator can do a good job on.
                */
                constexpr   size_t  kTargetMallocSize_   =   16360;                  // 16384 = 16K - leave a few bytes sluff...

                inline    /*constexpr*/ size_t  BlockAllocation_Private_ComputeChunks_ (size_t poolElementSize)
                {
                    return std::max (static_cast<size_t> (kTargetMallocSize_ / poolElementSize), static_cast<size_t> (10));
                }

                // This must be included here to keep genclass happy, since the .cc file will not be included
                // in the genclassed .cc file....
                inline  void**  GetMem_Util_ (size_t sz)
                {
                    Require (sz >= sizeof (void*));

                    const       size_t  kChunks = BlockAllocation_Private_ComputeChunks_ (sz);

                    /*
                     * Please note that the following line is NOT a memory leak. Please look at the
                     * Led FAQ question#29 - "Does Led have any memory leaks?
                     * How does qAllowBlockAllocation affect memory leaks?"
                     */
#if     qStroika_Foundation_Memory_BlockAllocator_UseMallocDirectly_
                    void**  newLinks    =   (void**)malloc (kChunks * sz);
                    Execution::ThrowIfNull (newLinks);
#else
                    void**  newLinks    =   (void**)new char [kChunks * sz];
#endif
                    AssertNotNull (newLinks);
                    void**  curLink     =   newLinks;
                    for (size_t i = 1; i < kChunks; i++) {
                        *curLink = &(((char*)newLinks)[i * sz]);
                        curLink = (void**)*curLink;
                    }
                    *curLink = nullptr;     // nullptr-terminate the link list
                    return newLinks;
                }

                /*
                 *       In order to reduce fragmentation, we allocate all chunks of common sizes out of
                 *   the same pool. Assuming the compiler is able to inline the references to these
                 *   below, there is really no cost in adding more. I picked the ones I thought most
                 *   likely to come up, but adding more should be just fine - strictly a win.
                 *
                 *       Don't bother implementing Block_Alloced_sizeof_N_GetMems() cuz flunging will
                 *   genericly give us the same code-sharing effect.
                 *
                 *  <<@todo - update docs -.... VERY OUT OF DATE>>>.
                 */
                template    <size_t SIZE>
                class   BlockAllocationPool_  {
                public:
                    static  void*   Allocate (size_t n);
                    static  void    Deallocate (void* p) noexcept;
                    static  void    Compact ();

                private:
                    // make op new inline for MOST important case
                    // were alloc is cheap linked list operation...
                    static  void*   sNextLink_;
                };

            }

            /*
             ********************************************************************************
             ***************** Private_::BlockAllocationPool_<SIZE> *************************
             ********************************************************************************
             */
            template    <size_t SIZE>
            inline  void*   Private_::BlockAllocationPool_<SIZE>::Allocate (size_t n)
            {
                static_assert (SIZE >= sizeof (void*), "SIZE >= sizeof (void*)");
                Require (n <= SIZE);
                Arg_Unused (n);                         // n only used for debuggging, avoid compiler warning

#if     qCompilerAndStdLib_make_unique_lock_IsSlow
                MACRO_LOCK_GUARD_CONTEXT (Private_::GetLock_ ());
#else
                auto    critSec { make_unique_lock (Private_::GetLock_ ()) };
#endif
                /*
                 * To implement linked list of BlockAllocated(T)'s before they are
                 * actually alloced, re-use the begining of this as a link pointer.
                 */
                if (sNextLink_ == nullptr) {
                    sNextLink_ = GetMem_Util_ (SIZE);
                }
                void*   result = sNextLink_;
                AssertNotNull (result);
                /*
                 * treat this as a linked list, and make head point to next member
                 */
                sNextLink_ = (*(void**)sNextLink_);
                return result;
            }
            template    <size_t SIZE>
            inline  void    Private_::BlockAllocationPool_<SIZE>::Deallocate (void* p) noexcept
            {
                static_assert (SIZE >= sizeof (void*), "SIZE >= sizeof (void*)");
                RequireNotNull (p);
#if     qStroika_FeatureSupported_Valgrind
                VALGRIND_HG_CLEAN_MEMORY (p, SIZE);
#endif
                Private_::DoDeleteHandlingLocksExceptionsEtc_ (p,  &sNextLink_);
            }
            template    <size_t SIZE>
            void    Private_::BlockAllocationPool_<SIZE>::Compact ()
            {
#if     qCompilerAndStdLib_make_unique_lock_IsSlow
                MACRO_LOCK_GUARD_CONTEXT (Private_::GetLock_ ());
#else
                auto    critSec { make_unique_lock (Private_::GetLock_ ()) };
#endif

                // step one: put all the links into a single, sorted vector
                const   size_t  kChunks = BlockAllocation_Private_ComputeChunks_ (SIZE);
                std::vector<void*> links;
                try {
                    links.reserve (kChunks * 2);
                    void*   link = sNextLink_;
                    while (link != nullptr) {
                        // probably should use Containers::ReserveSpeedTweekAddN - but want to avoid embrace of dependencies
                        if (links.size () == links.capacity ()) {
                            links.reserve (links.size () * 2);
                        }
                        links.push_back (link);
                        link = *(void**)link;
                    }
                }
                catch (...) {
                    return;
                }

                std::sort (links.begin (), links.end ());

                // now look for unused memory blocks. Since the vector is sorted by pointer value, the first pointer encountered is potentially the start of a block.
                // Look at the next N vector elements, where N is the amount of elements that would have been alloced (the chunk size). If they are all there, then the
                // block is unused can can be freed. Otherwise do the same thing to the first element found outside of the block.
                size_t  originalSize = links.size ();
                size_t  index = 0;
                while (index < links.size ()) {
                    Byte*   deleteCandidate = (Byte*)links[index];
                    bool    canDelete = true;
                    size_t  i = 1;
                    for (; i < kChunks; ++i) {
                        if ((index + i) >= links.size ()) break;
                        Byte* next = (Byte*)links[index + i];
                        Byte* prev = (Byte*)links[index + i - 1];
                        if (canDelete and ((next - prev) != SIZE)) {
                            canDelete = false;  // don't break here, as have to find end up this block, which could be further on
                        }
                        if (static_cast <size_t> (std::abs (next - deleteCandidate) / SIZE) >= kChunks) {
                            Assert (not canDelete);
                            break;
                        }
                    }
                    if (canDelete) {
                        links.erase (links.begin () + index, links.begin () + index + kChunks);
#if     qStroika_Foundation_Memory_BlockAllocator_UseMallocDirectly_
                        free ((void*)deleteCandidate);
#else
                        delete static_cast<Byte*> ((void*)deleteCandidate);
#endif
                    }
                    else {
                        index += i;
                    }
                }

                // finally, clean up the pool. The head link is probably wrong, and the elements are no longer linked up correctly
                sNextLink_ = (links.size () == 0) ? nullptr : links[0];
                if (links.size () != originalSize and links.size () > 0) {
                    // we delete some, which means that the next pointers are bad
                    void**  curLink     =   (void**)sNextLink_;
                    for (size_t i = 1; i < links.size (); i++) {
                        *curLink = links[i];
                        curLink = (void**)*curLink;
                    }
                    *curLink = nullptr;
                }
            }
            template    <size_t SIZE>
            void*   Private_::BlockAllocationPool_<SIZE>::sNextLink_ = nullptr;

            /*
             ********************************************************************************
             ********************************** BlockAllocator<T> ***************************
             ********************************************************************************
             */
            template    <typename   T>
            inline  void*   BlockAllocator<T>::Allocate (size_t n)
            {
                using Private_::BlockAllocationPool_;
                using Private_::BlockAllocation_Private_AdjustSizeForPool_;
                Require (n == sizeof (T));
                Arg_Unused (n);                         // n only used for debuggging, avoid compiler warning

#if     qAllowBlockAllocation
                return BlockAllocationPool_<BlockAllocation_Private_AdjustSizeForPool_ (sizeof (T))>::Allocate (n);
#else
                return ::operator new (n);
#endif
            }
            template    <typename   T>
            inline  void    BlockAllocator<T>::Deallocate (void* p) noexcept
            {
                using Private_::BlockAllocationPool_;
                using Private_::BlockAllocation_Private_AdjustSizeForPool_;
#if     qAllowBlockAllocation
                if (p != nullptr) {
                    BlockAllocationPool_<BlockAllocation_Private_AdjustSizeForPool_ (sizeof (T))>::Deallocate (p);
                }
#else
                ::operator delete (p);
#endif
            }
            template    <typename   T>
            void    BlockAllocator<T>::Compact ()
            {
                using Private_::BlockAllocationPool_;
                using Private_::BlockAllocation_Private_AdjustSizeForPool_;
#if     qAllowBlockAllocation
                BlockAllocationPool_<BlockAllocation_Private_AdjustSizeForPool_ (sizeof (T))>::Compact ();
#endif
            }

        }
    }
}
namespace   {
    Stroika::Foundation::Execution::ModuleInitializer<Stroika::Foundation::Memory::Private_::BlockAllocator_ModuleInit_>   _Stroika_Foundation_Memory_BlockAllocator_ModuleInit_;   // this object constructed for the CTOR/DTOR per-module side-effects
}
#endif  /*_Stroika_Foundation_Memory_BlockAllocator_inl_*/
<commit_msg>qStroika_FeatureSupported_Valgrind on allocate too<commit_after>/*
 * Copyright(c) Sophist Solutions, Inc. 1990-2016.  All rights reserved
 */
#ifndef _Stroika_Foundation_Memory_BlockAllocator_inl_
#define _Stroika_Foundation_Memory_BlockAllocator_inl_  1

/*
 ********************************************************************************
 ***************************** Implementation Details ***************************
 ********************************************************************************
 */
#include    <algorithm>
#include    <mutex>
#include    <vector>

#include    "../Debug/Assertions.h"
#include    "../Debug/Valgrind.h"
#include    "../Execution/Common.h"

#include    "Common.h"

/*
 *  qStroika_Foundation_Memory_BlockAllocator_UseSpinLock_ is probaly best true (empirical tests with the
 *  performance regression test indicated that this helped considerably).
 *
 *  It should be generally pretty safe because the locks are very narrow (so threads shoudlnt spend much time
 *  spinning. And we could reduce this further during mallocs of new blocks.
 */
#if     !defined (qStroika_Foundation_Memory_BlockAllocator_UseSpinLock_)
#define qStroika_Foundation_Memory_BlockAllocator_UseSpinLock_   1
#endif

#if    qStroika_Foundation_Memory_BlockAllocator_UseSpinLock_
#include    "../Execution/SpinLock.h"
#endif

/*
 *  qStroika_Foundation_Memory_BlockAllocator_UseMallocDirectly_ is currently experimental as
 *  a test to see if its faster than calling operator new (for blocks of RAM).
 *
 *  operator new[] does very little for us, and could in principle have call overhead and
 *  overhead to track the number of entries in the array (which would be pointless).
 */
#if     !defined (qStroika_Foundation_Memory_BlockAllocator_UseMallocDirectly_)
#define qStroika_Foundation_Memory_BlockAllocator_UseMallocDirectly_   1
#endif

#if     qStroika_Foundation_Memory_BlockAllocator_UseMallocDirectly_
#include    "../Execution/Exceptions.h"
#endif

namespace   Stroika {
    namespace   Foundation {
        namespace   Memory {

            namespace   Private_ {

                struct  BlockAllocator_ModuleInit_ {
                    BlockAllocator_ModuleInit_ ();
                    ~BlockAllocator_ModuleInit_ ();
                };

#if    qStroika_Foundation_Memory_BlockAllocator_UseSpinLock_
                using LockType_ = Execution::SpinLock;
#else
                using LockType_ = mutex;
#endif
                extern  LockType_*    sLock_;

                inline  LockType_&    GetLock_ ()
                {
                    AssertNotNull (sLock_);  // automatically initailized by BlockAllocated::Private_::ActualModuleInit
                    return *sLock_;
                }

                void    DoDeleteHandlingLocksExceptionsEtc_ (void* p, void** staticNextLinkP) noexcept;

            }

            namespace   Private_ {

                // not quite right - too much of a PITA to support both constexpr and non- just wait til all our compilers support constexpr and then fix!
                inline    constexpr size_t  BlockAllocation_Private_AdjustSizeForPool_ (size_t n)
                {
                    // when we really fix constexpr usage, we can use the below!
                    //return  Math::RoundUpTo (sizeof(T), sizeof (void*));
                    return (((n + sizeof (void*) - 1u) / sizeof (void*)) * sizeof (void*));
                }

                /*
                * Picked particular kTargetMallocSize since with malloc overhead likely to turn out to be
                * a chunk which memory allocator can do a good job on.
                */
                constexpr   size_t  kTargetMallocSize_   =   16360;                  // 16384 = 16K - leave a few bytes sluff...

                inline    /*constexpr*/ size_t  BlockAllocation_Private_ComputeChunks_ (size_t poolElementSize)
                {
                    return std::max (static_cast<size_t> (kTargetMallocSize_ / poolElementSize), static_cast<size_t> (10));
                }

                // This must be included here to keep genclass happy, since the .cc file will not be included
                // in the genclassed .cc file....
                inline  void**  GetMem_Util_ (size_t sz)
                {
                    Require (sz >= sizeof (void*));

                    const       size_t  kChunks = BlockAllocation_Private_ComputeChunks_ (sz);

                    /*
                     * Please note that the following line is NOT a memory leak. Please look at the
                     * Led FAQ question#29 - "Does Led have any memory leaks?
                     * How does qAllowBlockAllocation affect memory leaks?"
                     */
#if     qStroika_Foundation_Memory_BlockAllocator_UseMallocDirectly_
                    void**  newLinks    =   (void**)malloc (kChunks * sz);
                    Execution::ThrowIfNull (newLinks);
#else
                    void**  newLinks    =   (void**)new char [kChunks * sz];
#endif
                    AssertNotNull (newLinks);
                    void**  curLink     =   newLinks;
                    for (size_t i = 1; i < kChunks; i++) {
                        *curLink = &(((char*)newLinks)[i * sz]);
                        curLink = (void**)*curLink;
                    }
                    *curLink = nullptr;     // nullptr-terminate the link list
                    return newLinks;
                }

                /*
                 *       In order to reduce fragmentation, we allocate all chunks of common sizes out of
                 *   the same pool. Assuming the compiler is able to inline the references to these
                 *   below, there is really no cost in adding more. I picked the ones I thought most
                 *   likely to come up, but adding more should be just fine - strictly a win.
                 *
                 *       Don't bother implementing Block_Alloced_sizeof_N_GetMems() cuz flunging will
                 *   genericly give us the same code-sharing effect.
                 *
                 *  <<@todo - update docs -.... VERY OUT OF DATE>>>.
                 */
                template    <size_t SIZE>
                class   BlockAllocationPool_  {
                public:
                    static  void*   Allocate (size_t n);
                    static  void    Deallocate (void* p) noexcept;
                    static  void    Compact ();

                private:
                    // make op new inline for MOST important case
                    // were alloc is cheap linked list operation...
                    static  void*   sNextLink_;
                };

            }

            /*
             ********************************************************************************
             ***************** Private_::BlockAllocationPool_<SIZE> *************************
             ********************************************************************************
             */
            template    <size_t SIZE>
            inline  void*   Private_::BlockAllocationPool_<SIZE>::Allocate (size_t n)
            {
                static_assert (SIZE >= sizeof (void*), "SIZE >= sizeof (void*)");
                Require (n <= SIZE);
                Arg_Unused (n);                         // n only used for debuggging, avoid compiler warning

#if     qCompilerAndStdLib_make_unique_lock_IsSlow
                MACRO_LOCK_GUARD_CONTEXT (Private_::GetLock_ ());
#else
                auto    critSec { make_unique_lock (Private_::GetLock_ ()) };
#endif
                /*
                 * To implement linked list of BlockAllocated(T)'s before they are
                 * actually alloced, re-use the begining of this as a link pointer.
                 */
                if (sNextLink_ == nullptr) {
                    sNextLink_ = GetMem_Util_ (SIZE);
                }
                void*   result = sNextLink_;
                AssertNotNull (result);
                /*
                 * treat this as a linked list, and make head point to next member
                 */
                sNextLink_ = (*(void**)sNextLink_);
#if     qStroika_FeatureSupported_Valgrind
                VALGRIND_HG_CLEAN_MEMORY (result, SIZE);
#endif
                return result;
            }
            template    <size_t SIZE>
            inline  void    Private_::BlockAllocationPool_<SIZE>::Deallocate (void* p) noexcept
            {
                static_assert (SIZE >= sizeof (void*), "SIZE >= sizeof (void*)");
                RequireNotNull (p);
#if     qStroika_FeatureSupported_Valgrind
                VALGRIND_HG_CLEAN_MEMORY (p, SIZE);
#endif
                Private_::DoDeleteHandlingLocksExceptionsEtc_ (p,  &sNextLink_);
            }
            template    <size_t SIZE>
            void    Private_::BlockAllocationPool_<SIZE>::Compact ()
            {
#if     qCompilerAndStdLib_make_unique_lock_IsSlow
                MACRO_LOCK_GUARD_CONTEXT (Private_::GetLock_ ());
#else
                auto    critSec { make_unique_lock (Private_::GetLock_ ()) };
#endif

                // step one: put all the links into a single, sorted vector
                const   size_t  kChunks = BlockAllocation_Private_ComputeChunks_ (SIZE);
                std::vector<void*> links;
                try {
                    links.reserve (kChunks * 2);
                    void*   link = sNextLink_;
                    while (link != nullptr) {
                        // probably should use Containers::ReserveSpeedTweekAddN - but want to avoid embrace of dependencies
                        if (links.size () == links.capacity ()) {
                            links.reserve (links.size () * 2);
                        }
                        links.push_back (link);
                        link = *(void**)link;
                    }
                }
                catch (...) {
                    return;
                }

                std::sort (links.begin (), links.end ());

                // now look for unused memory blocks. Since the vector is sorted by pointer value, the first pointer encountered is potentially the start of a block.
                // Look at the next N vector elements, where N is the amount of elements that would have been alloced (the chunk size). If they are all there, then the
                // block is unused can can be freed. Otherwise do the same thing to the first element found outside of the block.
                size_t  originalSize = links.size ();
                size_t  index = 0;
                while (index < links.size ()) {
                    Byte*   deleteCandidate = (Byte*)links[index];
                    bool    canDelete = true;
                    size_t  i = 1;
                    for (; i < kChunks; ++i) {
                        if ((index + i) >= links.size ()) break;
                        Byte* next = (Byte*)links[index + i];
                        Byte* prev = (Byte*)links[index + i - 1];
                        if (canDelete and ((next - prev) != SIZE)) {
                            canDelete = false;  // don't break here, as have to find end up this block, which could be further on
                        }
                        if (static_cast <size_t> (std::abs (next - deleteCandidate) / SIZE) >= kChunks) {
                            Assert (not canDelete);
                            break;
                        }
                    }
                    if (canDelete) {
                        links.erase (links.begin () + index, links.begin () + index + kChunks);
#if     qStroika_Foundation_Memory_BlockAllocator_UseMallocDirectly_
                        free ((void*)deleteCandidate);
#else
                        delete static_cast<Byte*> ((void*)deleteCandidate);
#endif
                    }
                    else {
                        index += i;
                    }
                }

                // finally, clean up the pool. The head link is probably wrong, and the elements are no longer linked up correctly
                sNextLink_ = (links.size () == 0) ? nullptr : links[0];
                if (links.size () != originalSize and links.size () > 0) {
                    // we delete some, which means that the next pointers are bad
                    void**  curLink     =   (void**)sNextLink_;
                    for (size_t i = 1; i < links.size (); i++) {
                        *curLink = links[i];
                        curLink = (void**)*curLink;
                    }
                    *curLink = nullptr;
                }
            }
            template    <size_t SIZE>
            void*   Private_::BlockAllocationPool_<SIZE>::sNextLink_ = nullptr;

            /*
             ********************************************************************************
             ********************************** BlockAllocator<T> ***************************
             ********************************************************************************
             */
            template    <typename   T>
            inline  void*   BlockAllocator<T>::Allocate (size_t n)
            {
                using Private_::BlockAllocationPool_;
                using Private_::BlockAllocation_Private_AdjustSizeForPool_;
                Require (n == sizeof (T));
                Arg_Unused (n);                         // n only used for debuggging, avoid compiler warning

#if     qAllowBlockAllocation
                return BlockAllocationPool_<BlockAllocation_Private_AdjustSizeForPool_ (sizeof (T))>::Allocate (n);
#else
                return ::operator new (n);
#endif
            }
            template    <typename   T>
            inline  void    BlockAllocator<T>::Deallocate (void* p) noexcept
            {
                using Private_::BlockAllocationPool_;
                using Private_::BlockAllocation_Private_AdjustSizeForPool_;
#if     qAllowBlockAllocation
                if (p != nullptr) {
                    BlockAllocationPool_<BlockAllocation_Private_AdjustSizeForPool_ (sizeof (T))>::Deallocate (p);
                }
#else
                ::operator delete (p);
#endif
            }
            template    <typename   T>
            void    BlockAllocator<T>::Compact ()
            {
                using Private_::BlockAllocationPool_;
                using Private_::BlockAllocation_Private_AdjustSizeForPool_;
#if     qAllowBlockAllocation
                BlockAllocationPool_<BlockAllocation_Private_AdjustSizeForPool_ (sizeof (T))>::Compact ();
#endif
            }

        }
    }
}
namespace   {
    Stroika::Foundation::Execution::ModuleInitializer<Stroika::Foundation::Memory::Private_::BlockAllocator_ModuleInit_>   _Stroika_Foundation_Memory_BlockAllocator_ModuleInit_;   // this object constructed for the CTOR/DTOR per-module side-effects
}
#endif  /*_Stroika_Foundation_Memory_BlockAllocator_inl_*/
<|endoftext|>


<commit_before>// Copyright (c) 2010-16 Bifrost Entertainment AS and Tommy Nguyen
// Distributed under the MIT License.
// (See accompanying file LICENSE or copy at http://opensource.org/licenses/MIT)

#include "Graphics/SceneGraph.h"

#include "Graphics/Animation.h"
#include "Graphics/Drawable.h"
#include "Graphics/Label.h"
#include "Graphics/Renderer.h"
#include "Graphics/SpriteBatch.h"

static_assert(ShaderManager::kInvalidProgram == 0,
              "Inlined SceneNode(Type, void*) assumes kInvalidProgram == 0");

using rainbow::SceneNode;

namespace
{
    class AnimationNode final : public SceneNode
    {
    public:
        AnimationNode(Animation& animation) : animation_(animation) {}

    private:
        Animation& animation_;

        void draw_impl() const override {}
        void move_impl(const Vec2f&) const override {}

        void update_impl(unsigned long dt) const override
        {
            animation_.update(dt);
        }
    };

    class DrawableNode final : public SceneNode
    {
    public:
        DrawableNode(Drawable& drawable) : drawable_(drawable) {}

    private:
        Drawable& drawable_;

        void draw_impl() const override { drawable_.draw(); }

        void move_impl(const Vec2f& delta) const override
        {
            drawable_.move(delta);
        }

        void update_impl(unsigned long dt) const override
        {
            drawable_.update(dt);
        }
    };

    class LabelNode final : public SceneNode
    {
    public:
        LabelNode(Label& label) : label_(label) {}

    private:
        Label& label_;

        void draw_impl() const override { rainbow::graphics::draw(label_); }

        void move_impl(const Vec2f& delta) const override
        {
            label_.move(delta);
        }

        void update_impl(unsigned long) const override
        {
            label_.update();
        }
    };

    class SpriteBatchNode final : public SceneNode
    {
    public:
        SpriteBatchNode(SpriteBatch& batch) : sprite_batch_(batch) {}

    private:
        SpriteBatch& sprite_batch_;

        void draw_impl() const override
        {
            rainbow::graphics::draw(sprite_batch_);
        }

        void move_impl(const Vec2f& delta) const override
        {
            sprite_batch_.move(delta);
        }

        void update_impl(unsigned long) const override
        {
            sprite_batch_.update();
        }
    };
}

void SceneNode::draw() const
{
    if (!is_enabled())
        return;

    ShaderManager::Context context;
    if (program_ != ShaderManager::kInvalidProgram)
        ShaderManager::Get()->use(program_);

    draw_impl();

    for (auto&& child : children_)
        child->draw();
}

void SceneNode::move(const Vec2f& delta) const
{
    for_each(*this, [](const SceneNode& node, const Vec2f& delta) {
        node.move_impl(delta);
    }, delta);
}

void SceneNode::update(unsigned long dt) const
{
    if (!is_enabled())
        return;

    update_impl(dt);

    for (auto&& child : children_)
        child->update(dt);
}

// Workaround for https://gcc.gnu.org/bugzilla/show_bug.cgi?id=56480
#if defined(__GNUC__) && !defined(__clang__)
namespace rainbow {
#endif

auto SceneNode::create() -> std::unique_ptr<SceneNode>
{
    return std::unique_ptr<SceneNode>{std::make_unique<GroupNode>()};
}

auto SceneNode::create(Drawable& drawable) -> std::unique_ptr<SceneNode>
{
    return std::unique_ptr<SceneNode>{std::make_unique<DrawableNode>(drawable)};
}

template <>
auto SceneNode::create(Animation& a) -> std::unique_ptr<SceneNode>
{
    return std::unique_ptr<SceneNode>{std::make_unique<AnimationNode>(a)};
}

template <>
auto SceneNode::create(Label& l) -> std::unique_ptr<SceneNode>
{
    return std::unique_ptr<SceneNode>{std::make_unique<LabelNode>(l)};
}

template <>
auto SceneNode::create(SpriteBatch& s) -> std::unique_ptr<SceneNode>
{
    return std::unique_ptr<SceneNode>{std::make_unique<SpriteBatchNode>(s)};
}

#if defined(__GNUC__) && !defined(__clang__)
}
#endif
<commit_msg>Consolidated LabelNode and SpriteBatchNode.<commit_after>// Copyright (c) 2010-16 Bifrost Entertainment AS and Tommy Nguyen
// Distributed under the MIT License.
// (See accompanying file LICENSE or copy at http://opensource.org/licenses/MIT)

#include "Graphics/SceneGraph.h"

#include "Graphics/Animation.h"
#include "Graphics/Drawable.h"
#include "Graphics/Label.h"
#include "Graphics/Renderer.h"
#include "Graphics/SpriteBatch.h"

static_assert(ShaderManager::kInvalidProgram == 0,
              "Inlined SceneNode(Type, void*) assumes kInvalidProgram == 0");

using rainbow::SceneNode;

namespace
{
    class AnimationNode final : public SceneNode
    {
    public:
        AnimationNode(Animation& animation) : animation_(animation) {}

    private:
        Animation& animation_;

        void draw_impl() const override {}
        void move_impl(const Vec2f&) const override {}

        void update_impl(unsigned long dt) const override
        {
            animation_.update(dt);
        }
    };

    class DrawableNode final : public SceneNode
    {
    public:
        DrawableNode(Drawable& drawable) : drawable_(drawable) {}

    private:
        Drawable& drawable_;

        void draw_impl() const override { drawable_.draw(); }

        void move_impl(const Vec2f& delta) const override
        {
            drawable_.move(delta);
        }

        void update_impl(unsigned long dt) const override
        {
            drawable_.update(dt);
        }
    };

    template <typename T>
    class PrimitiveNode final : public SceneNode
    {
    public:
        PrimitiveNode(T& primitive) : primitive_(primitive) {}

    private:
        T& primitive_;

        void draw_impl() const override { rainbow::graphics::draw(primitive_); }

        void move_impl(const Vec2f& delta) const override
        {
            primitive_.move(delta);
        }

        void update_impl(unsigned long) const override { primitive_.update(); }
    };

    using LabelNode = PrimitiveNode<Label>;
    using SpriteBatchNode = PrimitiveNode<SpriteBatch>;
}

void SceneNode::draw() const
{
    if (!is_enabled())
        return;

    ShaderManager::Context context;
    if (program_ != ShaderManager::kInvalidProgram)
        ShaderManager::Get()->use(program_);

    draw_impl();

    for (auto&& child : children_)
        child->draw();
}

void SceneNode::move(const Vec2f& delta) const
{
    for_each(*this, [](const SceneNode& node, const Vec2f& delta) {
        node.move_impl(delta);
    }, delta);
}

void SceneNode::update(unsigned long dt) const
{
    if (!is_enabled())
        return;

    update_impl(dt);

    for (auto&& child : children_)
        child->update(dt);
}

// Workaround for https://gcc.gnu.org/bugzilla/show_bug.cgi?id=56480
#if defined(__GNUC__) && !defined(__clang__)
namespace rainbow {
#endif

auto SceneNode::create() -> std::unique_ptr<SceneNode>
{
    return std::unique_ptr<SceneNode>{std::make_unique<GroupNode>()};
}

auto SceneNode::create(Drawable& drawable) -> std::unique_ptr<SceneNode>
{
    return std::unique_ptr<SceneNode>{std::make_unique<DrawableNode>(drawable)};
}

template <>
auto SceneNode::create(Animation& a) -> std::unique_ptr<SceneNode>
{
    return std::unique_ptr<SceneNode>{std::make_unique<AnimationNode>(a)};
}

template <>
auto SceneNode::create(Label& l) -> std::unique_ptr<SceneNode>
{
    return std::unique_ptr<SceneNode>{std::make_unique<LabelNode>(l)};
}

template <>
auto SceneNode::create(SpriteBatch& s) -> std::unique_ptr<SceneNode>
{
    return std::unique_ptr<SceneNode>{std::make_unique<SpriteBatchNode>(s)};
}

#if defined(__GNUC__) && !defined(__clang__)
}
#endif
<|endoftext|>


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

#include "task_map/SmoothCollisionDistance.h"

REGISTER_TASKMAP_TYPE("SmoothCollisionDistance",
                      exotica::SmoothCollisionDistance);

namespace exotica
{
SmoothCollisionDistance::SmoothCollisionDistance() {}
void SmoothCollisionDistance::update(Eigen::VectorXdRefConst x,
                                     Eigen::VectorXdRef phi)
{
    if (phi.rows() != dim_) throw_named("Wrong size of phi!");
    phi.setZero();
    Eigen::MatrixXd J;
    update(x, phi, J, false);
}

void SmoothCollisionDistance::update(Eigen::VectorXdRefConst x,
                                     Eigen::VectorXdRef phi,
                                     Eigen::MatrixXdRef J)
{
    if (phi.rows() != dim_) throw_named("Wrong size of phi!");
    phi.setZero();
    J.setZero();

    update(x, phi, J, true);
}

void SmoothCollisionDistance::update(Eigen::VectorXdRefConst x,
                                     Eigen::VectorXdRef phi,
                                     Eigen::MatrixXdRef J,
                                     bool updateJacobian)
{
    double& d = phi(0);
    for (const auto& link : robotLinks_)
    {
        // Get all world collision links, then iterate through them
        std::vector<CollisionProxy> proxies = cscene_->getCollisionDistance(scene_->getControlledLinkToCollisionLinkMap()[link], check_self_collision_);

        for (const auto& proxy : proxies)
        {
            bool isRobotToRobot = (proxy.e1->isRobotLink || proxy.e1->ClosestRobotLink.lock()) && (proxy.e2->isRobotLink || proxy.e2->ClosestRobotLink.lock());
            double& margin = isRobotToRobot ? robot_margin_ : world_margin_;
            if (proxy.distance < margin)
            {
                // Cost
                d += std::pow((1. - proxy.distance / margin), linear_ ? 1 : 2);

                if (updateJacobian)
                {
                    // Jacobian
                    KDL::Frame arel = KDL::Frame(proxy.e1->Frame.Inverse(KDL::Vector(
                        proxy.contact1(0), proxy.contact1(1), proxy.contact1(2))));
                    KDL::Frame brel = KDL::Frame(proxy.e2->Frame.Inverse(KDL::Vector(
                        proxy.contact2(0), proxy.contact2(1), proxy.contact2(2))));

                    if (!linear_)
                    {
                        Eigen::MatrixXd tmpJ = scene_->getSolver().Jacobian(
                            proxy.e1, arel, nullptr, KDL::Frame());
                        J += (2. / (margin * margin)) * (proxy.normal1.transpose() * tmpJ);
                        tmpJ = scene_->getSolver().Jacobian(proxy.e2, brel, nullptr,
                                                            KDL::Frame());
                        J -= (2. / (margin * margin)) * (proxy.normal1.transpose() * tmpJ);
                    }
                    else
                    {
                        Eigen::MatrixXd tmpJ = scene_->getSolver().Jacobian(
                            proxy.e1, arel, nullptr, KDL::Frame());
                        J += 1 / margin * (proxy.normal1.transpose() * tmpJ);
                        tmpJ = scene_->getSolver().Jacobian(proxy.e2, brel, nullptr,
                                                            KDL::Frame());
                        J -= 1 / margin * (proxy.normal1.transpose() * tmpJ);
                    }
                }
            }
        }
    }
}

void SmoothCollisionDistance::Instantiate(
    SmoothCollisionDistanceInitializer& init)
{
    init_ = init;
}

void SmoothCollisionDistance::assignScene(Scene_ptr scene)
{
    scene_ = scene;
    Initialize();
}

void SmoothCollisionDistance::Initialize()
{
    cscene_ = scene_->getCollisionScene();
    world_margin_ = init_.WorldMargin;
    robot_margin_ = init_.RobotMargin;
    linear_ = init_.Linear;
    check_self_collision_ = init_.CheckSelfCollision;

    HIGHLIGHT_NAMED("Smooth Collision Distance",
                    "World Margin: " << world_margin_ << " Robot Margin: " << robot_margin_ << "\t Linear: " << linear_);

    // Get names of all controlled joints and their corresponding child links
    robotLinks_ = scene_->getControlledLinkNames();
}

int SmoothCollisionDistance::taskSpaceDim() { return dim_; }
}
<commit_msg>SmoothCollisionDistance: Change back to only using links as we haven't fixed Jacobian yet<commit_after>/*
 *      Author: Wolfgang Merkt
 *
 * Copyright (c) 2017, Wolfgang Merkt
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *  * Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *  * Neither the name of  nor the names of its contributors may be used to
 *    endorse or promote products derived from this software without specific
 *    prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include "task_map/SmoothCollisionDistance.h"

REGISTER_TASKMAP_TYPE("SmoothCollisionDistance",
                      exotica::SmoothCollisionDistance);

namespace exotica
{
SmoothCollisionDistance::SmoothCollisionDistance() {}
void SmoothCollisionDistance::update(Eigen::VectorXdRefConst x,
                                     Eigen::VectorXdRef phi)
{
    if (phi.rows() != dim_) throw_named("Wrong size of phi!");
    phi.setZero();
    Eigen::MatrixXd J;
    update(x, phi, J, false);
}

void SmoothCollisionDistance::update(Eigen::VectorXdRefConst x,
                                     Eigen::VectorXdRef phi,
                                     Eigen::MatrixXdRef J)
{
    if (phi.rows() != dim_) throw_named("Wrong size of phi!");
    phi.setZero();
    J.setZero();

    update(x, phi, J, true);
}

void SmoothCollisionDistance::update(Eigen::VectorXdRefConst x,
                                     Eigen::VectorXdRef phi,
                                     Eigen::MatrixXdRef J,
                                     bool updateJacobian)
{
    double& d = phi(0);
    for (const auto& link : robotLinks_)
    {
        // Get all world collision links, then iterate through them
        // std::vector<CollisionProxy> proxies = cscene_->getCollisionDistance(scene_->getControlledLinkToCollisionLinkMap()[link], check_self_collision_);
        std::vector<CollisionProxy> proxies = cscene_->getCollisionDistance(link, check_self_collision_);

        for (const auto& proxy : proxies)
        {
            bool isRobotToRobot = (proxy.e1->isRobotLink || proxy.e1->ClosestRobotLink.lock()) && (proxy.e2->isRobotLink || proxy.e2->ClosestRobotLink.lock());
            double& margin = isRobotToRobot ? robot_margin_ : world_margin_;
            if (proxy.distance < margin)
            {
                // Cost
                d += std::pow((1. - proxy.distance / margin), linear_ ? 1 : 2);

                if (updateJacobian)
                {
                    // Jacobian
                    KDL::Frame arel = KDL::Frame(proxy.e1->Frame.Inverse(KDL::Vector(
                        proxy.contact1(0), proxy.contact1(1), proxy.contact1(2))));
                    KDL::Frame brel = KDL::Frame(proxy.e2->Frame.Inverse(KDL::Vector(
                        proxy.contact2(0), proxy.contact2(1), proxy.contact2(2))));

                    if (!linear_)
                    {
                        Eigen::MatrixXd tmpJ = scene_->getSolver().Jacobian(
                            proxy.e1, arel, nullptr, KDL::Frame());
                        J += (2. / (margin * margin)) * (proxy.normal1.transpose() * tmpJ);
                        tmpJ = scene_->getSolver().Jacobian(proxy.e2, brel, nullptr,
                                                            KDL::Frame());
                        J -= (2. / (margin * margin)) * (proxy.normal1.transpose() * tmpJ);
                    }
                    else
                    {
                        Eigen::MatrixXd tmpJ = scene_->getSolver().Jacobian(
                            proxy.e1, arel, nullptr, KDL::Frame());
                        J += 1 / margin * (proxy.normal1.transpose() * tmpJ);
                        tmpJ = scene_->getSolver().Jacobian(proxy.e2, brel, nullptr,
                                                            KDL::Frame());
                        J -= 1 / margin * (proxy.normal1.transpose() * tmpJ);
                    }
                }
            }
        }
    }
}

void SmoothCollisionDistance::Instantiate(
    SmoothCollisionDistanceInitializer& init)
{
    init_ = init;
}

void SmoothCollisionDistance::assignScene(Scene_ptr scene)
{
    scene_ = scene;
    Initialize();
}

void SmoothCollisionDistance::Initialize()
{
    cscene_ = scene_->getCollisionScene();
    world_margin_ = init_.WorldMargin;
    robot_margin_ = init_.RobotMargin;
    linear_ = init_.Linear;
    check_self_collision_ = init_.CheckSelfCollision;

    HIGHLIGHT_NAMED("Smooth Collision Distance",
                    "World Margin: " << world_margin_ << " Robot Margin: " << robot_margin_ << "\t Linear: " << linear_);

    // Get names of all controlled joints and their corresponding child links
    robotLinks_ = scene_->getControlledLinkNames();
}

int SmoothCollisionDistance::taskSpaceDim() { return dim_; }
}
<|endoftext|>