<commit_before>#ifndef SiconosVisitables_hpp
#define SiconosVisitables_hpp

#undef REGISTER
#undef REGISTER_STRUCT
#undef REGISTER_BASE
#undef REGISTER_BASE_EXTERN

#define KERNEL_CLASSES()                          \
  REGISTER(DynamicalSystem)                       \
  REGISTER(DynamicalSystemXML)                    \
    REGISTER(Relation)                            \
    REGISTER(NonSmoothLaw)                        \
    REGISTER(MixedComplementarityConditionNSL)    \
    REGISTER(EqualityConditionNSL)                \
    REGISTER(ComplementarityConditionNSL)         \
    REGISTER(RelayNSL)                            \
    REGISTER(NewtonImpactNSL)                     \
    REGISTER(MultipleImpactNSL)                   \
    REGISTER(NewtonImpactFrictionNSL)             \
    REGISTER(Simulation)                          \
    REGISTER(TimeStepping)                        \
    REGISTER(TimeSteppingD1Minus)                 \
    REGISTER(TimeSteppingProjectOnConstraints)    \
    REGISTER(TimeSteppingCombinedProjection)      \
    REGISTER(EventDriven)                         \
    REGISTER(OneStepIntegrator)                   \
    REGISTER(Moreau)                              \
    REGISTER(MoreauProjectOnConstraintsOSI)       \
    REGISTER(MoreauCombinedProjectionOSI)         \
    REGISTER(Lsodar)                              \
    REGISTER(Hem5)                                \
    REGISTER(NewMarkAlphaOSI)                     \
    REGISTER(D1MinusLinear)                       \
    REGISTER(SchatzmanPaoli)                       \
    REGISTER(ZeroOrderHold)                        \
    REGISTER(OneStepNSProblem)                     \
    REGISTER(LinearOSNS)                           \
    REGISTER(LCP)                                  \
    REGISTER(MLCP)                                 \
    REGISTER(MLCPProjectOnConstraints)             \
    REGISTER(OSNSMultipleImpact)                   \
    REGISTER(FrictionContact)                      \
    REGISTER(SiconosVector)                        \
    REGISTER(SimpleMatrix)                         \
    REGISTER(BlockVector)                          \
    REGISTER(BlockMatrix)                          \
    REGISTER(LagrangianDS)                         \
    REGISTER(LagrangianLinearTIDS)                 \
    REGISTER(FirstOrderLinearDS)                   \
    REGISTER(FirstOrderNonLinearDS)                \
    REGISTER(FirstOrderLinearTIDS)                 \
    REGISTER(FirstOrderType1R)                     \
    REGISTER(FirstOrderType2R)                     \
    REGISTER(FirstOrderLinearR)                    \
    REGISTER(FirstOrderLinearTIR)                  \
    REGISTER(LagrangianScleronomousR)              \
    REGISTER(LagrangianRheonomousR)                \
    REGISTER(LagrangianCompliantR)                 \
    REGISTER(LagrangianLinearTIR)                  \
    REGISTER(NewtonEulerDS)                        \
    REGISTER(NewtonEulerR)                         \
    REGISTER_STRUCT(DynamicalSystemsGraph)         \
    REGISTER_STRUCT(InteractionsGraph)             \
    REGISTER_STRUCT(DynamicalSystemsSubGraph)      \
    REGISTER_STRUCT(InteractionsSubGraph)          \

#define MECHANICS_CLASSES()                       \
  REGISTER(DiskPlanR)                             \
  REGISTER(DiskMovingPlanR)                       \
    REGISTER(CircleCircleR)                       \
    REGISTER(DiskDiskR)                           \
    REGISTER(SphereNEDSPlanR)                     \
    REGISTER(SphereNEDSSphereNEDSR)               \
    REGISTER(SphereLDSSphereLDSR)                 \
    REGISTER(SphereLDSPlanR)                      \
    REGISTER_BASE(Disk, LagrangianDS)             \
    REGISTER_BASE(CircularDS, LagrangianDS)       \
    REGISTER_BASE(Circle, LagrangianDS)           \
    REGISTER_BASE(SphereLDS, LagrangianDS)        \
    REGISTER_BASE(SphereNEDS, NewtonEulerDS)      \

#define BULLET_CLASSES()                          \
  REGISTER(SpaceFilter)                           \
  REGISTER(BulletSpaceFilter)                     \
    REGISTER(BulletR)                             \
    REGISTER_BASE(BulletDS, NewtonEulerDS)

#define LMGC_CLASSES()                            \
  REGISTER_BASE_EXTERN(Lmgc2DDS, LagrangianDS)    \
  REGISTER_BASE_EXTERN(Lmgc2DDSK, LagrangianDS)   \
  REGISTER_BASE_EXTERN(Lmgc2DPOLYG, LagrangianDS) \
  REGISTER(Lmgc2DR)

#define SICONOS_VISITABLES()                    \
  KERNEL_CLASSES()                              \
  MECHANICS_CLASSES()                           \
  BULLET_CLASSES()                              \
  LMGC_CLASSES()

#endif
<commit_msg>IO compilation (cdash)<commit_after>#ifndef SiconosVisitables_hpp
#define SiconosVisitables_hpp

#undef REGISTER
#undef REGISTER_STRUCT
#undef REGISTER_BASE
#undef REGISTER_BASE_EXTERN

#define KERNEL_CLASSES()                          \
  REGISTER(DynamicalSystem)                       \
  REGISTER(DynamicalSystemXML)                    \
    REGISTER(Relation)                            \
    REGISTER(NonSmoothLaw)                        \
    REGISTER(MixedComplementarityConditionNSL)    \
    REGISTER(EqualityConditionNSL)                \
    REGISTER(ComplementarityConditionNSL)         \
    REGISTER(RelayNSL)                            \
    REGISTER(NewtonImpactNSL)                     \
    REGISTER(MultipleImpactNSL)                   \
    REGISTER(NewtonImpactFrictionNSL)             \
    REGISTER(Simulation)                          \
    REGISTER(TimeStepping)                        \
    REGISTER(TimeSteppingD1Minus)                 \
    REGISTER(TimeSteppingProjectOnConstraints)    \
    REGISTER(TimeSteppingCombinedProjection)      \
    REGISTER(EventDriven)                         \
    REGISTER(OneStepIntegrator)                   \
    REGISTER(Moreau)                              \
    REGISTER(MoreauProjectOnConstraintsOSI)       \
    REGISTER(MoreauCombinedProjectionOSI)         \
    REGISTER(Lsodar)                              \
    REGISTER(Hem5)                                \
    REGISTER(NewMarkAlphaOSI)                     \
    REGISTER(D1MinusLinear)                       \
    REGISTER(SchatzmanPaoli)                       \
    REGISTER(ZeroOrderHold)                        \
    REGISTER(OneStepNSProblem)                     \
    REGISTER(LinearOSNS)                           \
    REGISTER(LCP)                                  \
    REGISTER(MLCP)                                 \
    REGISTER(MLCPProjectOnConstraints)             \
    REGISTER(OSNSMultipleImpact)                   \
    REGISTER(FrictionContact)                      \
    REGISTER(SiconosVector)                        \
    REGISTER(SimpleMatrix)                         \
    REGISTER(BlockVector)                          \
    REGISTER(BlockMatrix)                          \
    REGISTER(LagrangianDS)                         \
    REGISTER(LagrangianLinearTIDS)                 \
    REGISTER(FirstOrderLinearDS)                   \
    REGISTER(FirstOrderNonLinearDS)                \
    REGISTER(FirstOrderLinearTIDS)                 \
    REGISTER(FirstOrderType1R)                     \
    REGISTER(FirstOrderType2R)                     \
    REGISTER(FirstOrderLinearR)                    \
    REGISTER(FirstOrderLinearTIR)                  \
    REGISTER(LagrangianScleronomousR)              \
    REGISTER(LagrangianRheonomousR)                \
    REGISTER(LagrangianCompliantR)                 \
    REGISTER(LagrangianLinearTIR)                  \
    REGISTER(NewtonEulerDS)                        \
    REGISTER(NewtonEulerR)                         \
    REGISTER_STRUCT(DynamicalSystemsGraph)         \
    REGISTER_STRUCT(InteractionsGraph)             \
    REGISTER_STRUCT(DynamicalSystemsSubGraph)      \
    REGISTER_STRUCT(InteractionsSubGraph)          \

#define MECHANICS_CLASSES()                       \
  REGISTER(SpaceFilter)                           \
  REGISTER(DiskPlanR)                             \
  REGISTER(DiskMovingPlanR)                       \
    REGISTER(CircleCircleR)                       \
    REGISTER(DiskDiskR)                           \
    REGISTER(SphereNEDSPlanR)                     \
    REGISTER(SphereNEDSSphereNEDSR)               \
    REGISTER(SphereLDSSphereLDSR)                 \
    REGISTER(SphereLDSPlanR)                      \
    REGISTER_BASE(Disk, LagrangianDS)             \
    REGISTER_BASE(CircularDS, LagrangianDS)       \
    REGISTER_BASE(Circle, LagrangianDS)           \
    REGISTER_BASE(SphereLDS, LagrangianDS)        \
    REGISTER_BASE(SphereNEDS, NewtonEulerDS)      \

#define BULLET_CLASSES()                          \
  REGISTER(BulletSpaceFilter)                     \
    REGISTER(BulletR)                             \
    REGISTER_BASE(BulletDS, NewtonEulerDS)

#define LMGC_CLASSES()                            \
  REGISTER_BASE_EXTERN(Lmgc2DDS, LagrangianDS)    \
  REGISTER_BASE_EXTERN(Lmgc2DDSK, LagrangianDS)   \
  REGISTER_BASE_EXTERN(Lmgc2DPOLYG, LagrangianDS) \
  REGISTER(Lmgc2DR)

#define SICONOS_VISITABLES()                    \
  KERNEL_CLASSES()                              \
  MECHANICS_CLASSES()                           \
  BULLET_CLASSES()                              \
  LMGC_CLASSES()

#endif
<|endoftext|>


<commit_before>AliAnalysisTaskPHOSPbPbQA* AddTaskPHOSPbPbQA(char* fname="PHOSPbPbQA.root",
					     char* contname=NULL)
{
  //Add PHOS PbPb QA task to the PWG1 QA train.
  //See PHOSPbPb.C how to run it locally or standalone.

  AliAnalysisManager *mgr = AliAnalysisManager::GetAnalysisManager();
  if (!mgr) {
    ::Error("AddTaskPHOSPbPbQA", "No analysis manager to connect to");
    return NULL;
  }

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

  AliAnalysisTaskPHOSPbPbQA* task = new AliAnalysisTaskPHOSPbPbQA("PbPbQA");
  mgr->AddTask(task);

  mgr->ConnectInput(task, 0, mgr->GetCommonInputContainer());

  // container output into particular file
  if (fname && contname)
    mgr->ConnectOutput(task, 1, mgr->CreateContainer(contname,TObjArray::Class(), AliAnalysisManager::kOutputContainer, fname));

  // container output into common file
  if (!fname) {
    if (!contname) contname = "PHOSPbPbQAResults";
    mgr->ConnectOutput(task, 1, mgr->CreateContainer(contname,TObjArray::Class(), AliAnalysisManager::kOutputContainer, mgr->GetCommonFileName()));
  }

  return task;
}
<commit_msg>TObjArray is replaced by TList<commit_after>AliAnalysisTaskPHOSPbPbQA* AddTaskPHOSPbPbQA(char* fname="PHOSPbPbQA.root",
					     char* contname=NULL)
{
  //Add PHOS PbPb QA task to the PWG1 QA train.
  //See PHOSPbPb.C how to run it locally or standalone.

  AliAnalysisManager *mgr = AliAnalysisManager::GetAnalysisManager();
  if (!mgr) {
    ::Error("AddTaskPHOSPbPbQA", "No analysis manager to connect to");
    return NULL;
  }

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

  AliAnalysisTaskPHOSPbPbQA* task = new AliAnalysisTaskPHOSPbPbQA("PbPbQA");
  mgr->AddTask(task);

  mgr->ConnectInput(task, 0, mgr->GetCommonInputContainer());

  // container output into particular file
  if (fname && contname)
    mgr->ConnectOutput(task, 1, mgr->CreateContainer(contname,TList::Class(), AliAnalysisManager::kOutputContainer, fname));

  // container output into common file
  if (!fname) {
    if (!contname) contname = "PHOSPbPbQAResults";
    mgr->ConnectOutput(task, 1, mgr->CreateContainer(contname,TList::Class(), AliAnalysisManager::kOutputContainer, mgr->GetCommonFileName()));
  }

  return task;
}
<|endoftext|>


<commit_before>/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, Willow Garage, Inc.
*  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 Willow Garage, Inc. 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 "rosrecord/Recorder.h"

#include "rosrecord/constants.h"

#include <iomanip>
#include <signal.h>
#include <sys/statvfs.h>

#include <boost/filesystem.hpp>
#include <boost/iostreams/filter/gzip.hpp>
#include <boost/iostreams/filter/bzip2.hpp>

using namespace ros::record;

ros::record::Recorder::Recorder()
  : file_header_pos_(0), index_data_pos_(0), header_buf_(NULL), header_buf_len_(0), header_buf_size_(0),
    message_buf_(NULL), message_buf_len_(0), message_buf_size_(0), free_space_(0)
{
}

ros::record::Recorder::~Recorder()
{
  close();
}

const void* ros::record::Recorder::getHeaderBuffer()
{
  return header_buf_;
}

unsigned int ros::record::Recorder::getHeaderBufferLength()
{
  return header_buf_len_;
}

void ros::record::Recorder::resetHeaderBuffer()
{
  header_buf_len_ = 0;
}

void ros::record::Recorder::writeFieldToHeaderBuffer(const std::string& name, const void* value, unsigned int value_len)
{
  // Do a little buffer-size management.
  unsigned int new_len = header_buf_len_ + name.size() + 1 + 4 + value_len;
  if (header_buf_size_ < new_len)
  {
    if (header_buf_size_ == 0)
      header_buf_size_ = new_len;
    else
    {
      while (header_buf_size_ < new_len)
        header_buf_size_ *= 2;
    }
    header_buf_ = (unsigned char*) realloc(header_buf_, header_buf_size_);
    ROS_ASSERT(header_buf_);
  }

  // Copy in the data as
  //   <name>=<value_len><value>
  // where <value_len> is a 4-byte little-endian integer.
  //
  // (see http://pr.willowgarage.com/wiki/ROS/LogFormat)
  memcpy(header_buf_ + header_buf_len_, name.c_str(), name.size());
  header_buf_len_ += name.size();
  header_buf_[header_buf_len_] = FIELD_DELIM;
  header_buf_len_ += 1;
  memcpy(header_buf_ + header_buf_len_, &value_len, 4);
  header_buf_len_ += 4;
  memcpy(header_buf_ + header_buf_len_, value, value_len);
  header_buf_len_ += value_len;
}

bool ros::record::Recorder::open(const std::string &file_name, bool random_access)
{
  file_name_ = file_name;

  std::string ext = boost::filesystem::extension(file_name);

  store_index_ = !(ext == ".gz" || ext == ".bz2");
  if (store_index_ && random_access)
    record_file_.open(file_name.c_str(), std::ios::in | std::ios::out | std::ios::binary);
  else
    record_file_.open(file_name.c_str(), std::ios::out | std::ios::binary | std::ios::app);

  if (record_file_.fail())
  {
    ROS_FATAL("rosrecord::Record: Failed to open file: %s", file_name.c_str());
    return false;
  }

  if (ext == ".gz")
    record_stream_.push(boost::iostreams::gzip_compressor());
  else if (ext == ".bz2")
    record_stream_.push(boost::iostreams::bzip2_compressor());
  record_stream_.push(record_file_);

  checkDisk();
  check_disk_next_ = ros::WallTime::now() + ros::WallDuration().fromSec(20.0);

  record_pos_ = 0;

  return true;
}

pos_t ros::record::Recorder::getOffset()
{
  return record_pos_;
}

void ros::record::Recorder::close()
{
  // Unfortuantely closing this possibly enormous file takes a while
  // (especially over NFS) and handling of a SIGINT while a file is
  // closing leads to a double free.  So, we disable signals while
  // we close the file.

  // Darwin doesn't have sighandler_t; I hope that sig_t on Linux does
  // the right thing.
  //sighandler_t old = signal(SIGINT, SIG_IGN);
  sig_t old = signal(SIGINT, SIG_IGN);
  if (record_file_.is_open())
  {
    while (!record_stream_.empty())
      record_stream_.pop();
    record_file_.close();
  }
  signal(SIGINT, old);
}

void ros::record::Recorder::writeVersion()
{
  std::string version = std::string("#ROSRECORD V") + VERSION + std::string("\n");
  write(version);
}

bool ros::record::Recorder::checkDisk()
{
  struct statvfs fiData;

  if ((statvfs(file_name_.c_str(), &fiData)) < 0)
  {
    ROS_WARN("rosrecord::Record: Failed to check filesystem stats.");
  }
  else
  {
    free_space_ = (unsigned long long)(fiData.f_bsize) * (unsigned long long)(fiData.f_bavail);

    if (free_space_ < 1073741824ull)
    {
      ROS_ERROR("rosrecord::Record: Less than 1GB of space free on disk with %s.  Disabling logging.", file_name_.c_str());
      return false;
    }
    else if (free_space_ < 5368709120ull)
    {
      ROS_WARN("rosrecord::Record: Less than 5GB of space free on disk with %s.", file_name_.c_str());
    }
  }
  return true;
}

bool ros::record::Recorder::record(std::string topic_name, ros::Message::ConstPtr msg, ros::Time time)
{
  bool needs_def_written = false;
  std::map<std::string, MsgInfo>::iterator key;
  {
    boost::mutex::scoped_lock lock(topics_recorded_mutex_);

    key = topics_recorded_.find(topic_name);

    if (key == topics_recorded_.end())
    {
      MsgInfo& info = topics_recorded_[topic_name];
      info.msg_def  = msg->__getMessageDefinition();
      info.datatype = msg->__getDataType();
      info.md5sum   = msg->__getMD5Sum();

      key = topics_recorded_.find(topic_name);

      topic_indexes_[topic_name] = std::vector<IndexEntry>();

      needs_def_written = true;
    }
  }
  const MsgInfo& msg_info = key->second;

  {
    boost::mutex::scoped_lock lock(check_disk_mutex_);

    if (ros::WallTime::now() > check_disk_next_)
    {
      if (!checkDisk())
        return false;

      check_disk_next_ = check_disk_next_ + ros::WallDuration().fromSec(20.0);
    }
  }

  {
    boost::mutex::scoped_lock lock(record_mutex_);

    // Assemble the header in memory first, because we need to write its length first.

    // Write a message definition record, if necessary
    if (needs_def_written)
    {
      M_string header;
      header[OP_FIELD_NAME]    = std::string((char*)&OP_MSG_DEF, 1);
      header[TOPIC_FIELD_NAME] = topic_name;
      header[MD5_FIELD_NAME]   = msg_info.md5sum;
      header[TYPE_FIELD_NAME]  = msg_info.datatype;
      header[DEF_FIELD_NAME]   = msg_info.msg_def;
      writeHeader(header, 0);
    }

    // Serialize the message into the message buffer
    if (message_buf_size_ < msg->__serialized_length)
    {
      if (message_buf_size_ == 0)
        message_buf_size_ = msg->__serialized_length;
      else
      {
        while (message_buf_size_ < msg->__serialized_length)
          message_buf_size_ *= 2;
      }
      message_buf_ = (unsigned char*)realloc(message_buf_, message_buf_size_);
      ROS_ASSERT(message_buf_);
    }
    msg->serialize(message_buf_, 0);

    // Write a message instance record
    M_string header;
    header[OP_FIELD_NAME]    = std::string((char*)&OP_MSG_DATA, 1);
    header[TOPIC_FIELD_NAME] = topic_name;
    header[MD5_FIELD_NAME]   = msg_info.md5sum;
    header[TYPE_FIELD_NAME]  = msg_info.datatype;
    header[SEC_FIELD_NAME]   = std::string((char*)&time.sec, 4);
    header[NSEC_FIELD_NAME]  = std::string((char*)&time.nsec, 4);
    writeRecord(header, (char*)message_buf_, msg->__serialized_length);
    if (record_file_.fail())
    {
      ROS_FATAL("rosrecord::Record: could not write to file.  Check permissions and diskspace\n");
      return false;
    }

    // Add to topic index
    IndexEntry index_entry;
    index_entry.sec  = time.sec;
    index_entry.nsec = time.nsec;
    index_entry.pos  = record_pos_;
    topic_indexes_[topic_name].push_back(index_entry);
  }

  return true;
}

void ros::record::Recorder::writeFileHeader()
{
  boost::mutex::scoped_lock lock(record_mutex_);

  // Remember position to file header record
  file_header_pos_ = record_pos_;

  // Write file header record
  M_string header;
  header[OP_FIELD_NAME]        = std::string((char*)&OP_FILE_HEADER, 1);
  header[INDEX_POS_FIELD_NAME] = std::string((char*)&index_data_pos_, 8);
  writeHeader(header, 0);

  // Pad the file header record out
  uint32_t padding_length = FILE_HEADER_LENGTH - (record_pos_ - file_header_pos_);
  std::string padding;
  padding.resize(padding_length, ' ');
  write(padding);
}

void ros::record::Recorder::writeIndex()
{
	{
		boost::mutex::scoped_lock lock(record_mutex_);

		// Remember position to first index record
		index_data_pos_ = record_pos_;

		for (std::map<std::string, std::vector<IndexEntry> >::const_iterator i = topic_indexes_.begin(); i != topic_indexes_.end(); i++)
		{
			const std::string&             topic_name  = i->first;
			const std::vector<IndexEntry>& topic_index = i->second;

			uint32_t topic_index_size = topic_index.size();

			// Write the index record header
			M_string header;
			header[OP_FIELD_NAME]    = std::string((char*)&OP_INDEX_DATA, 1);
			header[TOPIC_FIELD_NAME] = topic_name;
			header[VER_FIELD_NAME]   = std::string((char*)&INDEX_VERSION, 4);
			header[COUNT_FIELD_NAME] = std::string((char*)&topic_index_size, 4);

			uint32_t data_len = topic_index_size * sizeof(IndexEntry);
			writeHeader(header, data_len);

			// Write the index record data (pairs of timestamp and position in file)
			for (std::vector<IndexEntry>::const_iterator j = topic_index.begin(); j != topic_index.end(); j++)
			{
				const IndexEntry& index_entry = *j;
				write((char*)&index_entry.sec,  4);
				write((char*)&index_entry.nsec, 4);
				write((char*)&index_entry.pos,  8);
			}
		}
	}

	// Re-open the file for random access, and rewrite the file header to point to the first index data message
  close();
  open(file_name_, true);
  seek(file_header_pos_);
  writeFileHeader();
}

//

void ros::record::Recorder::writeRecord(const M_string& fields, char* data, uint32_t data_len)
{
  writeHeader(fields, data_len);
  write(data, data_len);
}

void ros::record::Recorder::writeHeader(const M_string& fields, uint32_t data_len)
{
  boost::shared_array<uint8_t> header_buffer;
  uint32_t header_len;
  Header::write(fields, header_buffer, header_len);

  write((char*)&header_len, 4);
  write((char*)header_buffer.get(), header_len);
  write((char*)&data_len, 4);
}

void ros::record::Recorder::write(const char* s, std::streamsize n)
{
  record_stream_.write(s, n);
  record_pos_ += n;
}

void ros::record::Recorder::write(const std::string& s)
{
  write(s.c_str(), s.length());
}

void ros::record::Recorder::seek(pos_t pos)
{
  record_file_.seekp(pos, std::ios_base::beg);
  record_pos_ = pos;
}
<commit_msg>rosrecord: pad the header correctly; point first index entry to message definition record<commit_after>/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, Willow Garage, Inc.
*  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 Willow Garage, Inc. 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 "rosrecord/Recorder.h"

#include "rosrecord/constants.h"

#include <iomanip>
#include <signal.h>
#include <sys/statvfs.h>

#include <boost/filesystem.hpp>
#include <boost/iostreams/filter/gzip.hpp>
#include <boost/iostreams/filter/bzip2.hpp>

using namespace ros::record;

ros::record::Recorder::Recorder()
  : file_header_pos_(0), index_data_pos_(0), header_buf_(NULL), header_buf_len_(0), header_buf_size_(0),
    message_buf_(NULL), message_buf_len_(0), message_buf_size_(0), free_space_(0)
{
}

ros::record::Recorder::~Recorder()
{
  close();
}

const void* ros::record::Recorder::getHeaderBuffer()
{
  return header_buf_;
}

unsigned int ros::record::Recorder::getHeaderBufferLength()
{
  return header_buf_len_;
}

void ros::record::Recorder::resetHeaderBuffer()
{
  header_buf_len_ = 0;
}

void ros::record::Recorder::writeFieldToHeaderBuffer(const std::string& name, const void* value, unsigned int value_len)
{
  // Do a little buffer-size management.
  unsigned int new_len = header_buf_len_ + name.size() + 1 + 4 + value_len;
  if (header_buf_size_ < new_len)
  {
    if (header_buf_size_ == 0)
      header_buf_size_ = new_len;
    else
    {
      while (header_buf_size_ < new_len)
        header_buf_size_ *= 2;
    }
    header_buf_ = (unsigned char*) realloc(header_buf_, header_buf_size_);
    ROS_ASSERT(header_buf_);
  }

  // Copy in the data as
  //   <name>=<value_len><value>
  // where <value_len> is a 4-byte little-endian integer.
  //
  // (see http://pr.willowgarage.com/wiki/ROS/LogFormat)
  memcpy(header_buf_ + header_buf_len_, name.c_str(), name.size());
  header_buf_len_ += name.size();
  header_buf_[header_buf_len_] = FIELD_DELIM;
  header_buf_len_ += 1;
  memcpy(header_buf_ + header_buf_len_, &value_len, 4);
  header_buf_len_ += 4;
  memcpy(header_buf_ + header_buf_len_, value, value_len);
  header_buf_len_ += value_len;
}

bool ros::record::Recorder::open(const std::string &file_name, bool random_access)
{
  file_name_ = file_name;

  std::string ext = boost::filesystem::extension(file_name);

  store_index_ = !(ext == ".gz" || ext == ".bz2");
  if (store_index_ && random_access)
    record_file_.open(file_name.c_str(), std::ios::in | std::ios::out | std::ios::binary);
  else
    record_file_.open(file_name.c_str(), std::ios::out | std::ios::binary | std::ios::app);

  if (record_file_.fail())
  {
    ROS_FATAL("rosrecord::Record: Failed to open file: %s", file_name.c_str());
    return false;
  }

  if (ext == ".gz")
    record_stream_.push(boost::iostreams::gzip_compressor());
  else if (ext == ".bz2")
    record_stream_.push(boost::iostreams::bzip2_compressor());
  record_stream_.push(record_file_);

  checkDisk();
  check_disk_next_ = ros::WallTime::now() + ros::WallDuration().fromSec(20.0);

  record_pos_ = 0;

  return true;
}

pos_t ros::record::Recorder::getOffset()
{
  return record_pos_;
}

void ros::record::Recorder::close()
{
  // Unfortuantely closing this possibly enormous file takes a while
  // (especially over NFS) and handling of a SIGINT while a file is
  // closing leads to a double free.  So, we disable signals while
  // we close the file.

  // Darwin doesn't have sighandler_t; I hope that sig_t on Linux does
  // the right thing.
  //sighandler_t old = signal(SIGINT, SIG_IGN);
  sig_t old = signal(SIGINT, SIG_IGN);
  if (record_file_.is_open())
  {
    while (!record_stream_.empty())
      record_stream_.pop();
    record_file_.close();
  }
  signal(SIGINT, old);
}

void ros::record::Recorder::writeVersion()
{
  std::string version = std::string("#ROSRECORD V") + VERSION + std::string("\n");
  write(version);
}

bool ros::record::Recorder::checkDisk()
{
  struct statvfs fiData;

  if ((statvfs(file_name_.c_str(), &fiData)) < 0)
  {
    ROS_WARN("rosrecord::Record: Failed to check filesystem stats.");
  }
  else
  {
    free_space_ = (unsigned long long)(fiData.f_bsize) * (unsigned long long)(fiData.f_bavail);

    if (free_space_ < 1073741824ull)
    {
      ROS_ERROR("rosrecord::Record: Less than 1GB of space free on disk with %s.  Disabling logging.", file_name_.c_str());
      return false;
    }
    else if (free_space_ < 5368709120ull)
    {
      ROS_WARN("rosrecord::Record: Less than 5GB of space free on disk with %s.", file_name_.c_str());
    }
  }
  return true;
}

bool ros::record::Recorder::record(std::string topic_name, ros::Message::ConstPtr msg, ros::Time time)
{
  bool needs_def_written = false;
  std::map<std::string, MsgInfo>::iterator key;
  {
    boost::mutex::scoped_lock lock(topics_recorded_mutex_);

    key = topics_recorded_.find(topic_name);

    if (key == topics_recorded_.end())
    {
      MsgInfo& info = topics_recorded_[topic_name];
      info.msg_def  = msg->__getMessageDefinition();
      info.datatype = msg->__getDataType();
      info.md5sum   = msg->__getMD5Sum();

      key = topics_recorded_.find(topic_name);

      topic_indexes_[topic_name] = std::vector<IndexEntry>();

      needs_def_written = true;
    }
  }
  const MsgInfo& msg_info = key->second;

  {
    boost::mutex::scoped_lock lock(check_disk_mutex_);

    if (ros::WallTime::now() > check_disk_next_)
    {
      if (!checkDisk())
        return false;

      check_disk_next_ = check_disk_next_ + ros::WallDuration().fromSec(20.0);
    }
  }

  {
    boost::mutex::scoped_lock lock(record_mutex_);

    // Assemble the header in memory first, because we need to write its length first.

    // Add to topic index
    IndexEntry index_entry;
    index_entry.sec  = time.sec;
    index_entry.nsec = time.nsec;
    index_entry.pos  = record_pos_;
    topic_indexes_[topic_name].push_back(index_entry);

    // Write a message definition record, if necessary
    if (needs_def_written)
    {
      M_string header;
      header[OP_FIELD_NAME]    = std::string((char*)&OP_MSG_DEF, 1);
      header[TOPIC_FIELD_NAME] = topic_name;
      header[MD5_FIELD_NAME]   = msg_info.md5sum;
      header[TYPE_FIELD_NAME]  = msg_info.datatype;
      header[DEF_FIELD_NAME]   = msg_info.msg_def;
      writeHeader(header, 0);
    }

    // Serialize the message into the message buffer
    if (message_buf_size_ < msg->__serialized_length)
    {
      if (message_buf_size_ == 0)
        message_buf_size_ = msg->__serialized_length;
      else
      {
        while (message_buf_size_ < msg->__serialized_length)
          message_buf_size_ *= 2;
      }
      message_buf_ = (unsigned char*)realloc(message_buf_, message_buf_size_);
      ROS_ASSERT(message_buf_);
    }
    msg->serialize(message_buf_, 0);

    // Write a message instance record
    M_string header;
    header[OP_FIELD_NAME]    = std::string((char*)&OP_MSG_DATA, 1);
    header[TOPIC_FIELD_NAME] = topic_name;
    header[MD5_FIELD_NAME]   = msg_info.md5sum;
    header[TYPE_FIELD_NAME]  = msg_info.datatype;
    header[SEC_FIELD_NAME]   = std::string((char*)&time.sec, 4);
    header[NSEC_FIELD_NAME]  = std::string((char*)&time.nsec, 4);
    writeRecord(header, (char*)message_buf_, msg->__serialized_length);
    if (record_file_.fail())
    {
      ROS_FATAL("rosrecord::Record: could not write to file.  Check permissions and diskspace\n");
      return false;
    }
  }

  return true;
}

void ros::record::Recorder::writeFileHeader()
{
  boost::mutex::scoped_lock lock(record_mutex_);

  // Remember position to file header record
  file_header_pos_ = record_pos_;

  // Write file header record
  M_string header;
  header[OP_FIELD_NAME]        = std::string((char*)&OP_FILE_HEADER, 1);
  header[INDEX_POS_FIELD_NAME] = std::string((char*)&index_data_pos_, 8);

  boost::shared_array<uint8_t> header_buffer;
  uint32_t header_len;
  Header::write(header, header_buffer, header_len);
  uint32_t data_len = 0;
  if (header_len < FILE_HEADER_LENGTH)
    data_len = FILE_HEADER_LENGTH - header_len;
  write((char*)&header_len, 4);
  write((char*)header_buffer.get(), header_len);
  write((char*)&data_len, 4);

  // Pad the file header record out
  std::string padding;
  padding.resize(data_len, ' ');
  write(padding);
}

void ros::record::Recorder::writeIndex()
{
	{
		boost::mutex::scoped_lock lock(record_mutex_);

		// Remember position to first index record
		index_data_pos_ = record_pos_;

		for (std::map<std::string, std::vector<IndexEntry> >::const_iterator i = topic_indexes_.begin(); i != topic_indexes_.end(); i++)
		{
			const std::string&             topic_name  = i->first;
			const std::vector<IndexEntry>& topic_index = i->second;

			uint32_t topic_index_size = topic_index.size();

	    const MsgInfo& msg_info = topics_recorded_[topic_name];

	    // Write the index record header
			M_string header;
			header[OP_FIELD_NAME]    = std::string((char*)&OP_INDEX_DATA, 1);
			header[TOPIC_FIELD_NAME] = topic_name;
	    header[TYPE_FIELD_NAME]  = msg_info.datatype;
			header[VER_FIELD_NAME]   = std::string((char*)&INDEX_VERSION, 4);
			header[COUNT_FIELD_NAME] = std::string((char*)&topic_index_size, 4);

			uint32_t data_len = topic_index_size * sizeof(IndexEntry);
			writeHeader(header, data_len);

			// Write the index record data (pairs of timestamp and position in file)
			for (std::vector<IndexEntry>::const_iterator j = topic_index.begin(); j != topic_index.end(); j++)
			{
				const IndexEntry& index_entry = *j;
				write((char*)&index_entry.sec,  4);
				write((char*)&index_entry.nsec, 4);
				write((char*)&index_entry.pos,  8);
			}
		}
	}

	// Re-open the file for random access, and rewrite the file header to point to the first index data message
  close();
  open(file_name_, true);
  seek(file_header_pos_);
  writeFileHeader();
}

//

void ros::record::Recorder::writeRecord(const M_string& fields, char* data, uint32_t data_len)
{
  writeHeader(fields, data_len);
  write(data, data_len);
}

void ros::record::Recorder::writeHeader(const M_string& fields, uint32_t data_len)
{
  boost::shared_array<uint8_t> header_buffer;
  uint32_t header_len;
  Header::write(fields, header_buffer, header_len);

  write((char*)&header_len, 4);
  write((char*)header_buffer.get(), header_len);
  write((char*)&data_len, 4);
}

void ros::record::Recorder::write(const char* s, std::streamsize n)
{
  record_stream_.write(s, n);
  record_pos_ += n;
}

void ros::record::Recorder::write(const std::string& s)
{
  write(s.c_str(), s.length());
}

void ros::record::Recorder::seek(pos_t pos)
{
  record_file_.seekp(pos, std::ios_base::beg);
  record_pos_ = pos;
}
<|endoftext|>


<commit_before>/*
 * Copyright (c) Facebook, Inc. and its affiliates.
 *
 * This source code is licensed under the MIT license found in the
 * LICENSE file in the root directory of this source tree.
 */

#include "MountingCoordinator.h"

#ifdef RN_SHADOW_TREE_INTROSPECTION
#include <glog/logging.h>
#endif

#include <condition_variable>

#include <react/mounting/Differentiator.h>
#include <react/mounting/ShadowViewMutation.h>
#include <react/utils/TimeUtils.h>

namespace facebook {
namespace react {

MountingCoordinator::MountingCoordinator(ShadowTreeRevision baseRevision)
    : surfaceId_(baseRevision.getRootShadowNode().getSurfaceId()),
      baseRevision_(baseRevision) {
#ifdef RN_SHADOW_TREE_INTROSPECTION
  stubViewTree_ = stubViewTreeFromShadowNode(baseRevision_.getRootShadowNode());
#endif
}

SurfaceId MountingCoordinator::getSurfaceId() const {
  return surfaceId_;
}

void MountingCoordinator::push(ShadowTreeRevision &&revision) const {
  {
    std::lock_guard<std::mutex> lock(mutex_);

    assert(revision.getNumber() > baseRevision_.getNumber());
    assert(
        !lastRevision_.has_value() ||
        revision.getNumber() != lastRevision_->getNumber());

    if (!lastRevision_.has_value() ||
        lastRevision_->getNumber() < revision.getNumber()) {
      lastRevision_ = std::move(revision);
    }
  }

  signal_.notify_all();
}

void MountingCoordinator::revoke() const {
  std::lock_guard<std::mutex> lock(mutex_);
  lastRevision_.reset();
}

bool MountingCoordinator::waitForTransaction(
    std::chrono::duration<double> timeout) const {
  std::unique_lock<std::mutex> lock(mutex_);
  return signal_.wait_for(
      lock, timeout, [this]() { return lastRevision_.has_value(); });
}

better::optional<MountingTransaction> MountingCoordinator::pullTransaction()
    const {
  std::lock_guard<std::mutex> lock(mutex_);

  if (!lastRevision_.has_value()) {
    return {};
  }

  number_++;

  auto telemetry = lastRevision_->getTelemetry();
  telemetry.willDiff();

  auto mutations = calculateShadowViewMutations(
      baseRevision_.getRootShadowNode(), lastRevision_->getRootShadowNode());

  telemetry.didDiff();

#ifdef RN_SHADOW_TREE_INTROSPECTION
  stubViewTree_.mutate(mutations);
  auto stubViewTree =
      stubViewTreeFromShadowNode(lastRevision_->getRootShadowNode());
  if (stubViewTree_ != stubViewTree) {
    LOG(ERROR) << "Old tree:"
               << "\n"
               << baseRevision_.getRootShadowNode().getDebugDescription()
               << "\n";
    LOG(ERROR) << "New tree:"
               << "\n"
               << lastRevision_->getRootShadowNode().getDebugDescription()
               << "\n";
    LOG(ERROR) << "Mutations:"
               << "\n"
               << getDebugDescription(mutations);
    assert(false);
  }
#endif

  baseRevision_ = std::move(*lastRevision_);
  lastRevision_.reset();

  return MountingTransaction{
      surfaceId_, number_, std::move(mutations), telemetry};
}

} // namespace react
} // namespace facebook
<commit_msg>Fabric: Resetting *all* ShadowNode pointers in `MountingCoordinator::revoke`<commit_after>/*
 * Copyright (c) Facebook, Inc. and its affiliates.
 *
 * This source code is licensed under the MIT license found in the
 * LICENSE file in the root directory of this source tree.
 */

#include "MountingCoordinator.h"

#ifdef RN_SHADOW_TREE_INTROSPECTION
#include <glog/logging.h>
#endif

#include <condition_variable>

#include <react/mounting/Differentiator.h>
#include <react/mounting/ShadowViewMutation.h>
#include <react/utils/TimeUtils.h>

namespace facebook {
namespace react {

MountingCoordinator::MountingCoordinator(ShadowTreeRevision baseRevision)
    : surfaceId_(baseRevision.getRootShadowNode().getSurfaceId()),
      baseRevision_(baseRevision) {
#ifdef RN_SHADOW_TREE_INTROSPECTION
  stubViewTree_ = stubViewTreeFromShadowNode(baseRevision_.getRootShadowNode());
#endif
}

SurfaceId MountingCoordinator::getSurfaceId() const {
  return surfaceId_;
}

void MountingCoordinator::push(ShadowTreeRevision &&revision) const {
  {
    std::lock_guard<std::mutex> lock(mutex_);

    assert(revision.getNumber() > baseRevision_.getNumber());
    assert(
        !lastRevision_.has_value() ||
        revision.getNumber() != lastRevision_->getNumber());

    if (!lastRevision_.has_value() ||
        lastRevision_->getNumber() < revision.getNumber()) {
      lastRevision_ = std::move(revision);
    }
  }

  signal_.notify_all();
}

void MountingCoordinator::revoke() const {
  std::lock_guard<std::mutex> lock(mutex_);
  // We have two goals here.
  // 1. We need to stop retaining `ShadowNode`s to not prolong their lifetime
  // to prevent them from overliving `ComponentDescriptor`s.
  // 2. A possible call to `pullTransaction()` should return empty optional.
  baseRevision_.rootShadowNode_.reset();
  lastRevision_.reset();
}

bool MountingCoordinator::waitForTransaction(
    std::chrono::duration<double> timeout) const {
  std::unique_lock<std::mutex> lock(mutex_);
  return signal_.wait_for(
      lock, timeout, [this]() { return lastRevision_.has_value(); });
}

better::optional<MountingTransaction> MountingCoordinator::pullTransaction()
    const {
  std::lock_guard<std::mutex> lock(mutex_);

  if (!lastRevision_.has_value()) {
    return {};
  }

  number_++;

  auto telemetry = lastRevision_->getTelemetry();
  telemetry.willDiff();

  auto mutations = calculateShadowViewMutations(
      baseRevision_.getRootShadowNode(), lastRevision_->getRootShadowNode());

  telemetry.didDiff();

#ifdef RN_SHADOW_TREE_INTROSPECTION
  stubViewTree_.mutate(mutations);
  auto stubViewTree =
      stubViewTreeFromShadowNode(lastRevision_->getRootShadowNode());
  if (stubViewTree_ != stubViewTree) {
    LOG(ERROR) << "Old tree:"
               << "\n"
               << baseRevision_.getRootShadowNode().getDebugDescription()
               << "\n";
    LOG(ERROR) << "New tree:"
               << "\n"
               << lastRevision_->getRootShadowNode().getDebugDescription()
               << "\n";
    LOG(ERROR) << "Mutations:"
               << "\n"
               << getDebugDescription(mutations);
    assert(false);
  }
#endif

  baseRevision_ = std::move(*lastRevision_);
  lastRevision_.reset();

  return MountingTransaction{
      surfaceId_, number_, std::move(mutations), telemetry};
}

} // namespace react
} // namespace facebook
<|endoftext|>


<commit_before>/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2012, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage 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.
*********************************************************************/

/* Author: Ioan Sucan */

#include <planning_request_adapter/planning_request_adapter.h>
#include <boost/math/constants/constants.hpp>
#include <planning_models/conversions.h>
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>

namespace default_planner_request_adapters
{

class FixStartStateBoundsPlanningRequestAdapter : public planning_request_adapter::PlanningRequestAdapter
{
public:

  static const std::string BOUNDS_PARAM_NAME;
  static const std::string DT_PARAM_NAME;

  FixStartStateBoundsPlanningRequestAdapter(void) : planning_request_adapter::PlanningRequestAdapter(), nh_("~")
  {
    if (!nh_.getParam(BOUNDS_PARAM_NAME, bounds_dist_))
    {
      bounds_dist_ = 1e-3;
      ROS_INFO_STREAM("Param '" << BOUNDS_PARAM_NAME << "' was not set. Using default value: " << bounds_dist_);
    }
    else
      ROS_INFO_STREAM("Param '" << BOUNDS_PARAM_NAME << "' was set to " << bounds_dist_);

    if (!nh_.getParam(DT_PARAM_NAME, max_dt_offset_))
    {
      max_dt_offset_ = 0.5;
      ROS_INFO_STREAM("Param '" << DT_PARAM_NAME << "' was not set. Using default value: " << max_dt_offset_);
    }
    else
      ROS_INFO_STREAM("Param '" << DT_PARAM_NAME << "' was set to " << max_dt_offset_);
  }

  virtual std::string getDescription(void) const { return "Fix Start State Bounds"; }

  virtual bool adaptAndPlan(const planning_request_adapter::PlannerFn &planner,
                            const planning_scene::PlanningSceneConstPtr& planning_scene,
                            const moveit_msgs::GetMotionPlan::Request &req,
                            moveit_msgs::GetMotionPlan::Response &res) const
  {
    ROS_DEBUG("Running '%s'", getDescription().c_str());

    // get the specified start state
    planning_models::KinematicState start_state = planning_scene->getCurrentState();
    planning_models::robotStateToKinematicState(*planning_scene->getTransforms(), req.motion_plan_request.start_state, start_state);

    const std::vector<planning_models::KinematicState::JointState*> &jstates = start_state.getJointStateVector();
    std::map<std::string, double> update;
    std::map<std::string, double> continuous_joints;

    for (std::size_t i = 0 ; i < jstates.size() ; ++i)
    {
      const std::vector<double> &vv = jstates[i]->getVariableValues();

      // Check if we have a revolute, continuous joint. If we do, then we only need to make sure
      // it is within de model's declared bounds (usually -Pi, Pi), since the values wrap around.
      // It is possible that the encoder maintains values outside the range [-Pi, Pi], to inform
      // how many times the joint was wrapped. Because of this, we remember the offsets for continuous
      // joints, and we un-do them when the plan comes from the planner

      const planning_models::KinematicModel::JointModel* jm = jstates[i]->getJointModel();
      if (jm->getType() == planning_models::KinematicModel::JointModel::REVOLUTE)
        if (static_cast<const planning_models::KinematicModel::RevoluteJointModel*>(jm)->isContinuous())
        {
          double initial = vv[0];
          jstates[i]->enforceBounds();
          continuous_joints[jstates[i]->getName()] = initial - vv[0];
          continue;
        }

      // Normalize yaw; no offset needs to be remembered
      if (jm->getType() == planning_models::KinematicModel::JointModel::PLANAR)
        static_cast<const planning_models::KinematicModel::PlanarJointModel*>(jm)->normalizeRotation(jstates[i]->getVariableValues());

      // Normalize quaternions
      if (jm->getType() == planning_models::KinematicModel::JointModel::FLOATING)
        static_cast<const planning_models::KinematicModel::FloatingJointModel*>(jm)->normalizeRotation(jstates[i]->getVariableValues());

      const std::vector<std::string> &vn = jstates[i]->getVariableNames();
      const std::vector<std::pair<double, double> > &vb = jstates[i]->getVariableBounds();

      // we have some other type of joint;
      for (std::size_t j = 0 ; j < vn.size() ; ++j)
      {
        // we make sure the bounds are ok
        if (vb[j].first > vv[j])
        {
          if (vb[j].first - vv[j] < bounds_dist_)
          {
            ROS_INFO("Starting state is just outside bounds (variable '%s'). Assuming within bounds.", vn[j].c_str());
            update[vn[j]] = std::min(vb[j].first + std::numeric_limits<double>::epsilon(),  vb[j].second);
          }
          else
            ROS_WARN("Variable '%s' from the starting state is outside bounds by a significant margin: %lf < %lf (by more than %lf). "
                     "Set ~%s on the param server to update the error margin.", vn[j].c_str(), vv[j], vb[j].first, bounds_dist_, BOUNDS_PARAM_NAME.c_str());
        }

        if (vb[j].second < vv[j])
        {
          if (vv[j] - vb[j].second < bounds_dist_)
          {
            ROS_INFO("Starting state is just outside bounds (variable '%s'). Assuming within bounds.", vn[j].c_str());
            update[vn[j]] = std::max(vb[j].first, vb[j].second - std::numeric_limits<double>::epsilon());
          }
          else
            ROS_WARN("Variable '%s' from the starting state is outside bounds by a significant margin: %lf < %lf (by more than %lf). "
                     "Set ~%s on the param server to update the error margin.", vn[j].c_str(), vv[j], vb[j].first, bounds_dist_, BOUNDS_PARAM_NAME.c_str());
        }
      }
    }

    planning_models::KinematicStatePtr prefix;
    if (!update.empty())
    {
      prefix.reset(new planning_models::KinematicState(start_state));
      start_state.setStateValues(update);
    }

    // make sure we use the full, normalized start state, with potential updates applied
    moveit_msgs::GetMotionPlan::Request req2 = req;
    planning_models::kinematicStateToRobotState(start_state, req2.motion_plan_request.start_state);
    bool solved = planner(planning_scene, req2, res);

    // re-add the prefix state, if it was constructed
    if (prefix)
    {
      if (solved)
      {
        // heuristically decide a duration offset for the trajectory (induced by the additional point added as a prefix to the computed trajectory)
        double d = max_dt_offset_;
        if (res.trajectory.joint_trajectory.points.size() > 1 || res.trajectory.multi_dof_joint_trajectory.points.size() > 1)
        {
          double temp = (res.trajectory.joint_trajectory.points.size() > res.trajectory.multi_dof_joint_trajectory.points.size()) ?
            res.trajectory.joint_trajectory.points.back().time_from_start.toSec() / (double)(res.trajectory.joint_trajectory.points.size() - 1) :
            res.trajectory.multi_dof_joint_trajectory.points.back().time_from_start.toSec() / (double)(res.trajectory.multi_dof_joint_trajectory.points.size() - 1);
          if (temp < d)
            d = temp;
        }
        addPrefixState(*prefix, res, d, planning_scene->getTransforms());
      }
      else
        planning_models::kinematicStateToRobotState(*prefix, res.trajectory_start);
    }

    // re-add continuous joint offsets
    for (std::map<std::string, double>::const_iterator it = continuous_joints.begin() ; it != continuous_joints.end() ; ++it)
    {
      // update the start state with the original request value
      for (std::size_t i = 0 ; i < res.trajectory_start.joint_state.name.size() ; ++i)
        if (res.trajectory_start.joint_state.name[i] == it->first)
        {
          res.trajectory_start.joint_state.position[i] += it->second;
          break;
        }

      for (std::size_t i = 0 ; i < res.trajectory.joint_trajectory.joint_names.size() ; ++i)
        if (res.trajectory.joint_trajectory.joint_names[i] == it->first)
        {
          // unwrap continuous joints
          double running_offset = 0.0;
          for (std::size_t j = 1 ; j < res.trajectory.joint_trajectory.points.size() ; ++j)
          {
            if (res.trajectory.joint_trajectory.points[j - 1].positions[i] >
                res.trajectory.joint_trajectory.points[j].positions[i] + boost::math::constants::pi<double>())
              running_offset += 2.0 * boost::math::constants::pi<double>();
            else
              if (res.trajectory.joint_trajectory.points[j].positions[i] >
                  res.trajectory.joint_trajectory.points[j - 1].positions[i] + boost::math::constants::pi<double>())
                running_offset -= 2.0 * boost::math::constants::pi<double>();
            res.trajectory.joint_trajectory.points[j].positions[i] += running_offset;
          }
          // undo wrapping due to start state
          for (std::size_t j = 0 ; j < res.trajectory.joint_trajectory.points.size() ; ++j)
            res.trajectory.joint_trajectory.points[j].positions[i] += it->second;
          break;
        }
    }
    return solved;
  }

private:

  ros::NodeHandle nh_;
  double bounds_dist_;
  double max_dt_offset_;
};

const std::string FixStartStateBoundsPlanningRequestAdapter::BOUNDS_PARAM_NAME = "start_state_max_bounds_error";
const std::string FixStartStateBoundsPlanningRequestAdapter::DT_PARAM_NAME = "state_state_max_dt";

}

PLUGINLIB_DECLARE_CLASS(default_planner_request_adapters, FixStartStateBoundsPlanningRequestAdapter,
                        default_planner_request_adapters::FixStartStateBoundsPlanningRequestAdapter,
                        planning_request_adapter::PlanningRequestAdapter);
<commit_msg>Fixing bug in wrapping stuff<commit_after>/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2012, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage 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.
*********************************************************************/

/* Author: Ioan Sucan */

#include <planning_request_adapter/planning_request_adapter.h>
#include <boost/math/constants/constants.hpp>
#include <planning_models/conversions.h>
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>

namespace default_planner_request_adapters
{

class FixStartStateBoundsPlanningRequestAdapter : public planning_request_adapter::PlanningRequestAdapter
{
public:

  static const std::string BOUNDS_PARAM_NAME;
  static const std::string DT_PARAM_NAME;

  FixStartStateBoundsPlanningRequestAdapter(void) : planning_request_adapter::PlanningRequestAdapter(), nh_("~")
  {
    if (!nh_.getParam(BOUNDS_PARAM_NAME, bounds_dist_))
    {
      bounds_dist_ = 1e-3;
      ROS_INFO_STREAM("Param '" << BOUNDS_PARAM_NAME << "' was not set. Using default value: " << bounds_dist_);
    }
    else
      ROS_INFO_STREAM("Param '" << BOUNDS_PARAM_NAME << "' was set to " << bounds_dist_);

    if (!nh_.getParam(DT_PARAM_NAME, max_dt_offset_))
    {
      max_dt_offset_ = 0.5;
      ROS_INFO_STREAM("Param '" << DT_PARAM_NAME << "' was not set. Using default value: " << max_dt_offset_);
    }
    else
      ROS_INFO_STREAM("Param '" << DT_PARAM_NAME << "' was set to " << max_dt_offset_);
  }

  virtual std::string getDescription(void) const { return "Fix Start State Bounds"; }

  virtual bool adaptAndPlan(const planning_request_adapter::PlannerFn &planner,
                            const planning_scene::PlanningSceneConstPtr& planning_scene,
                            const moveit_msgs::GetMotionPlan::Request &req,
                            moveit_msgs::GetMotionPlan::Response &res) const
  {
    ROS_DEBUG("Running '%s'", getDescription().c_str());

    // get the specified start state
    planning_models::KinematicState start_state = planning_scene->getCurrentState();
    planning_models::robotStateToKinematicState(*planning_scene->getTransforms(), req.motion_plan_request.start_state, start_state);

    const std::vector<planning_models::KinematicState::JointState*> &jstates = start_state.getJointStateVector();
    std::map<std::string, double> update;
    std::map<std::string, double> continuous_joints;

    for (std::size_t i = 0 ; i < jstates.size() ; ++i)
    {
      const std::vector<double> &vv = jstates[i]->getVariableValues();

      // Check if we have a revolute, continuous joint. If we do, then we only need to make sure
      // it is within de model's declared bounds (usually -Pi, Pi), since the values wrap around.
      // It is possible that the encoder maintains values outside the range [-Pi, Pi], to inform
      // how many times the joint was wrapped. Because of this, we remember the offsets for continuous
      // joints, and we un-do them when the plan comes from the planner

      const planning_models::KinematicModel::JointModel* jm = jstates[i]->getJointModel();
      if (jm->getType() == planning_models::KinematicModel::JointModel::REVOLUTE)
        if (static_cast<const planning_models::KinematicModel::RevoluteJointModel*>(jm)->isContinuous())
        {
          double initial = vv[0];
          jstates[i]->enforceBounds();
          continuous_joints[jstates[i]->getName()] = initial - vv[0];
          continue;
        }

      // Normalize yaw; no offset needs to be remembered
      if (jm->getType() == planning_models::KinematicModel::JointModel::PLANAR)
        static_cast<const planning_models::KinematicModel::PlanarJointModel*>(jm)->normalizeRotation(jstates[i]->getVariableValues());

      // Normalize quaternions
      if (jm->getType() == planning_models::KinematicModel::JointModel::FLOATING)
        static_cast<const planning_models::KinematicModel::FloatingJointModel*>(jm)->normalizeRotation(jstates[i]->getVariableValues());

      const std::vector<std::string> &vn = jstates[i]->getVariableNames();
      const std::vector<std::pair<double, double> > &vb = jstates[i]->getVariableBounds();

      // we have some other type of joint;
      for (std::size_t j = 0 ; j < vn.size() ; ++j)
      {
        // we make sure the bounds are ok
        if (vb[j].first > vv[j])
        {
          if (vb[j].first - vv[j] < bounds_dist_)
          {
            ROS_INFO("Starting state is just outside bounds (variable '%s'). Assuming within bounds.", vn[j].c_str());
            update[vn[j]] = std::min(vb[j].first + std::numeric_limits<double>::epsilon(),  vb[j].second);
          }
          else
            ROS_WARN("Variable '%s' from the starting state is outside bounds by a significant margin: %lf < %lf (by more than %lf). "
                     "Set ~%s on the param server to update the error margin.", vn[j].c_str(), vv[j], vb[j].first, bounds_dist_, BOUNDS_PARAM_NAME.c_str());
        }

        if (vb[j].second < vv[j])
        {
          if (vv[j] - vb[j].second < bounds_dist_)
          {
            ROS_INFO("Starting state is just outside bounds (variable '%s'). Assuming within bounds.", vn[j].c_str());
            update[vn[j]] = std::max(vb[j].first, vb[j].second - std::numeric_limits<double>::epsilon());
          }
          else
            ROS_WARN("Variable '%s' from the starting state is outside bounds by a significant margin: %lf < %lf (by more than %lf). "
                     "Set ~%s on the param server to update the error margin.", vn[j].c_str(), vv[j], vb[j].first, bounds_dist_, BOUNDS_PARAM_NAME.c_str());
        }
      }
    }

    planning_models::KinematicStatePtr prefix;
    if (!update.empty())
    {
      prefix.reset(new planning_models::KinematicState(start_state));
      start_state.setStateValues(update);
    }

    // make sure we use the full, normalized start state, with potential updates applied
    moveit_msgs::GetMotionPlan::Request req2 = req;
    planning_models::kinematicStateToRobotState(start_state, req2.motion_plan_request.start_state);
    bool solved = planner(planning_scene, req2, res);

    // re-add the prefix state, if it was constructed
    if (prefix)
    {
      if (solved)
      {
        // heuristically decide a duration offset for the trajectory (induced by the additional point added as a prefix to the computed trajectory)
        double d = max_dt_offset_;
        if (res.trajectory.joint_trajectory.points.size() > 1 || res.trajectory.multi_dof_joint_trajectory.points.size() > 1)
        {
          double temp = (res.trajectory.joint_trajectory.points.size() > res.trajectory.multi_dof_joint_trajectory.points.size()) ?
            res.trajectory.joint_trajectory.points.back().time_from_start.toSec() / (double)(res.trajectory.joint_trajectory.points.size() - 1) :
            res.trajectory.multi_dof_joint_trajectory.points.back().time_from_start.toSec() / (double)(res.trajectory.multi_dof_joint_trajectory.points.size() - 1);
          if (temp < d)
            d = temp;
        }
        addPrefixState(*prefix, res, d, planning_scene->getTransforms());
      }
      else
        planning_models::kinematicStateToRobotState(*prefix, res.trajectory_start);
    }

    trajectory_msgs::JointTrajectory orig_trajectory = res.trajectory.joint_trajectory;

    // re-add continuous joint offsets
    for (std::map<std::string, double>::const_iterator it = continuous_joints.begin() ; it != continuous_joints.end() ; ++it)
    {
      // update the start state with the original request value
      for (std::size_t i = 0 ; i < res.trajectory_start.joint_state.name.size() ; ++i)
        if (res.trajectory_start.joint_state.name[i] == it->first)
        {
          res.trajectory_start.joint_state.position[i] += it->second;
          break;
        }

      for (std::size_t i = 0 ; i < res.trajectory.joint_trajectory.joint_names.size() ; ++i)
        if (res.trajectory.joint_trajectory.joint_names[i] == it->first)
        {
          // unwrap continuous joints
          double running_offset = 0.0;
          for (std::size_t j = 1 ; j < res.trajectory.joint_trajectory.points.size() ; ++j)
          {
            if (orig_trajectory.points[j - 1].positions[i] >
                orig_trajectory.points[j].positions[i] + boost::math::constants::pi<double>())
              running_offset += 2.0 * boost::math::constants::pi<double>();
            else
              if (orig_trajectory.points[j].positions[i] >
                  orig_trajectory.points[j - 1].positions[i] + boost::math::constants::pi<double>())
                running_offset -= 2.0 * boost::math::constants::pi<double>();
            res.trajectory.joint_trajectory.points[j].positions[i] += running_offset;
          }
          // undo wrapping due to start state
          for (std::size_t j = 0 ; j < res.trajectory.joint_trajectory.points.size() ; ++j)
            res.trajectory.joint_trajectory.points[j].positions[i] += it->second;
          break;
        }
    }
    return solved;
  }

private:

  ros::NodeHandle nh_;
  double bounds_dist_;
  double max_dt_offset_;
};

const std::string FixStartStateBoundsPlanningRequestAdapter::BOUNDS_PARAM_NAME = "start_state_max_bounds_error";
const std::string FixStartStateBoundsPlanningRequestAdapter::DT_PARAM_NAME = "state_state_max_dt";

}

PLUGINLIB_DECLARE_CLASS(default_planner_request_adapters, FixStartStateBoundsPlanningRequestAdapter,
                        default_planner_request_adapters::FixStartStateBoundsPlanningRequestAdapter,
                        planning_request_adapter::PlanningRequestAdapter);
<|endoftext|>


<commit_before>//-----------------------------------------------
//
//	This file is part of the Siv3D Engine.
//
//	Copyright (c) 2008-2017 Ryo Suzuki
//	Copyright (c) 2016-2017 OpenSiv3D Project
//
//	Licensed under the MIT License.
//
//-----------------------------------------------

# include <Siv3D/Platform.hpp>
# if defined(SIV3D_TARGET_WINDOWS)

# define  NOMINMAX
# define  STRICT
# define  _WIN32_WINNT _WIN32_WINNT_WIN7
# define  NTDDI_VERSION NTDDI_WIN7
# include <Windows.h>
# include "../Siv3DEngine.hpp"
# include "../Window/IWindow.hpp"
# include "CClipboard_Windows.hpp"
# include <Siv3D/FileSystem.hpp>
# include <Siv3D/ReaderView.hpp>

namespace s3d
{
	namespace detail
	{
		static bool HasInvalidPremultipliedColors(const Color* image, const size_t num_pixels)
		{
			const Color* pSrc = image;
			const Color* const pSrcEnd = pSrc + num_pixels;

			while (pSrc != pSrcEnd)
			{
				if (pSrc->r > pSrc->a || pSrc->g > pSrc->a || pSrc->b > pSrc->a)
				{
					return true;
				}

				++pSrc;
			}

			return false;
		}

		static void LoadImageFromClipboard(Image& image)
		{
			const HANDLE hDIB = ::GetClipboardData(CF_DIB);

			if (!hDIB)
			{
				return;
			}

			const size_t memorySize = ::GlobalSize(hDIB);

			if (const void* memory = reinterpret_cast<uint8*>(::GlobalLock(hDIB)))
			{
				const BITMAPINFO* header = static_cast<const BITMAPINFO*>(memory);
				const int32 depth = header->bmiHeader.biBitCount;

				size_t colorTableSize = 0;

				if (depth == 8)
				{
					colorTableSize = header->bmiHeader.biClrUsed ? header->bmiHeader.biClrUsed
						: (1 << header->bmiHeader.biBitCount);
				}
				else if (depth == 32 && header->bmiHeader.biCompression == BI_BITFIELDS)
				{
					colorTableSize = 3;
				}

				const size_t offsetSize = header->bmiHeader.biSize + colorTableSize * sizeof(RGBQUAD);
				const void* bitmapData = static_cast<const Byte*>(memory) + offsetSize;

				const bool reversed	= header->bmiHeader.biHeight > 0;
				const int32 width	= header->bmiHeader.biWidth;
				const int32 height	= reversed ? header->bmiHeader.biHeight : -header->bmiHeader.biHeight;
				const bool hasAlpha = (depth == 32)
					&& !HasInvalidPremultipliedColors(static_cast<const Color*>(bitmapData), width * height);
				image.resize(width, height);

				ReaderView reader(bitmapData, memorySize - offsetSize);

				switch (depth)
				{
				case 8:
					{
						uint8 palette[1024];
						reader.read(palette);

						const uint32 rowSize = width + (width % 4 ? 4 - width % 4 : 0);
						const int32 lineStep = reversed ? -width : width;
						Color* pDstLine = image[reversed ? height - 1 : 0];
						uint8* const buffer = static_cast<uint8*>(::malloc(rowSize));

						for (int32 y = 0; y < height; ++y)
						{
							if (y == height - 1)
							{
								reader.read(buffer, width);
							}
							else
							{
								reader.read(buffer, rowSize);
							}

							const uint8* pSrc = buffer;
							const Color* const pDstEnd = pDstLine + width;

							for (Color* pDst = pDstLine; pDst != pDstEnd; ++pDst)
							{
								const uint8 *src = palette + ((*pSrc++) << 2);

								pDst->set(src[2], src[1], src[0]);
							}

							pDstLine += lineStep;
						}

						::free(buffer);

						break;
					}
				case 24:
				case 32:
					{
						const size_t rowSize = depth == 24 ? width * 3 + width % 4 : width * 4;
						const int32 depthBytes = depth / 8;
						const int32 lineStep = reversed ? -width : width;
						Color* pDstLine = image[reversed ? height - 1 : 0];
						uint8* const buffer = static_cast<uint8*>(::malloc(rowSize));

						for (int32 y = 0; y < height; ++y)
						{
							if (y == height - 1)
							{
								reader.read(buffer, depthBytes * width);
							}
							else
							{
								reader.read(buffer, rowSize);
							}

							const Color* const pDstEnd = pDstLine + width;
							const uint8* pSrc = buffer;

							for (Color* pDst = pDstLine; pDst != pDstEnd; ++pDst)
							{
								pDst->set(pSrc[2], pSrc[1], pSrc[0], hasAlpha ? pSrc[3] : 255);

								pSrc += depthBytes;
							}

							pDstLine += lineStep;
						}

						::free(buffer);

						break;
					}
				}

			}
			::GlobalUnlock(hDIB);
		}

		static void WriteBitmapToClipboard(HBITMAP hSourceBitmap, const Size& size)
		{
			HDC hDC				= ::GetDC(nullptr);
			HDC hCompatibleDC	= ::CreateCompatibleDC(nullptr);
			HDC hSourceDC		= ::CreateCompatibleDC(nullptr);
			HBITMAP hBitmap		= ::CreateCompatibleBitmap(hDC, size.x, size.y);

			if (!hBitmap)
			{
				::DeleteDC(hCompatibleDC);
				::DeleteDC(hSourceDC);
				::ReleaseDC(nullptr, hDC);
				return;
			}

			HBITMAP hOldBitmap = (HBITMAP)::SelectObject(hCompatibleDC, hBitmap);
			HBITMAP hOldSource = (HBITMAP)::SelectObject(hSourceDC, hSourceBitmap);
			{
				const BLENDFUNCTION blend = { AC_SRC_OVER, 0, 255, AC_SRC_ALPHA };
				::GdiAlphaBlend(hCompatibleDC, 0, 0, size.x, size.y, hSourceDC, 0, 0, size.x, size.y, blend);
			}
			::SelectObject(hCompatibleDC, hOldBitmap);
			::SelectObject(hSourceDC, hOldSource);

			::DeleteObject(hOldBitmap);
			::DeleteObject(hOldSource);
			::DeleteDC(hCompatibleDC);
			::DeleteDC(hSourceDC);
			::ReleaseDC(nullptr, hDC);

			if (::OpenClipboard(nullptr))
			{
				::EmptyClipboard();
				::SetClipboardData(CF_BITMAP, hBitmap);
				::CloseClipboard();
			}

			::DeleteObject(hBitmap);
		}
	}

	CClipboard_Windows::CClipboard_Windows()
	{

	}

	CClipboard_Windows::~CClipboard_Windows()
	{

	}

	bool CClipboard_Windows::init()
	{
		m_sequenceNumber = ::GetClipboardSequenceNumber();

		return true;
	}

	bool CClipboard_Windows::hasChanged()
	{
		const uint32 currentSequenceNumber = ::GetClipboardSequenceNumber();

		const bool hasChanged = (currentSequenceNumber != m_sequenceNumber);

		m_sequenceNumber = currentSequenceNumber;

		return hasChanged;
	}

	bool CClipboard_Windows::getText(String& text)
	{
		text.clear();

		if (::IsClipboardFormatAvailable(CF_TEXT))
		{
			if (!::OpenClipboard(nullptr))
			{
				return false;
			}

			if (const HGLOBAL hGlobal = static_cast<HGLOBAL>(::GetClipboardData(CF_TEXT)))
			{
				text = CharacterSet::Widen(static_cast<char*>(::GlobalLock(hGlobal)));

				::GlobalUnlock(hGlobal);
			}

			::CloseClipboard();
		}

		return !text.isEmpty();
	}

	bool CClipboard_Windows::getImage(Image& image)
	{
		image.clear();

		if (::IsClipboardFormatAvailable(CF_DIB))
		{
			if (!::OpenClipboard(nullptr))
			{
				return false;
			}

			detail::LoadImageFromClipboard(image);

			::CloseClipboard();
		}

		return !image.isEmpty();
	}

	bool CClipboard_Windows::getFilePaths(Array<FilePath>& paths)
	{
		paths.clear();

		if (::IsClipboardFormatAvailable(CF_HDROP))
		{
			if (!::OpenClipboard(nullptr))
			{
				return false;
			}

			if (const HDROP hDrop = (HDROP)::GetClipboardData(CF_HDROP))
			{
				const uint32 dropSize = ::DragQueryFileW(hDrop, ~0u, nullptr, 0);

				paths.reserve(dropSize);

				wchar_t tmpFilePath[MAX_PATH];

				for (uint32 i = 0; i < dropSize; ++i)
				{
					::DragQueryFileW(hDrop, i, tmpFilePath, MAX_PATH);

					paths.push_back(FileSystem::FullPath(tmpFilePath));
				}
			}

			::CloseClipboard();
		}

		return !paths.isEmpty();
	}

	void CClipboard_Windows::setText(const String& text)
	{
		const size_t size = sizeof(char16_t) * (text.length() + 1);

		HANDLE hData = ::GlobalAlloc(GMEM_MOVEABLE, size);

		void* pDst = ::GlobalLock(hData);

		::memcpy(pDst, text.data(), size);

		::GlobalUnlock(hData);

		if (!::OpenClipboard(nullptr))
		{
			::EmptyClipboard();
			::SetClipboardData(CF_UNICODETEXT, hData);
			::CloseClipboard();

			m_sequenceNumber = ::GetClipboardSequenceNumber();
		}

		::GlobalFree(hData);
	}

	void CClipboard_Windows::setImage(const Image& image)
	{
		HDC hDC = ::GetDC(nullptr);
		BITMAPINFO header = {};
		header.bmiHeader.biSize			= sizeof(BITMAPINFOHEADER);
		header.bmiHeader.biWidth		= image.width();
		header.bmiHeader.biHeight		= -image.height();
		header.bmiHeader.biPlanes		= 1;
		header.bmiHeader.biBitCount		= 32;
		header.bmiHeader.biCompression	= BI_RGB;

		void* bitmapData;
		HBITMAP hBitmap = ::CreateDIBSection(hDC, &header, DIB_RGB_COLORS, &bitmapData, nullptr, 0);

		if (bitmapData && hBitmap)
		{
			const Color* pSrc = image.data();
			const Color* pSrcEnd = pSrc + image.num_pixels();
			Color* pDst = static_cast<Color*>(bitmapData);

			while (pSrc != pSrcEnd)
			{
				if (pSrc->a == 255)
				{
					pDst->r = pSrc->b;
					pDst->g = pSrc->g;
					pDst->b = pSrc->r;
					pDst->a = 255;
				}
				else
				{
					const float fa = pSrc->a / 255.0f;
					pDst->r = static_cast<uint8>(pSrc->b * fa);
					pDst->g = static_cast<uint8>(pSrc->g * fa);
					pDst->b = static_cast<uint8>(pSrc->r * fa);
					pDst->a = pSrc->a;
				}

				++pDst;
				++pSrc;
			}

			detail::WriteBitmapToClipboard(hBitmap, image.size());

			m_sequenceNumber = ::GetClipboardSequenceNumber();
		}

		::DeleteObject(hBitmap);
		::ReleaseDC(nullptr, hDC);
	}

	void CClipboard_Windows::clear()
	{
		if (::OpenClipboard(nullptr))
		{
			::EmptyClipboard();
			::CloseClipboard();
		}

		m_sequenceNumber = ::GetClipboardSequenceNumber();
	}
}

# endif
<commit_msg>Clipboard update<commit_after>//-----------------------------------------------
//
//	This file is part of the Siv3D Engine.
//
//	Copyright (c) 2008-2017 Ryo Suzuki
//	Copyright (c) 2016-2017 OpenSiv3D Project
//
//	Licensed under the MIT License.
//
//-----------------------------------------------

# include <Siv3D/Platform.hpp>
# if defined(SIV3D_TARGET_WINDOWS)

# define  NOMINMAX
# define  STRICT
# define  _WIN32_WINNT _WIN32_WINNT_WIN7
# define  NTDDI_VERSION NTDDI_WIN7
# include <Windows.h>
# include "../Siv3DEngine.hpp"
# include "../Window/IWindow.hpp"
# include "CClipboard_Windows.hpp"
# include <Siv3D/FileSystem.hpp>
# include <Siv3D/ReaderView.hpp>

namespace s3d
{
	namespace detail
	{
		static bool HasInvalidPremultipliedColors(const Color* image, const size_t num_pixels)
		{
			const Color* pSrc = image;
			const Color* const pSrcEnd = pSrc + num_pixels;

			while (pSrc != pSrcEnd)
			{
				if (pSrc->r > pSrc->a || pSrc->g > pSrc->a || pSrc->b > pSrc->a)
				{
					return true;
				}

				++pSrc;
			}

			return false;
		}

		static void LoadImageFromClipboard(Image& image)
		{
			const HANDLE hDIB = ::GetClipboardData(CF_DIB);

			if (!hDIB)
			{
				return;
			}

			const size_t memorySize = ::GlobalSize(hDIB);

			if (const void* memory = reinterpret_cast<uint8*>(::GlobalLock(hDIB)))
			{
				const BITMAPINFO* header = static_cast<const BITMAPINFO*>(memory);
				const int32 depth = header->bmiHeader.biBitCount;

				size_t colorTableSize = 0;

				if (depth == 8)
				{
					colorTableSize = header->bmiHeader.biClrUsed ? header->bmiHeader.biClrUsed
						: (1 << header->bmiHeader.biBitCount);
				}
				else if (depth == 32 && header->bmiHeader.biCompression == BI_BITFIELDS)
				{
					colorTableSize = 3;
				}

				const size_t offsetSize = header->bmiHeader.biSize + colorTableSize * sizeof(RGBQUAD);
				const void* bitmapData = static_cast<const Byte*>(memory) + offsetSize;

				const bool reversed	= header->bmiHeader.biHeight > 0;
				const int32 width	= header->bmiHeader.biWidth;
				const int32 height	= reversed ? header->bmiHeader.biHeight : -header->bmiHeader.biHeight;
				const bool hasAlpha = (depth == 32)
					&& !HasInvalidPremultipliedColors(static_cast<const Color*>(bitmapData), width * height);
				image.resize(width, height);

				ReaderView reader(bitmapData, memorySize - offsetSize);

				switch (depth)
				{
				case 8:
					{
						uint8 palette[1024];
						reader.read(palette);

						const uint32 rowSize = width + (width % 4 ? 4 - width % 4 : 0);
						const int32 lineStep = reversed ? -width : width;
						Color* pDstLine = image[reversed ? height - 1 : 0];
						uint8* const buffer = static_cast<uint8*>(::malloc(rowSize));

						for (int32 y = 0; y < height; ++y)
						{
							if (y == height - 1)
							{
								reader.read(buffer, width);
							}
							else
							{
								reader.read(buffer, rowSize);
							}

							const uint8* pSrc = buffer;
							const Color* const pDstEnd = pDstLine + width;

							for (Color* pDst = pDstLine; pDst != pDstEnd; ++pDst)
							{
								const uint8 *src = palette + ((*pSrc++) << 2);

								pDst->set(src[2], src[1], src[0]);
							}

							pDstLine += lineStep;
						}

						::free(buffer);

						break;
					}
				case 24:
				case 32:
					{
						const size_t rowSize = depth == 24 ? width * 3 + width % 4 : width * 4;
						const int32 depthBytes = depth / 8;
						const int32 lineStep = reversed ? -width : width;
						Color* pDstLine = image[reversed ? height - 1 : 0];
						uint8* const buffer = static_cast<uint8*>(::malloc(rowSize));

						for (int32 y = 0; y < height; ++y)
						{
							if (y == height - 1)
							{
								reader.read(buffer, depthBytes * width);
							}
							else
							{
								reader.read(buffer, rowSize);
							}

							const Color* const pDstEnd = pDstLine + width;
							const uint8* pSrc = buffer;

							for (Color* pDst = pDstLine; pDst != pDstEnd; ++pDst)
							{
								pDst->set(pSrc[2], pSrc[1], pSrc[0], hasAlpha ? pSrc[3] : 255);

								pSrc += depthBytes;
							}

							pDstLine += lineStep;
						}

						::free(buffer);

						break;
					}
				}

			}
			::GlobalUnlock(hDIB);
		}

		static void WriteBitmapToClipboard(HBITMAP hSourceBitmap, const Size& size)
		{
			HDC hDC				= ::GetDC(nullptr);
			HDC hCompatibleDC	= ::CreateCompatibleDC(nullptr);
			HDC hSourceDC		= ::CreateCompatibleDC(nullptr);
			HBITMAP hBitmap		= ::CreateCompatibleBitmap(hDC, size.x, size.y);

			if (!hBitmap)
			{
				::DeleteDC(hCompatibleDC);
				::DeleteDC(hSourceDC);
				::ReleaseDC(nullptr, hDC);
				return;
			}

			HBITMAP hOldBitmap = (HBITMAP)::SelectObject(hCompatibleDC, hBitmap);
			HBITMAP hOldSource = (HBITMAP)::SelectObject(hSourceDC, hSourceBitmap);
			{
				const BLENDFUNCTION blend = { AC_SRC_OVER, 0, 255, AC_SRC_ALPHA };
				::GdiAlphaBlend(hCompatibleDC, 0, 0, size.x, size.y, hSourceDC, 0, 0, size.x, size.y, blend);
			}
			::SelectObject(hCompatibleDC, hOldBitmap);
			::SelectObject(hSourceDC, hOldSource);

			::DeleteObject(hOldBitmap);
			::DeleteObject(hOldSource);
			::DeleteDC(hCompatibleDC);
			::DeleteDC(hSourceDC);
			::ReleaseDC(nullptr, hDC);

			if (::OpenClipboard(nullptr))
			{
				::EmptyClipboard();
				::SetClipboardData(CF_BITMAP, hBitmap);
				::CloseClipboard();
			}

			::DeleteObject(hBitmap);
		}
	}

	CClipboard_Windows::CClipboard_Windows()
	{

	}

	CClipboard_Windows::~CClipboard_Windows()
	{

	}

	bool CClipboard_Windows::init()
	{
		m_sequenceNumber = ::GetClipboardSequenceNumber();

		return true;
	}

	bool CClipboard_Windows::hasChanged()
	{
		const uint32 currentSequenceNumber = ::GetClipboardSequenceNumber();

		const bool hasChanged = (currentSequenceNumber != m_sequenceNumber);

		m_sequenceNumber = currentSequenceNumber;

		return hasChanged;
	}

	bool CClipboard_Windows::getText(String& text)
	{
		text.clear();

		if (::IsClipboardFormatAvailable(CF_UNICODETEXT))
		{
			if (!::OpenClipboard(nullptr))
			{
				return false;
			}

			if (const HGLOBAL hGlobal = static_cast<HGLOBAL>(::GetClipboardData(CF_UNICODETEXT)))
			{
				text = static_cast<const wchar_t*>(::GlobalLock(hGlobal));

				::GlobalUnlock(hGlobal);

				text.remove(L'\r');
			}

			::CloseClipboard();
		}

		return !text.isEmpty();
	}

	bool CClipboard_Windows::getImage(Image& image)
	{
		image.clear();

		if (::IsClipboardFormatAvailable(CF_DIB))
		{
			if (!::OpenClipboard(nullptr))
			{
				return false;
			}

			detail::LoadImageFromClipboard(image);

			::CloseClipboard();
		}

		return !image.isEmpty();
	}

	bool CClipboard_Windows::getFilePaths(Array<FilePath>& paths)
	{
		paths.clear();

		if (::IsClipboardFormatAvailable(CF_HDROP))
		{
			if (!::OpenClipboard(nullptr))
			{
				return false;
			}

			if (const HDROP hDrop = (HDROP)::GetClipboardData(CF_HDROP))
			{
				const uint32 dropSize = ::DragQueryFileW(hDrop, ~0u, nullptr, 0);

				paths.reserve(dropSize);

				wchar_t tmpFilePath[MAX_PATH];

				for (uint32 i = 0; i < dropSize; ++i)
				{
					::DragQueryFileW(hDrop, i, tmpFilePath, MAX_PATH);

					paths.push_back(FileSystem::FullPath(tmpFilePath));
				}
			}

			::CloseClipboard();
		}

		return !paths.isEmpty();
	}

	void CClipboard_Windows::setText(const String& text)
	{
		const size_t size = sizeof(char16_t) * (text.length() + 1);

		HANDLE hData = ::GlobalAlloc(GMEM_MOVEABLE, size);

		void* pDst = ::GlobalLock(hData);

		::memcpy(pDst, text.data(), size);

		::GlobalUnlock(hData);

		if (!::OpenClipboard(nullptr))
		{
			::EmptyClipboard();
			::SetClipboardData(CF_UNICODETEXT, hData);
			::CloseClipboard();

			m_sequenceNumber = ::GetClipboardSequenceNumber();
		}

		::GlobalFree(hData);
	}

	void CClipboard_Windows::setImage(const Image& image)
	{
		HDC hDC = ::GetDC(nullptr);
		BITMAPINFO header = {};
		header.bmiHeader.biSize			= sizeof(BITMAPINFOHEADER);
		header.bmiHeader.biWidth		= image.width();
		header.bmiHeader.biHeight		= -image.height();
		header.bmiHeader.biPlanes		= 1;
		header.bmiHeader.biBitCount		= 32;
		header.bmiHeader.biCompression	= BI_RGB;

		void* bitmapData;
		HBITMAP hBitmap = ::CreateDIBSection(hDC, &header, DIB_RGB_COLORS, &bitmapData, nullptr, 0);

		if (bitmapData && hBitmap)
		{
			const Color* pSrc = image.data();
			const Color* pSrcEnd = pSrc + image.num_pixels();
			Color* pDst = static_cast<Color*>(bitmapData);

			while (pSrc != pSrcEnd)
			{
				if (pSrc->a == 255)
				{
					pDst->r = pSrc->b;
					pDst->g = pSrc->g;
					pDst->b = pSrc->r;
					pDst->a = 255;
				}
				else
				{
					const float fa = pSrc->a / 255.0f;
					pDst->r = static_cast<uint8>(pSrc->b * fa);
					pDst->g = static_cast<uint8>(pSrc->g * fa);
					pDst->b = static_cast<uint8>(pSrc->r * fa);
					pDst->a = pSrc->a;
				}

				++pDst;
				++pSrc;
			}

			detail::WriteBitmapToClipboard(hBitmap, image.size());

			m_sequenceNumber = ::GetClipboardSequenceNumber();
		}

		::DeleteObject(hBitmap);
		::ReleaseDC(nullptr, hDC);
	}

	void CClipboard_Windows::clear()
	{
		if (::OpenClipboard(nullptr))
		{
			::EmptyClipboard();
			::CloseClipboard();
		}

		m_sequenceNumber = ::GetClipboardSequenceNumber();
	}
}

# endif
<|endoftext|>


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

  This file is part of the elastix software.

  Copyright (c) University Medical Center Utrecht. All rights reserved.
  See src/CopyrightElastix.txt or http://elastix.isi.uu.nl/legal.php 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.

======================================================================*/

#ifndef __elxAdvancedMattesMutualInformationMetric_HXX__
#define __elxAdvancedMattesMutualInformationMetric_HXX__

#include "elxAdvancedMattesMutualInformationMetric.h"
#include "itkHardLimiterFunction.h"
#include "itkExponentialLimiterFunction.h"
#include <string>
#include "vnl/vnl_math.h"

namespace elastix
{
using namespace itk;

  /**
   * ****************** Constructor ***********************
   */

  template <class TElastix>
    AdvancedMattesMutualInformationMetric<TElastix>
    ::AdvancedMattesMutualInformationMetric()
  {
    this->m_Param_c = 1.0;
    this->m_Param_gamma = 0.101;
    this->SetUseDerivative( true );

  } // end Constructor()

  /**
   * ******************* Initialize ***********************
   */

  template <class TElastix>
    void AdvancedMattesMutualInformationMetric<TElastix>
    ::Initialize( void ) throw (ExceptionObject)
  {
    TimerPointer timer = TimerType::New();
    timer->StartTimer();
    this->Superclass1::Initialize();
    timer->StopTimer();
    elxout << "Initialization of AdvancedMattesMutualInformation metric took: "
      << static_cast<long>( timer->GetElapsedClockSec() * 1000 ) << " ms." << std::endl;

  } // end Initialize()

  /**
   * ***************** BeforeEachResolution ***********************
   */

  template <class TElastix>
    void AdvancedMattesMutualInformationMetric<TElastix>
    ::BeforeEachResolution( void )
  {
    /** Get the current resolution level. */
    unsigned int level =
      ( this->m_Registration->GetAsITKBaseType() )->GetCurrentLevel();

    /** Get and set the number of histogram bins. */
    unsigned int numberOfHistogramBins = 32;
    this->GetConfiguration()->ReadParameter( numberOfHistogramBins,
      "NumberOfHistogramBins", this->GetComponentLabel(), level, 0 );
    this->SetNumberOfFixedHistogramBins( numberOfHistogramBins );
    this->SetNumberOfMovingHistogramBins( numberOfHistogramBins );

    unsigned int numberOfFixedHistogramBins = numberOfHistogramBins;
    unsigned int numberOfMovingHistogramBins = numberOfHistogramBins;
    this->GetConfiguration()->ReadParameter( numberOfFixedHistogramBins,
      "NumberOfFixedHistogramBins", this->GetComponentLabel(), level, 0 );
    this->GetConfiguration()->ReadParameter( numberOfMovingHistogramBins,
      "NumberOfMovingHistogramBins", this->GetComponentLabel(), level, 0 );
    this->SetNumberOfFixedHistogramBins( numberOfFixedHistogramBins );
    this->SetNumberOfMovingHistogramBins( numberOfMovingHistogramBins );

    /** Set limiters. */
    typedef HardLimiterFunction< RealType, FixedImageDimension > FixedLimiterType;
    typedef ExponentialLimiterFunction< RealType, MovingImageDimension > MovingLimiterType;
    this->SetFixedImageLimiter( FixedLimiterType::New() );
    this->SetMovingImageLimiter( MovingLimiterType::New() );

    /** Get and set the limit range ratios. */
    double fixedLimitRangeRatio = 0.01;
    double movingLimitRangeRatio = 0.01;
    this->GetConfiguration()->ReadParameter( fixedLimitRangeRatio,
      "FixedLimitRangeRatio", this->GetComponentLabel(), level, 0 );
    this->GetConfiguration()->ReadParameter( movingLimitRangeRatio,
      "MovingLimitRangeRatio", this->GetComponentLabel(), level, 0 );
    this->SetFixedLimitRangeRatio( fixedLimitRangeRatio );
    this->SetMovingLimitRangeRatio( movingLimitRangeRatio );

    /** Set B-spline Parzen kernel orders. */
    unsigned int fixedKernelBSplineOrder = 0;
    unsigned int movingKernelBSplineOrder = 3;
    this->GetConfiguration()->ReadParameter( fixedKernelBSplineOrder,
      "FixedKernelBSplineOrder", this->GetComponentLabel(), level, 0 );
    this->GetConfiguration()->ReadParameter( movingKernelBSplineOrder,
      "MovingKernelBSplineOrder", this->GetComponentLabel(), level, 0 );
    this->SetFixedKernelBSplineOrder( fixedKernelBSplineOrder );
    this->SetMovingKernelBSplineOrder( movingKernelBSplineOrder );

    /** Set whether a low memory consumption should be used. */
    bool useExplicitPDFDerivatives = true;
    this->GetConfiguration()->ReadParameter( useExplicitPDFDerivatives,
      "UseFastAndLowMemoryVersion", this->GetComponentLabel(), level, 0 );
    this->SetUseExplicitPDFDerivatives( useExplicitPDFDerivatives );

    /** Set whether a finite difference derivative should be used. */
    bool useFiniteDifferenceDerivative = false;
    this->GetConfiguration()->ReadParameter( useFiniteDifferenceDerivative,
      "FiniteDifferenceDerivative", this->GetComponentLabel(), level, 0 );
    this->SetUseFiniteDifferenceDerivative( useFiniteDifferenceDerivative );

    /** Prepare for computing the perturbation gain c_k. */
    this->SetCurrentIteration( 0 );
    if ( useFiniteDifferenceDerivative )
    {
      double c = 1.0;
      double gamma = 0.101;
      this->GetConfiguration()->ReadParameter(c, "SP_c", this->GetComponentLabel(), level, 0 );
      this->GetConfiguration()->ReadParameter(gamma, "SP_gamma", this->GetComponentLabel(), level, 0 );
      this->SetParam_c( c );
      this->SetParam_gamma( gamma );
      this->SetFiniteDifferencePerturbation( this->Compute_c( 0 ) );
    }

    this->SetUseMovingImageDerivativeScales( false );
    MovingImageDerivativeScalesType movingImageDerivativeScales;
    int usescales = 0;
    for ( unsigned int i = 0; i < MovingImageDimension; ++i )
    {
      usescales |= this->GetConfiguration()->ReadParameter( movingImageDerivativeScales[i],
        "MovingImageDerivativeScales", this->GetComponentLabel(), i, -1, true );
    }
    if ( usescales == 0 )
    {
      this->SetUseMovingImageDerivativeScales( true );
      this->SetMovingImageDerivativeScales( movingImageDerivativeScales );
      elxout << "Multiplying moving image derivatives by: " << movingImageDerivativeScales << std::endl;
    }

  } // end BeforeEachResolution()

  /**
   * ***************** AfterEachIteration ***********************
   */

  template <class TElastix>
    void AdvancedMattesMutualInformationMetric<TElastix>
    ::AfterEachIteration(void)
  {
    if ( this->GetUseFiniteDifferenceDerivative() )
    {
      this->m_CurrentIteration++;
      this->SetFiniteDifferencePerturbation(
        this->Compute_c( this->m_CurrentIteration )  );
    }
  }  // end AfterEachIteration()

  /**
   * ************************** Compute_c *************************
   *
   * This function computes the parameter a at iteration k, as
   * in the finite difference optimizer.
   */

  template <class TElastix>
    double AdvancedMattesMutualInformationMetric<TElastix>
    ::Compute_c( unsigned long k ) const
  {
    return static_cast<double>(
      this->m_Param_c / vcl_pow( k + 1, this->m_Param_gamma ) );

  } // end Compute_c()

} // end namespace elastix

#endif // end #ifndef __elxAdvancedMattesMutualInformationMetric_HXX__
<commit_msg>MS:<commit_after>/*======================================================================

  This file is part of the elastix software.

  Copyright (c) University Medical Center Utrecht. All rights reserved.
  See src/CopyrightElastix.txt or http://elastix.isi.uu.nl/legal.php 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.

======================================================================*/

#ifndef __elxAdvancedMattesMutualInformationMetric_HXX__
#define __elxAdvancedMattesMutualInformationMetric_HXX__

#include "elxAdvancedMattesMutualInformationMetric.h"
#include "itkHardLimiterFunction.h"
#include "itkExponentialLimiterFunction.h"
#include <string>
#include "vnl/vnl_math.h"

namespace elastix
{
using namespace itk;

  /**
   * ****************** Constructor ***********************
   */

  template <class TElastix>
    AdvancedMattesMutualInformationMetric<TElastix>
    ::AdvancedMattesMutualInformationMetric()
  {
    this->m_Param_c = 1.0;
    this->m_Param_gamma = 0.101;
    this->SetUseDerivative( true );

  } // end Constructor()

  /**
   * ******************* Initialize ***********************
   */

  template <class TElastix>
    void AdvancedMattesMutualInformationMetric<TElastix>
    ::Initialize( void ) throw (ExceptionObject)
  {
    TimerPointer timer = TimerType::New();
    timer->StartTimer();
    this->Superclass1::Initialize();
    timer->StopTimer();
    elxout << "Initialization of AdvancedMattesMutualInformation metric took: "
      << static_cast<long>( timer->GetElapsedClockSec() * 1000 ) << " ms." << std::endl;

  } // end Initialize()

  /**
   * ***************** BeforeEachResolution ***********************
   */

  template <class TElastix>
    void AdvancedMattesMutualInformationMetric<TElastix>
    ::BeforeEachResolution( void )
  {
    /** Get the current resolution level. */
    unsigned int level =
      ( this->m_Registration->GetAsITKBaseType() )->GetCurrentLevel();

    /** Get and set the number of histogram bins. */
    unsigned int numberOfHistogramBins = 32;
    this->GetConfiguration()->ReadParameter( numberOfHistogramBins,
      "NumberOfHistogramBins", this->GetComponentLabel(), level, 0 );
    this->SetNumberOfFixedHistogramBins( numberOfHistogramBins );
    this->SetNumberOfMovingHistogramBins( numberOfHistogramBins );

    unsigned int numberOfFixedHistogramBins = numberOfHistogramBins;
    unsigned int numberOfMovingHistogramBins = numberOfHistogramBins;
    this->GetConfiguration()->ReadParameter( numberOfFixedHistogramBins,
      "NumberOfFixedHistogramBins", this->GetComponentLabel(), level, 0 );
    this->GetConfiguration()->ReadParameter( numberOfMovingHistogramBins,
      "NumberOfMovingHistogramBins", this->GetComponentLabel(), level, 0 );
    this->SetNumberOfFixedHistogramBins( numberOfFixedHistogramBins );
    this->SetNumberOfMovingHistogramBins( numberOfMovingHistogramBins );

    /** Set limiters. */
    typedef HardLimiterFunction< RealType, FixedImageDimension > FixedLimiterType;
    typedef ExponentialLimiterFunction< RealType, MovingImageDimension > MovingLimiterType;
    this->SetFixedImageLimiter( FixedLimiterType::New() );
    this->SetMovingImageLimiter( MovingLimiterType::New() );

    /** Get and set the limit range ratios. */
    double fixedLimitRangeRatio = 0.01;
    double movingLimitRangeRatio = 0.01;
    this->GetConfiguration()->ReadParameter( fixedLimitRangeRatio,
      "FixedLimitRangeRatio", this->GetComponentLabel(), level, 0 );
    this->GetConfiguration()->ReadParameter( movingLimitRangeRatio,
      "MovingLimitRangeRatio", this->GetComponentLabel(), level, 0 );
    this->SetFixedLimitRangeRatio( fixedLimitRangeRatio );
    this->SetMovingLimitRangeRatio( movingLimitRangeRatio );

    /** Set B-spline Parzen kernel orders. */
    unsigned int fixedKernelBSplineOrder = 0;
    unsigned int movingKernelBSplineOrder = 3;
    this->GetConfiguration()->ReadParameter( fixedKernelBSplineOrder,
      "FixedKernelBSplineOrder", this->GetComponentLabel(), level, 0 );
    this->GetConfiguration()->ReadParameter( movingKernelBSplineOrder,
      "MovingKernelBSplineOrder", this->GetComponentLabel(), level, 0 );
    this->SetFixedKernelBSplineOrder( fixedKernelBSplineOrder );
    this->SetMovingKernelBSplineOrder( movingKernelBSplineOrder );

    /** Set whether a low memory consumption should be used. */
    bool useFastAndLowMemoryVersion = true;
    this->GetConfiguration()->ReadParameter( useFastAndLowMemoryVersion,
      "UseFastAndLowMemoryVersion", this->GetComponentLabel(), level, 0 );
    this->SetUseExplicitPDFDerivatives( !useFastAndLowMemoryVersion );

    /** Set whether a finite difference derivative should be used. */
    bool useFiniteDifferenceDerivative = false;
    this->GetConfiguration()->ReadParameter( useFiniteDifferenceDerivative,
      "FiniteDifferenceDerivative", this->GetComponentLabel(), level, 0 );
    this->SetUseFiniteDifferenceDerivative( useFiniteDifferenceDerivative );

    /** Prepare for computing the perturbation gain c_k. */
    this->SetCurrentIteration( 0 );
    if ( useFiniteDifferenceDerivative )
    {
      double c = 1.0;
      double gamma = 0.101;
      this->GetConfiguration()->ReadParameter(c, "SP_c", this->GetComponentLabel(), level, 0 );
      this->GetConfiguration()->ReadParameter(gamma, "SP_gamma", this->GetComponentLabel(), level, 0 );
      this->SetParam_c( c );
      this->SetParam_gamma( gamma );
      this->SetFiniteDifferencePerturbation( this->Compute_c( 0 ) );
    }

    this->SetUseMovingImageDerivativeScales( false );
    MovingImageDerivativeScalesType movingImageDerivativeScales;
    int usescales = 0;
    for ( unsigned int i = 0; i < MovingImageDimension; ++i )
    {
      usescales |= this->GetConfiguration()->ReadParameter( movingImageDerivativeScales[i],
        "MovingImageDerivativeScales", this->GetComponentLabel(), i, -1, true );
    }
    if ( usescales == 0 )
    {
      this->SetUseMovingImageDerivativeScales( true );
      this->SetMovingImageDerivativeScales( movingImageDerivativeScales );
      elxout << "Multiplying moving image derivatives by: " << movingImageDerivativeScales << std::endl;
    }

  } // end BeforeEachResolution()

  /**
   * ***************** AfterEachIteration ***********************
   */

  template <class TElastix>
    void AdvancedMattesMutualInformationMetric<TElastix>
    ::AfterEachIteration(void)
  {
    if ( this->GetUseFiniteDifferenceDerivative() )
    {
      this->m_CurrentIteration++;
      this->SetFiniteDifferencePerturbation(
        this->Compute_c( this->m_CurrentIteration )  );
    }
  }  // end AfterEachIteration()

  /**
   * ************************** Compute_c *************************
   *
   * This function computes the parameter a at iteration k, as
   * in the finite difference optimizer.
   */

  template <class TElastix>
    double AdvancedMattesMutualInformationMetric<TElastix>
    ::Compute_c( unsigned long k ) const
  {
    return static_cast<double>(
      this->m_Param_c / vcl_pow( k + 1, this->m_Param_gamma ) );

  } // end Compute_c()

} // end namespace elastix

#endif // end #ifndef __elxAdvancedMattesMutualInformationMetric_HXX__
<|endoftext|>